1 // This is oxl/mvl/TriTensor.cxx
2 //:
3 // \file
4
5 #include <iostream>
6 #include <cmath>
7 #include <vector>
8 #include "TriTensor.h"
9
10 #ifdef _MSC_VER
11 # include "vcl_msvc_warnings.h"
12 #endif
13 #include <cassert>
14
15 #include "vul/vul_printf.h"
16
17 #include "vnl/vnl_matrix.h"
18 #include "vnl/vnl_matlab_print.h"
19 #include "vnl/vnl_double_3.h"
20 #include "vnl/vnl_double_3x3.h"
21 #include "vnl/vnl_identity_3x3.h"
22 #include "vnl/vnl_cross_product_matrix.h"
23 #include <vnl/algo/vnl_svd.h>
24 #include "vnl/vnl_inverse.h"
25
26 #include "vgl/vgl_homg_point_2d.h"
27 #include "vgl/vgl_homg_line_2d.h"
28 #include <vgl/algo/vgl_h_matrix_2d.h>
29 #include <vgl/algo/vgl_homg_operators_2d.h>
30
31 #include <mvl/HomgLine2D.h>
32 #include <mvl/HomgPoint2D.h>
33 #include <mvl/HMatrix2D.h>
34 #include <mvl/FMatrix.h>
35 #include <mvl/PMatrix.h>
36 #include <mvl/PMatrixDecompAa.h>
37 #include <mvl/HMatrix3D.h>
38 #include <mvl/HomgOperator2D.h>
39 #include <mvl/FManifoldProject.h>
40
41 struct OuterProduct3x3 : public vnl_double_3x3
42 {
OuterProduct3x3OuterProduct3x343 OuterProduct3x3(vnl_double_3 const& a, vnl_double_3 const& b)
44 {
45 for (int i = 0; i < 3; ++i)
46 for (int j = 0; j < 3; ++j)
47 put(i,j, a[i] * b[j]);
48 }
49 };
50
51 static bool tt_verbose = false;
52
53 #define MUTABLE_CAST(slot) (((TriTensor*)this)->slot) // const violation ...
54
55 //--------------------------------------------------------------
56
57
58 //: Default constructor.
TriTensor()59 TriTensor::TriTensor() : T(3, 3, 3) {}
60
61 //
62 //: Construct a TriTensor from a linear array of doubles.
63 // The doubles are stored in ``matrix'' order, with the last index
64 // increasing fastest.
TriTensor(const double * tritensor_array)65 TriTensor::TriTensor(const double *tritensor_array)
66 : T(3, 3, 3, tritensor_array), e12_(nullptr), e13_(nullptr), fmp12_(nullptr), fmp13_(nullptr), fmp23_(nullptr)
67 {
68 }
69
70 //: Copy constructor.
TriTensor(const TriTensor & that)71 TriTensor::TriTensor(const TriTensor& that)
72 : T(that.T), e12_(nullptr), e13_(nullptr), fmp12_(nullptr), fmp13_(nullptr), fmp23_(nullptr)
73 {
74 }
75
76 //: Construct from 3 projection matrices.
TriTensor(PMatrix const & P1,PMatrix const & P2,PMatrix const & P3)77 TriTensor::TriTensor(PMatrix const& P1,
78 PMatrix const& P2,
79 PMatrix const& P3)
80 : T(3, 3, 3), e12_(nullptr), e13_(nullptr), fmp12_(nullptr), fmp13_(nullptr), fmp23_(nullptr)
81 {
82 set(P1, P2, P3);
83 }
84
85 //: Construct from 2 projection matrices, as described in set.
TriTensor(PMatrix const & P2,PMatrix const & P3)86 TriTensor::TriTensor(PMatrix const& P2,
87 PMatrix const& P3)
88 : T(3, 3, 3), e12_(nullptr), e13_(nullptr), fmp12_(nullptr), fmp13_(nullptr), fmp23_(nullptr)
89 {
90 set(P2, P3);
91 }
92
93 //: Construct from 3 matrices.
TriTensor(vnl_matrix<double> const & T1,vnl_matrix<double> const & T2,vnl_matrix<double> const & T3)94 TriTensor::TriTensor(vnl_matrix<double> const& T1,
95 vnl_matrix<double> const& T2,
96 vnl_matrix<double> const& T3)
97 : T(3, 3, 3), e12_(nullptr), e13_(nullptr), fmp12_(nullptr), fmp13_(nullptr), fmp23_(nullptr)
98 {
99 set(T1, T2, T3);
100 }
101
102 //: Assignment
operator =(const TriTensor & that)103 TriTensor& TriTensor::operator=(const TriTensor& that)
104 {
105 T = that.T;
106 delete_caches();
107
108 return *this;
109 }
110
111 //: Destructor
~TriTensor()112 TriTensor::~TriTensor()
113 {
114 delete_caches();
115 }
116
117 // - Delete and zero epipole and manifold projector classes.
delete_caches() const118 void TriTensor::delete_caches() const
119 {
120 delete e12_; e12_ = nullptr;
121 delete e13_; e13_ = nullptr;
122 delete fmp12_; fmp12_ = nullptr;
123 delete fmp13_; fmp13_ = nullptr;
124 delete fmp23_; fmp23_ = nullptr;
125 }
126
127 //: Convert T to 27x1 matrix. Out is assumed to have been appropriately resized.
convert_to_vector(vnl_matrix<double> * out) const128 void TriTensor::convert_to_vector(vnl_matrix<double> * out) const
129 {
130 // tr_convert_tensor_to_vector
131 assert(out-> rows() == 27 && out-> columns() == 1);
132 T.get(out->data_block());
133 }
134
135 //: Convert from 27x1 matrix.
set(const vnl_matrix<double> & in)136 void TriTensor::set(const vnl_matrix<double>& in)
137 {
138 assert(in.rows() == 27 && in.columns() == 1);
139 T.set(in.data_block());
140 delete_caches();
141 }
142
143 // == BUILDING ==
144
145 //: Construct from 3 projection matrices.
146 void
set(const PMatrix & P1,const PMatrix & P2,const PMatrix & P3)147 TriTensor::set(const PMatrix& P1, const PMatrix& P2, const PMatrix& P3)
148 {
149 PMatrixDecompAa p2;
150 PMatrixDecompAa p3;
151
152 bool canon1 = P1.is_canonical(1e-12);
153 if (canon1) {
154 p2.set(P2);
155 p3.set(P3);
156 }
157 else {
158 HMatrix3D H = P1.get_canonical_H();
159 p2.set(P2 * H);
160 p3.set(P3 * H);
161 }
162
163 for (int i = 0; i < 3; ++i)
164 for (int j = 0; j < 3; ++j)
165 for (int k = 0; k < 3; ++k)
166 T(i,j,k) = p2.A(j,i) * p3.a[k] - p3.A(k,i) * p2.a[j];
167 delete_caches();
168 }
169
170 //: Construct from 2 projection matrices, P2 and P3.
171 // The first is assumed to be the canonical [I | 0].
172 void
set(const PMatrix & P2,const PMatrix & P3)173 TriTensor::set(const PMatrix& P2, const PMatrix& P3)
174 {
175 PMatrixDecompAa p2;
176 PMatrixDecompAa p3;
177
178 p2.set(P2);
179 p3.set(P3);
180
181 for (int i = 0; i < 3; ++i)
182 for (int j = 0; j < 3; ++j)
183 for (int k = 0; k < 3; ++k)
184 T(i,j,k) = p2.A(j,i) * p3.a[k] - p3.A(k,i) * p2.a[j];
185
186 delete_caches();
187 }
188
189 //: Construct from 3 T matrices.
190 void
set(vnl_matrix<double> const & T1,vnl_matrix<double> const & T2,vnl_matrix<double> const & T3)191 TriTensor::set(vnl_matrix<double> const& T1,
192 vnl_matrix<double> const& T2,
193 vnl_matrix<double> const& T3)
194 {
195 std::cerr << "TriTensor: construct from 3 T-matrices: Unimplemented\n";
196 vnl_matrix<double> const* Ts[3];
197 Ts[0] = &T1;
198 Ts[1] = &T2;
199 Ts[2] = &T3;
200
201 for (int i = 0; i < 3; ++i) {
202 const vnl_matrix<double>& Ti = *Ts[i];
203 for (int j = 0; j < 3; ++j)
204 for (int k = 0; k < 3; ++k)
205 T(i,j,k) = Ti(j,k);
206 }
207 delete_caches();
208 }
209
210 // == TRANSFER ==
211
212 //-----------------------------------------------------------------------------
213 //
214 //: For the specified points in image 1/2, return the transferred point in image 3.
215 // Transfer is via optimal backprojection.
216 //
217
218 HomgPoint2D
image3_transfer(HomgPoint2D const & point1,HomgPoint2D const & point2,HomgPoint2D corrected[]) const219 TriTensor::image3_transfer(HomgPoint2D const& point1,
220 HomgPoint2D const& point2,
221 HomgPoint2D corrected[]) const
222 {
223 HomgPoint2D corr[2];
224 if (!corrected) corrected = corr;
225 get_fmp12()->correct(point1, point2, &corrected[0], &corrected[1]);
226
227 std::vector<HomgLine2D> constraint_lines(9);
228 get_constraint_lines_image3(corrected[0], corrected[1], &constraint_lines);
229 return HomgOperator2D::lines_to_point(constraint_lines);
230 }
231
232 //-----------------------------------------------------------------------------
233 //
234 //: For the specified points in image 1/3, return the transferred point in image 2.
235 //
236
237 HomgPoint2D
image2_transfer(HomgPoint2D const & point1,HomgPoint2D const & point3,HomgPoint2D corrected[]) const238 TriTensor::image2_transfer(HomgPoint2D const& point1,
239 HomgPoint2D const& point3,
240 HomgPoint2D corrected[]) const
241 {
242 HomgPoint2D corr[2];
243 if (!corrected) corrected = corr;
244 get_fmp13()->correct(point1, point3, &corrected[0], &corrected[1]);
245
246 std::vector<HomgLine2D> constraint_lines(9);
247 get_constraint_lines_image2(corrected[0], corrected[1], &constraint_lines);
248 return HomgOperator2D::lines_to_point(constraint_lines);
249 }
250
251 //-----------------------------------------------------------------------------
252 //
253 //: For the specified points in image 2/3, return the transferred point in image 1.
254 //
255
256 vgl_homg_point_2d<double>
image1_transfer(vgl_homg_point_2d<double> const & point2,vgl_homg_point_2d<double> const & point3,vgl_homg_point_2d<double> corrected[]) const257 TriTensor::image1_transfer(vgl_homg_point_2d<double> const& point2,
258 vgl_homg_point_2d<double> const& point3,
259 vgl_homg_point_2d<double> corrected[]) const
260 {
261 vgl_homg_point_2d<double> corr[2];
262 if (!corrected) corrected = corr;
263 get_fmp23()->correct(point2, point3, corrected[0], corrected[1]);
264
265 std::vector<vgl_homg_line_2d<double> > constraint_lines(9);
266 get_constraint_lines_image1(corrected[0], corrected[1], constraint_lines);
267 return vgl_homg_operators_2d<double>::lines_to_point(constraint_lines);
268 }
269
270 HomgPoint2D
image1_transfer(HomgPoint2D const & point2,HomgPoint2D const & point3,HomgPoint2D corrected[]) const271 TriTensor::image1_transfer(HomgPoint2D const& point2,
272 HomgPoint2D const& point3,
273 HomgPoint2D corrected[]) const
274 {
275 HomgPoint2D corr[2];
276 if (!corrected) corrected = corr;
277 get_fmp23()->correct(point2, point3, &corrected[0], &corrected[1]);
278
279 std::vector<HomgLine2D> constraint_lines(9);
280 get_constraint_lines_image1(corrected[0], corrected[1], &constraint_lines);
281 return HomgOperator2D::lines_to_point(constraint_lines);
282 }
283
284 // == TRANSFER: ``Quick and Dirty'' ==
285
286 //-----------------------------------------------------------------------------
287 //
288 //: For the specified points in image 1/2, return the transferred point in image 3.
289 //
290
291 vgl_homg_point_2d<double>
image3_transfer_qd(vgl_homg_point_2d<double> const & point1,vgl_homg_point_2d<double> const & point2) const292 TriTensor::image3_transfer_qd(vgl_homg_point_2d<double> const& point1,
293 vgl_homg_point_2d<double> const& point2) const
294 {
295 // tr_transfer_point_12_to_3
296 std::vector<vgl_homg_line_2d<double> > constraint_lines(9);
297 get_constraint_lines_image3(point1, point2, constraint_lines);
298 return vgl_homg_operators_2d<double>::lines_to_point(constraint_lines);
299 }
300
301 HomgPoint2D
image3_transfer_qd(HomgPoint2D const & point1,HomgPoint2D const & point2) const302 TriTensor::image3_transfer_qd(HomgPoint2D const& point1,
303 HomgPoint2D const& point2) const
304 {
305 // tr_transfer_point_12_to_3
306 std::vector<HomgLine2D> constraint_lines(9);
307 get_constraint_lines_image3(point1, point2, &constraint_lines);
308 return HomgOperator2D::lines_to_point(constraint_lines);
309 }
310
311 //-----------------------------------------------------------------------------
312 //
313 //: For the specified points in image 1/3, return the transferred point in image 2.
314 //
315
316 vgl_homg_point_2d<double>
image2_transfer_qd(vgl_homg_point_2d<double> const & point1,vgl_homg_point_2d<double> const & point3) const317 TriTensor::image2_transfer_qd(vgl_homg_point_2d<double> const& point1,
318 vgl_homg_point_2d<double> const& point3) const
319 {
320 std::vector<vgl_homg_line_2d<double> > constraint_lines(9);
321 get_constraint_lines_image2(point1, point3, constraint_lines);
322 return vgl_homg_operators_2d<double>::lines_to_point(constraint_lines);
323 }
324
325 HomgPoint2D
image2_transfer_qd(HomgPoint2D const & point1,HomgPoint2D const & point3) const326 TriTensor::image2_transfer_qd(HomgPoint2D const& point1,
327 HomgPoint2D const& point3) const
328 {
329 std::vector<HomgLine2D> constraint_lines(9);
330 get_constraint_lines_image2(point1, point3, &constraint_lines);
331 return HomgOperator2D::lines_to_point(constraint_lines);
332 }
333
334 //-----------------------------------------------------------------------------
335 //
336 //: For the specified points in image 2/3, return the transferred point in image 1.
337 //
338
339 vgl_homg_point_2d<double>
image1_transfer_qd(vgl_homg_point_2d<double> const & point2,vgl_homg_point_2d<double> const & point3) const340 TriTensor::image1_transfer_qd(vgl_homg_point_2d<double> const& point2,
341 vgl_homg_point_2d<double> const& point3) const
342 {
343 std::vector<vgl_homg_line_2d<double> > constraint_lines(9);
344 get_constraint_lines_image1(point2, point3, constraint_lines);
345 return vgl_homg_operators_2d<double>::lines_to_point(constraint_lines);
346 }
347
348 HomgPoint2D
image1_transfer_qd(HomgPoint2D const & point2,HomgPoint2D const & point3) const349 TriTensor::image1_transfer_qd(HomgPoint2D const& point2,
350 HomgPoint2D const& point3) const
351 {
352 std::vector<HomgLine2D> constraint_lines(9);
353 get_constraint_lines_image1(point2, point3, &constraint_lines);
354 return HomgOperator2D::lines_to_point(constraint_lines);
355 }
356
357 //-----------------------------------------------------------------------------
358 //
359 //: For the specified lines in image 2/3, return the transferred line in image 1.
360 //
361
362 vgl_homg_line_2d<double>
image1_transfer(vgl_homg_line_2d<double> const & line2,vgl_homg_line_2d<double> const & line3) const363 TriTensor::image1_transfer(vgl_homg_line_2d<double> const& line2,
364 vgl_homg_line_2d<double> const& line3) const
365 {
366 vnl_double_3 l1(0.0,0.0,0.0);
367 vnl_double_3 l2(line2.a(),line2.b(),line2.c());
368 vnl_double_3 l3(line3.a(),line3.b(),line3.c());
369
370 for (int i = 0; i < 3; i++)
371 for (int j = 0; j < 3; j++)
372 for (int k = 0; k < 3; k++)
373 l1 [i] += T(i,j,k) * l2 [j] * l3 [k];
374
375 return {l1[0],l1[1],l1[2]};
376 }
377
378 HomgLine2D
image1_transfer(HomgLine2D const & line2,HomgLine2D const & line3) const379 TriTensor::image1_transfer(HomgLine2D const& line2,
380 HomgLine2D const& line3) const
381 {
382 vnl_double_3 l1(0,0,0);
383 const vnl_vector<double>& l2 = line2.get_vector().as_ref();
384 const vnl_vector<double>& l3 = line3.get_vector().as_ref();
385
386 for (int i = 0; i < 3; i++)
387 for (int j = 0; j < 3; j++)
388 for (int k = 0; k < 3; k++)
389 l1 [i] += T(i,j,k) * l2 [j] * l3 [k];
390
391 return HomgLine2D(l1);
392 }
393
394 //-----------------------------------------------------------------------------
395 //
396 //: For the specified lines in image 2/3, return the transferred line in image 1.
397 //
398
399 vgl_homg_line_2d<double>
image2_transfer(vgl_homg_line_2d<double> const & line1,vgl_homg_line_2d<double> const & line3) const400 TriTensor::image2_transfer(vgl_homg_line_2d<double> const& line1,
401 vgl_homg_line_2d<double> const& line3) const
402 {
403 vnl_double_3 l1(line1.a(),line1.b(),line1.c());
404 vnl_double_3 l3(line3.a(),line3.b(),line3.c());
405 vnl_double_3 l = vnl_inverse(dot3(l3)) * l1;
406 return {l[0],l[1],l[2]};
407 }
408
409 HomgLine2D
image2_transfer(HomgLine2D const & line1,HomgLine2D const & line3) const410 TriTensor::image2_transfer(HomgLine2D const& line1,
411 HomgLine2D const& line3) const
412 {
413 return HomgLine2D(vnl_inverse(dot3(line3.get_vector().as_ref())) * line1.get_vector());
414 }
415
416 //-----------------------------------------------------------------------------
417 //
418 //: For the specified lines in image 1/2, return the transferred line in image 3.
419 //
420
421 vgl_homg_line_2d<double>
image3_transfer(vgl_homg_line_2d<double> const & line1,vgl_homg_line_2d<double> const & line2) const422 TriTensor::image3_transfer(vgl_homg_line_2d<double> const& line1,
423 vgl_homg_line_2d<double> const& line2) const
424 {
425 vnl_double_3 l1(line1.a(),line1.b(),line1.c());
426 vnl_double_3 l2(line2.a(),line2.b(),line2.c());
427 vnl_double_3 l = vnl_inverse(dot2(l2)) * l1;
428 return {l[0],l[1],l[2]};
429 }
430
431 HomgLine2D
image3_transfer(HomgLine2D const & line1,HomgLine2D const & line2) const432 TriTensor::image3_transfer(HomgLine2D const& line1,
433 HomgLine2D const& line2) const
434 {
435 return HomgLine2D(vnl_inverse(dot2(line2.get_vector().as_ref())) * line1.get_vector());
436 }
437
438 // == HOMOGRAPHIES FROM LINES ==
439
440 //: Return the planar homography between views 3 and 1 induced by line 2
get_hmatrix_31(vgl_homg_line_2d<double> const & line2) const441 vgl_h_matrix_2d<double> TriTensor::get_hmatrix_31(vgl_homg_line_2d<double> const& line2) const
442 {
443 vnl_double_3 l2(line2.a(),line2.b(),line2.c());
444 return vgl_h_matrix_2d<double>(dot2(l2));
445 }
446
get_hmatrix_31(const HomgLine2D & line2) const447 HMatrix2D TriTensor::get_hmatrix_31(const HomgLine2D& line2) const
448 {
449 return HMatrix2D(dot2(line2.get_vector().as_ref()));
450 }
451
452 //: Return the planar homography between views 2 and 1 induced by line 3
get_hmatrix_21(vgl_homg_line_2d<double> const & line3) const453 vgl_h_matrix_2d<double> TriTensor::get_hmatrix_21(vgl_homg_line_2d<double> const& line3) const
454 {
455 vnl_double_3 l3(line3.a(),line3.b(),line3.c());
456 return vgl_h_matrix_2d<double>(dot3(l3));
457 }
458
get_hmatrix_21(const HomgLine2D & line3) const459 HMatrix2D TriTensor::get_hmatrix_21(const HomgLine2D& line3) const
460 {
461 return HMatrix2D(dot3(line3.get_vector().as_ref()));
462 }
463
464 // == CONTRACTION WITH VECTORS ==
465
466 //: Compute ${\tt M}_{jk} = T_{ijk} v_i$.
dot1(const vnl_double_3 & v) const467 vnl_double_3x3 TriTensor::dot1(const vnl_double_3& v) const
468 {
469 vnl_double_3x3 answer; answer.fill(0.0);
470 for (int i = 0; i < 3; i++)
471 for (int j = 0; j < 3; j++)
472 for (int k = 0; k < 3; k++)
473 answer(j,k) += v[i] * T(i,j,k);
474 return answer;
475 }
476
477 //: Compute ${\tt M}_{ik} = T_{ijk} v_j$.
dot2(const vnl_double_3 & v) const478 vnl_double_3x3 TriTensor::dot2(const vnl_double_3& v) const
479 {
480 vnl_double_3x3 answer; answer.fill(0.0);
481 for (int i = 0; i < 3; i++)
482 for (int j = 0; j < 3; j++)
483 for (int k = 0; k < 3; k++)
484 answer(i,k) += v[j] * T(i,j,k);
485 return answer;
486 }
487
488 //: Compute ${\tt M}_{ij} = T_{ijk} v_k$.
dot3(const vnl_double_3 & v) const489 vnl_double_3x3 TriTensor::dot3(const vnl_double_3& v) const
490 {
491 vnl_double_3x3 answer; answer.fill(0.0);
492 for (int i = 0; i < 3; i++)
493 for (int j = 0; j < 3; j++)
494 for (int k = 0; k < 3; k++)
495 answer(i,j) += v[k] * T(i,j,k);
496 return answer;
497 }
498
499 //: Compute ${\tt M}_{kj} = T_{ijk} v_i$. (The transpose of dot1).
dot1t(const vnl_double_3 & v) const500 vnl_double_3x3 TriTensor::dot1t(const vnl_double_3& v) const
501 {
502 vnl_double_3x3 answer; answer.fill(0.0);
503 for (int i = 0; i < 3; i++)
504 for (int j = 0; j < 3; j++)
505 for (int k = 0; k < 3; k++)
506 answer(k,j) += v[i] * T(i,j,k);
507 return answer;
508 }
509
510 //: Compute ${\tt M}_{ki} = T_{ijk} v_j$.
dot2t(const vnl_double_3 & v) const511 vnl_double_3x3 TriTensor::dot2t(const vnl_double_3& v) const
512 {
513 vnl_double_3x3 answer; answer.fill(0.0);
514 for (int i = 0; i < 3; i++)
515 for (int j = 0; j < 3; j++)
516 for (int k = 0; k < 3; k++)
517 answer(k,i) += v[j] * T(i,j,k);
518 return answer;
519 }
520
521 //: Compute ${\tt M}_{ji} = T_{ijk} v_k$.
dot3t(const vnl_double_3 & v) const522 vnl_double_3x3 TriTensor::dot3t(const vnl_double_3& v) const
523 {
524 vnl_double_3x3 answer; answer.fill(0.0);
525 for (int i = 0; i < 3; i++)
526 for (int j = 0; j < 3; j++)
527 for (int k = 0; k < 3; k++)
528 answer(j,i) += v[k] * T(i,j,k);
529 return answer;
530 }
531
532 // == COMPOSITION WITH MATRICES ==
533
534 //: Contract tensor axis tensor_axis with first component of matrix $M$.
535 // That is, where $S$ is the result of the operation:
536 //
537 // - For tensor_axis = 1, compute $S_{ijk} = T_{pjk} M_{pi}$
538 // - For tensor_axis = 2, compute $S_{ijk} = T_{ipk} M_{pj}$
539 // - For tensor_axis = 3, compute $S_{ijk} = T_{ijp} M_{pk}$
postmultiply(unsigned tensor_axis,const vnl_matrix<double> & M) const540 TriTensor TriTensor::postmultiply(unsigned tensor_axis, const vnl_matrix<double>& M) const
541 {
542 switch (tensor_axis) {
543 case 1: return postmultiply1(M);
544 case 2: return postmultiply2(M);
545 case 3: return postmultiply3(M);
546 default:
547 assert(tensor_axis <= 3);
548 return *this; // Shut up compiler
549 }
550 }
551
552 //: Contract tensor axis tensor_axis with second component of matrix $M$.
553 // That is, where $S$ is the result of the operation:
554 //
555 // - For tensor_axis = 1, compute $S_{ijk} = M_{ip} T_{pjk}$
556 // - For tensor_axis = 2, compute $S_{ijk} = M_{jp} T_{ipk}$
557 // - For tensor_axis = 3, compute $S_{ijk} = M_{kp} T_{ijp}$
premultiply(unsigned tensor_axis,const vnl_matrix<double> & M) const558 TriTensor TriTensor::premultiply(unsigned tensor_axis, const vnl_matrix<double>& M) const
559 {
560 switch (tensor_axis) {
561 case 1: return premultiply1(M);
562 case 2: return premultiply2(M);
563 case 3: return premultiply3(M);
564 default:
565 assert(tensor_axis <= 3);
566 return *this; // Shut up compiler
567 }
568 }
569
570 //: Compute $ S_{ijk} = T_{pjk} M_{pi} $.
postmultiply1(const vnl_matrix<double> & M) const571 TriTensor TriTensor::postmultiply1(const vnl_matrix<double>& M) const
572 {
573 TriTensor S;
574 for (int i = 0; i < 3; ++i)
575 for (int j = 0; j < 3; ++j)
576 for (int k = 0; k < 3; ++k) {
577 double v = 0;
578 for (int p = 0; p < 3; ++p)
579 v += T(p,j,k) * M(p,i);
580 S(i,j,k) = v;
581 }
582 return S;
583 }
584
585 //: Compute $ S_{ijk} = T_{ipk} M_{pj} $.
postmultiply2(const vnl_matrix<double> & M) const586 TriTensor TriTensor::postmultiply2(const vnl_matrix<double>& M) const
587 {
588 TriTensor S;
589 for (int i = 0; i < 3; ++i)
590 for (int j = 0; j < 3; ++j)
591 for (int k = 0; k < 3; ++k) {
592 double v = 0;
593 for (int p = 0; p < 3; ++p)
594 v += T(i,p,k) * M(p,j);
595 S(i,j,k) = v;
596 }
597 return S;
598 }
599
600 //: Compute $ S_{ijk} = T_{ijp} M_{pk} $.
postmultiply3(const vnl_matrix<double> & M) const601 TriTensor TriTensor::postmultiply3(const vnl_matrix<double>& M) const
602 {
603 TriTensor S;
604 for (int i = 0; i < 3; ++i)
605 for (int j = 0; j < 3; ++j)
606 for (int k = 0; k < 3; ++k) {
607 double v = 0;
608 for (int p = 0; p < 3; ++p)
609 v += T(i,j,p) * M(p,k);
610 S(i,j,k) = v;
611 }
612 return S;
613 }
614
615 //: Compute $ S_{ijk} = M_{ip} T_{pjk} $.
premultiply1(const vnl_matrix<double> & M) const616 TriTensor TriTensor::premultiply1(const vnl_matrix<double>& M) const
617 {
618 TriTensor S;
619 for (int i = 0; i < 3; ++i)
620 for (int j = 0; j < 3; ++j)
621 for (int k = 0; k < 3; ++k) {
622 double v = 0;
623 for (int p = 0; p < 3; ++p)
624 v += M(i,p) * T(p,j,k);
625 S(i,j,k) = v;
626 }
627 return S;
628 }
629
630 //: Compute $ S_{ijk} = M_{jp} T_{ipk} $.
premultiply2(const vnl_matrix<double> & M) const631 TriTensor TriTensor::premultiply2(const vnl_matrix<double>& M) const
632 {
633 TriTensor S;
634 for (int i = 0; i < 3; ++i)
635 for (int j = 0; j < 3; ++j)
636 for (int k = 0; k < 3; ++k) {
637 double v = 0;
638 for (int p = 0; p < 3; ++p)
639 v += M(j,p) * T(i,p,k);
640 S(i,j,k) = v;
641 }
642 return S;
643 }
644
645 //: Compute $ S_{ijk} = M_{kp} T_{ijp} $.
premultiply3(const vnl_matrix<double> & M) const646 TriTensor TriTensor::premultiply3(const vnl_matrix<double>& M) const
647 {
648 TriTensor S;
649 for (int i = 0; i < 3; ++i)
650 for (int j = 0; j < 3; ++j)
651 for (int k = 0; k < 3; ++k) {
652 double v = 0;
653 for (int p = 0; p < 3; ++p)
654 v += M(k,p) * T(i,j,p);
655 S(i,j,k) = v;
656 }
657 return S;
658 }
659
660 // INTERNALS---------------------------------------------------------------
661
662 // == TRANSFORMATION ==
663
664 //:
665 // The ${\tt C}_{123}$ are line transformation matrices. If
666 // ${\tt C}_{v} l_v = \hat l_v$ describes the transformation of each image plane under
667 // a planar homography, and $l_1 = T l_2 l_3$ describes the action of this TriTensor, then
668 // this routine computes $\hat T$ such that $\hat l_1 = \hat T \hat l_2 \hat l_3$.
669 //
670 // Specifically $\hat T = T.\mbox{decondition}(C_1^{-1}, C_2, C_3)$ is the
671 // transformed tensor. Note that unless transfer is via Hartley-Sturm, the deconditioned
672 // tensor will not be covariant with the conditioned one.
condition(vnl_matrix<double> const & line_1_denorm,vnl_matrix<double> const & line_2_norm,vnl_matrix<double> const & line_3_norm) const673 TriTensor TriTensor::condition(vnl_matrix<double> const& line_1_denorm,
674 vnl_matrix<double> const& line_2_norm,
675 vnl_matrix<double> const& line_3_norm) const
676 {
677 return premultiply1(line_1_denorm).postmultiply2(line_2_norm).postmultiply3(line_3_norm);
678 }
679
decondition(vnl_matrix<double> const & line_1_norm,vnl_matrix<double> const & line_2_denorm,vnl_matrix<double> const & line_3_denorm) const680 TriTensor TriTensor::decondition(vnl_matrix<double> const& line_1_norm,
681 vnl_matrix<double> const& line_2_denorm,
682 vnl_matrix<double> const& line_3_denorm) const
683 {
684 return premultiply1(line_1_norm).postmultiply2(line_2_denorm).postmultiply3(line_3_denorm);
685 }
686
687 //-----------------------------------------------------------------------------
688 // - Return the 9 lines on which a transferred point ought to lie.
get_constraint_lines_image3(vgl_homg_point_2d<double> const & p1,vgl_homg_point_2d<double> const & p2,std::vector<vgl_homg_line_2d<double>> & lines) const689 void TriTensor::get_constraint_lines_image3(vgl_homg_point_2d<double> const& p1,
690 vgl_homg_point_2d<double> const& p2,
691 std::vector<vgl_homg_line_2d<double> >& lines) const
692 {
693 // use the same notation as the output of tr_hartley_equation.
694 double x1 = p1.x();
695 double y1 = p1.y();
696 double z1 = p1.w();
697
698 double x2 = p2.x();
699 double y2 = p2.y();
700 double z2 = p2.w();
701
702 lines.clear();
703
704 /* 0 */
705 {
706 double lx =
707 -x1 * x2 * T(0,1,1) + x1 * y2 * T(0,0,1)
708 -y1 * x2 * T(1,1,1) + y1 * y2 * T(1,0,1)
709 -z1 * x2 * T(2,1,1) + z1 * y2 * T(2,0,1);
710
711 double ly =
712 x1 * x2 * T(0,1,0) - x1 * y2 * T(0,0,0) +
713 y1 * x2 * T(1,1,0) - y1 * y2 * T(1,0,0) +
714 z1 * x2 * T(2,1,0) - z1 * y2 * T(2,0,0);
715
716 double lz = 0;
717 lines.emplace_back(lx, ly, lz);
718 }
719
720 /* 1 */
721 {
722 double lx =
723 -x1 * x2 * T(0,1,2) + x1 * y2 * T(0,0,2)
724 - y1 * x2 * T(1,1,2) + y1 * y2 * T(1,0,2)
725 - z1 * x2 * T(2,1,2) + z1 * y2 * T(2,0,2);
726
727 double ly = 0;
728
729 double lz
730 = x1 * x2 * T(0,1,0) - x1 * y2 * T(0,0,0)
731 + y1 * x2 * T(1,1,0) - y1 * y2 * T(1,0,0)
732 + z1 * x2 * T(2,1,0) - z1 * y2 * T(2,0,0);
733
734 lines.emplace_back(lx, ly, lz);
735 }
736
737 /* 2 */
738 {
739 double lx =
740 0;
741
742 double ly =
743 -x1 * x2 * T(0,1,2) + x1 * y2 * T(0,0,2)
744 -y1 * x2 * T(1,1,2) + y1 * y2 * T(1,0,2)
745 -z1 * x2 * T(2,1,2) + z1 * y2 * T(2,0,2);
746
747 double lz =
748 x1 * x2 * T(0,1,1) - x1 * y2 * T(0,0,1)
749 + y1 * x2 * T(1,1,1) - y1 * y2 * T(1,0,1)
750 + z1 * x2 * T(2,1,1) - z1 * y2 * T(2,0,1);
751
752 lines.emplace_back(lx, ly, lz);
753 }
754
755 /* 3 */
756 {
757 double lx =
758 -x1 * x2 * T(0,2,1) + x1 * z2 * T(0,0,1)
759 - y1 * x2 * T(1,2,1) + y1 * z2 * T(1,0,1)
760 - z1 * x2 * T(2,2,1) + z1 * z2 * T(2,0,1);
761
762 double ly =
763 x1 * x2 * T(0,2,0) - x1 * z2 * T(0,0,0)
764 + y1 * x2 * T(1,2,0) - y1 * z2 * T(1,0,0)
765 + z1 * x2 * T(2,2,0) - z1 * z2 * T(2,0,0);
766
767 double lz = 0;
768 lines.emplace_back(lx, ly, lz);
769 }
770
771 /* 4 */
772 {
773 double lx =
774 -x1 * x2 * T(0,2,2) + x1 * z2 * T(0,0,2)
775 -y1 * x2 * T(1,2,2) + y1 * z2 * T(1,0,2)
776 -z1 * x2 * T(2,2,2) + z1 * z2 * T(2,0,2);
777
778 double ly = 0;
779
780 double lz =
781 x1 * x2 * T(0,2,0) - x1 * z2 * T(0,0,0) +
782 y1 * x2 * T(1,2,0) - y1 * z2 * T(1,0,0) +
783 z1 * x2 * T(2,2,0) - z1 * z2 * T(2,0,0);
784 lines.emplace_back(lx, ly, lz);
785 }
786
787 /* 5 */
788 {
789 double lx = 0;
790
791 double ly =
792 -x1 * x2 * T(0,2,2) + x1 * z2 * T(0,0,2)
793 -y1 * x2 * T(1,2,2) + y1 * z2 * T(1,0,2)
794 -z1 * x2 * T(2,2,2) + z1 * z2 * T(2,0,2);
795
796 double lz
797 = x1 * x2 * T(0,2,1) - x1 * z2 * T(0,0,1)
798 + y1 * x2 * T(1,2,1) - y1 * z2 * T(1,0,1)
799 + z1 * x2 * T(2,2,1) - z1 * z2 * T(2,0,1);
800 lines.emplace_back(lx, ly, lz);
801 }
802
803 /* 6 */
804 {
805 double lx
806 = - x1 * y2 * T(0,2,1) + x1 * z2 * T(0,1,1)
807 - y1 * y2 * T(1,2,1) + y1 * z2 * T(1,1,1)
808 - z1 * y2 * T(2,2,1) + z1 * z2 * T(2,1,1);
809
810 double ly
811 = x1 * y2 * T(0,2,0) - x1 * z2 * T(0,1,0)
812 + y1 * y2 * T(1,2,0) - y1 * z2 * T(1,1,0)
813 + z1 * y2 * T(2,2,0) - z1 * z2 * T(2,1,0);
814
815 double lz = 0;
816 lines.emplace_back(lx, ly, lz);
817 }
818
819 /* 7 */
820 {
821 double lx
822 = -x1 * y2 * T(0,2,2) + x1 * z2 * T(0,1,2)
823 - y1 * y2 * T(1,2,2) + y1 * z2 * T(1,1,2)
824 - z1 * y2 * T(2,2,2) + z1 * z2 * T(2,1,2);
825
826 double ly = 0;
827
828 double lz
829 = x1 * y2 * T(0,2,0) - x1 * z2 * T(0,1,0)
830 + y1 * y2 * T(1,2,0) - y1 * z2 * T(1,1,0)
831 + z1 * y2 * T(2,2,0) - z1 * z2 * T(2,1,0);
832
833 lines.emplace_back(lx, ly, lz);
834 }
835
836 /* 8 */
837 {
838 double lx = 0;
839
840 double ly
841 = -x1 * y2 * T(0,2,2) + x1 * z2 * T(0,1,2)
842 - y1 * y2 * T(1,2,2) + y1 * z2 * T(1,1,2)
843 - z1 * y2 * T(2,2,2) + z1 * z2 * T(2,1,2);
844
845 double lz
846 = x1 * y2 * T(0,2,1) - x1 * z2 * T(0,1,1)
847 + y1 * y2 * T(1,2,1) - y1 * z2 * T(1,1,1)
848 + z1 * y2 * T(2,2,1) - z1 * z2 * T(2,1,1);
849
850 lines.emplace_back(lx, ly, lz);
851 }
852
853 assert(lines.size() == 9);
854 if (tt_verbose)
855 for (int i = 0; i < 9; ++i)
856 std::cerr << lines[i]<< '\n';
857
858 return;
859 }
860
get_constraint_lines_image3(HomgPoint2D const & p1,HomgPoint2D const & p2,std::vector<HomgLine2D> * lines) const861 void TriTensor::get_constraint_lines_image3(HomgPoint2D const& p1,
862 HomgPoint2D const& p2,
863 std::vector<HomgLine2D>* lines) const
864 {
865 #if 0 // Old code assumed points were in image coordinates and conditioned them,
866 // deconditioning the lines on return.
867 ma2_static_multiply_3x3_trivec(tritensor_ptr->corner1_norm_matrix, trivec1_ptr, &mapped_trivec1);
868 ma2_static_multiply_3x3_trivec(tritensor_ptr->corner2_norm_matrix, trivec2_ptr, &mapped_trivec2);
869 #endif // 0
870
871 // use the same notation as the output of tr_hartley_equation.
872 double x1 = p1.x();
873 double y1 = p1.y();
874 double z1 = p1.w();
875
876 double x2 = p2.x();
877 double y2 = p2.y();
878 double z2 = p2.w();
879
880 #ifdef DEBUG
881 std::cerr << "CLINES = [" << x1 << ' ' << y1 << ' ' << z1 << "; " << x2 << ' ' << y2 << ' ' << z2 << "];\n";
882 #endif
883
884 lines->resize(0);
885
886
887 /* 0 */
888 {
889 double lx =
890 -x1 * x2 * T(0,1,1) + x1 * y2 * T(0,0,1)
891 -y1 * x2 * T(1,1,1) + y1 * y2 * T(1,0,1)
892 -z1 * x2 * T(2,1,1) + z1 * y2 * T(2,0,1);
893
894 double ly =
895 x1 * x2 * T(0,1,0) - x1 * y2 * T(0,0,0) +
896 y1 * x2 * T(1,1,0) - y1 * y2 * T(1,0,0) +
897 z1 * x2 * T(2,1,0) - z1 * y2 * T(2,0,0);
898
899 double lz = 0;
900 lines->push_back(HomgLine2D(lx, ly, lz));
901 }
902
903 /* 1 */
904 {
905 double lx =
906 -x1 * x2 * T(0,1,2) + x1 * y2 * T(0,0,2)
907 - y1 * x2 * T(1,1,2) + y1 * y2 * T(1,0,2)
908 - z1 * x2 * T(2,1,2) + z1 * y2 * T(2,0,2);
909
910 double ly = 0;
911
912 double lz
913 = x1 * x2 * T(0,1,0) - x1 * y2 * T(0,0,0)
914 + y1 * x2 * T(1,1,0) - y1 * y2 * T(1,0,0)
915 + z1 * x2 * T(2,1,0) - z1 * y2 * T(2,0,0);
916
917 lines->push_back(HomgLine2D(lx, ly, lz));
918 }
919
920 /* 2 */
921 {
922 double lx =
923 0;
924
925 double ly =
926 -x1 * x2 * T(0,1,2) + x1 * y2 * T(0,0,2)
927 -y1 * x2 * T(1,1,2) + y1 * y2 * T(1,0,2)
928 -z1 * x2 * T(2,1,2) + z1 * y2 * T(2,0,2);
929
930 double lz =
931 x1 * x2 * T(0,1,1) - x1 * y2 * T(0,0,1)
932 + y1 * x2 * T(1,1,1) - y1 * y2 * T(1,0,1)
933 + z1 * x2 * T(2,1,1) - z1 * y2 * T(2,0,1);
934
935 lines->push_back(HomgLine2D(lx, ly, lz));
936 }
937
938 /* 3 */
939 {
940 double lx =
941 -x1 * x2 * T(0,2,1) + x1 * z2 * T(0,0,1)
942 - y1 * x2 * T(1,2,1) + y1 * z2 * T(1,0,1)
943 - z1 * x2 * T(2,2,1) + z1 * z2 * T(2,0,1);
944
945 double ly =
946 x1 * x2 * T(0,2,0) - x1 * z2 * T(0,0,0)
947 + y1 * x2 * T(1,2,0) - y1 * z2 * T(1,0,0)
948 + z1 * x2 * T(2,2,0) - z1 * z2 * T(2,0,0);
949
950 double lz = 0;
951 lines->push_back(HomgLine2D(lx, ly, lz));
952 }
953
954 /* 4 */
955 {
956 double lx =
957 -x1 * x2 * T(0,2,2) + x1 * z2 * T(0,0,2)
958 -y1 * x2 * T(1,2,2) + y1 * z2 * T(1,0,2)
959 -z1 * x2 * T(2,2,2) + z1 * z2 * T(2,0,2);
960
961 double ly = 0;
962
963 double lz =
964 x1 * x2 * T(0,2,0) - x1 * z2 * T(0,0,0) +
965 y1 * x2 * T(1,2,0) - y1 * z2 * T(1,0,0) +
966 z1 * x2 * T(2,2,0) - z1 * z2 * T(2,0,0);
967 lines->push_back(HomgLine2D(lx, ly, lz));
968 }
969
970 /* 5 */
971 {
972 double lx = 0;
973
974 double ly =
975 -x1 * x2 * T(0,2,2) + x1 * z2 * T(0,0,2)
976 -y1 * x2 * T(1,2,2) + y1 * z2 * T(1,0,2)
977 -z1 * x2 * T(2,2,2) + z1 * z2 * T(2,0,2);
978
979 double lz
980 = x1 * x2 * T(0,2,1) - x1 * z2 * T(0,0,1)
981 + y1 * x2 * T(1,2,1) - y1 * z2 * T(1,0,1)
982 + z1 * x2 * T(2,2,1) - z1 * z2 * T(2,0,1);
983 lines->push_back(HomgLine2D(lx, ly, lz));
984 }
985
986 /* 6 */
987 {
988 double lx
989 = - x1 * y2 * T(0,2,1) + x1 * z2 * T(0,1,1)
990 - y1 * y2 * T(1,2,1) + y1 * z2 * T(1,1,1)
991 - z1 * y2 * T(2,2,1) + z1 * z2 * T(2,1,1);
992
993 double ly
994 = x1 * y2 * T(0,2,0) - x1 * z2 * T(0,1,0)
995 + y1 * y2 * T(1,2,0) - y1 * z2 * T(1,1,0)
996 + z1 * y2 * T(2,2,0) - z1 * z2 * T(2,1,0);
997
998 double lz = 0;
999 lines->push_back(HomgLine2D(lx, ly, lz));
1000 }
1001
1002 /* 7 */
1003 {
1004 double lx
1005 = -x1 * y2 * T(0,2,2) + x1 * z2 * T(0,1,2)
1006 - y1 * y2 * T(1,2,2) + y1 * z2 * T(1,1,2)
1007 - z1 * y2 * T(2,2,2) + z1 * z2 * T(2,1,2);
1008
1009 double ly = 0;
1010
1011 double lz
1012 = x1 * y2 * T(0,2,0) - x1 * z2 * T(0,1,0)
1013 + y1 * y2 * T(1,2,0) - y1 * z2 * T(1,1,0)
1014 + z1 * y2 * T(2,2,0) - z1 * z2 * T(2,1,0);
1015
1016 lines->push_back(HomgLine2D(lx, ly, lz));
1017 }
1018
1019 /* 8 */
1020 {
1021 double lx = 0;
1022
1023 double ly
1024 = -x1 * y2 * T(0,2,2) + x1 * z2 * T(0,1,2)
1025 - y1 * y2 * T(1,2,2) + y1 * z2 * T(1,1,2)
1026 - z1 * y2 * T(2,2,2) + z1 * z2 * T(2,1,2);
1027
1028 double lz
1029 = x1 * y2 * T(0,2,1) - x1 * z2 * T(0,1,1)
1030 + y1 * y2 * T(1,2,1) - y1 * z2 * T(1,1,1)
1031 + z1 * y2 * T(2,2,1) - z1 * z2 * T(2,1,1);
1032
1033 lines->push_back(HomgLine2D(lx, ly, lz));
1034 }
1035
1036 assert(lines->size() == 9);
1037 if (tt_verbose)
1038 for (int i = 0; i < 9; ++i)
1039 std::cerr << (*lines)[i]<< '\n';
1040
1041 return;
1042
1043 #if 0
1044 *trivec3_ptr = ho_trivec_orthog(line_table_ptr);
1045 ma2_static_multiply_3x3_trivec(point_invnorm_matrix3, trivec3_ptr, trivec3_ptr);
1046
1047 // Decondition lines
1048 if (false)
1049 for (int line_index = 0; line_index < lines->size(); line_index++)
1050 ma2_static_multiply_3x3_trivec(line_invnorm_matrix3, lines[line_index], lines[line_index]);
1051
1052
1053 ho_triveccam_aspect_lines_to_point(line_table_ptr, trivec3_ptr);
1054 #endif
1055 }
1056
get_constraint_lines_image2(vgl_homg_point_2d<double> const & p1,vgl_homg_point_2d<double> const & p3,std::vector<vgl_homg_line_2d<double>> & lines) const1057 void TriTensor::get_constraint_lines_image2(vgl_homg_point_2d<double> const& p1,
1058 vgl_homg_point_2d<double> const& p3,
1059 std::vector<vgl_homg_line_2d<double> >& lines) const
1060 {
1061 double x1 = p1.x();
1062 double y1 = p1.y();
1063 double z1 = p1.w();
1064
1065 double x3 = p3.x();
1066 double y3 = p3.y();
1067 double z3 = p3.w();
1068
1069 lines.resize(0);
1070
1071 /* 0 */
1072 {
1073 double lx
1074 = x1 * y3 * T(0,1,0) - x1 * x3 * T(0,1,1)
1075 + y1 * y3 * T(1,1,0) - y1 * x3 * T(1,1,1)
1076 + z1 * y3 * T(2,1,0) - z1 * x3 * T(2,1,1);
1077
1078 double ly
1079 = - x1 * y3 * T(0,0,0) + x1 * x3 * T(0,0,1)
1080 - y1 * y3 * T(1,0,0) + y1 * x3 * T(1,0,1)
1081 - z1 * y3 * T(2,0,0) + z1 * x3 * T(2,0,1);
1082
1083 double lz = 0;
1084
1085 lines.emplace_back(lx, ly, lz);
1086 }
1087
1088 /* 1 */
1089 {
1090 double lx
1091 = x1 * z3 * T(0,1,0) - x1 * x3 * T(0,1,2)
1092 + y1 * z3 * T(1,1,0) - y1 * x3 * T(1,1,2)
1093 + z1 * z3 * T(2,1,0) - z1 * x3 * T(2,1,2);
1094
1095 double ly
1096 = - x1 * z3 * T(0,0,0) + x1 * x3 * T(0,0,2)
1097 - y1 * z3 * T(1,0,0) + y1 * x3 * T(1,0,2)
1098 - z1 * z3 * T(2,0,0) + z1 * x3 * T(2,0,2);
1099
1100 double lz = 0;
1101
1102 lines.emplace_back(lx, ly, lz);
1103 }
1104
1105 /* 2 */
1106 {
1107 double lx
1108 = x1 * z3 * T(0,1,1) - x1 * y3 * T(0,1,2)
1109 + y1 * z3 * T(1,1,1) - y1 * y3 * T(1,1,2)
1110 + z1 * z3 * T(2,1,1) - z1 * y3 * T(2,1,2);
1111
1112 double ly
1113 = -x1 * z3 * T(0,0,1) + x1 * y3 * T(0,0,2)
1114 - y1 * z3 * T(1,0,1) + y1 * y3 * T(1,0,2)
1115 - z1 * z3 * T(2,0,1) + z1 * y3 * T(2,0,2);
1116
1117 double lz = 0;
1118
1119 lines.emplace_back(lx, ly, lz);
1120 }
1121
1122 /* 3 */
1123 {
1124 double lx
1125 = x1 * y3 * T(0,2,0) - x1 * x3 * T(0,2,1)
1126 + y1 * y3 * T(1,2,0) - y1 * x3 * T(1,2,1)
1127 + z1 * y3 * T(2,2,0) - z1 * x3 * T(2,2,1);
1128
1129 double ly = 0;
1130
1131 double lz
1132 = -x1 * y3 * T(0,0,0) + x1 * x3 * T(0,0,1)
1133 - y1 * y3 * T(1,0,0) + y1 * x3 * T(1,0,1)
1134 - z1 * y3 * T(2,0,0) + z1 * x3 * T(2,0,1);
1135
1136 lines.emplace_back(lx, ly, lz);
1137 }
1138
1139 /* 4 */
1140 {
1141 double lx
1142 = x1 * z3 * T(0,2,0) - x1 * x3 * T(0,2,2)
1143 + y1 * z3 * T(1,2,0) - y1 * x3 * T(1,2,2)
1144 + z1 * z3 * T(2,2,0) - z1 * x3 * T(2,2,2);
1145
1146 double ly = 0;
1147
1148 double lz
1149 = - x1 * z3 * T(0,0,0) + x1 * x3 * T(0,0,2)
1150 - y1 * z3 * T(1,0,0) + y1 * x3 * T(1,0,2)
1151 - z1 * z3 * T(2,0,0) + z1 * x3 * T(2,0,2);
1152
1153 lines.emplace_back(lx, ly, lz);
1154 }
1155
1156 /* 5 */
1157 {
1158 double lx
1159 = x1 * z3 * T(0,2,1) - x1 * y3 * T(0,2,2)
1160 + y1 * z3 * T(1,2,1) - y1 * y3 * T(1,2,2)
1161 + z1 * z3 * T(2,2,1) - z1 * y3 * T(2,2,2);
1162
1163 double ly = 0;
1164
1165 double lz
1166 = - x1 * z3 * T(0,0,1) + x1 * y3 * T(0,0,2)
1167 - y1 * z3 * T(1,0,1) + y1 * y3 * T(1,0,2)
1168 - z1 * z3 * T(2,0,1) + z1 * y3 * T(2,0,2);
1169
1170 lines.emplace_back(lx, ly, lz);
1171 }
1172
1173 /* 6 */
1174 {
1175 double lx = 0;
1176
1177 double ly
1178 = x1 * y3 * T(0,2,0) - x1 * x3 * T(0,2,1)
1179 + y1 * y3 * T(1,2,0) - y1 * x3 * T(1,2,1)
1180 + z1 * y3 * T(2,2,0) - z1 * x3 * T(2,2,1);
1181
1182 double lz
1183 = -x1 * y3 * T(0,1,0) + x1 * x3 * T(0,1,1)
1184 - y1 * y3 * T(1,1,0) + y1 * x3 * T(1,1,1)
1185 - z1 * y3 * T(2,1,0) + z1 * x3 * T(2,1,1);
1186
1187 lines.emplace_back(lx, ly, lz);
1188 }
1189
1190 /* 7 */
1191 {
1192 double lx = 0;
1193
1194 double ly
1195 = x1 * z3 * T(0,2,0) - x1 * x3 * T(0,2,2)
1196 + y1 * z3 * T(1,2,0) - y1 * x3 * T(1,2,2)
1197 + z1 * z3 * T(2,2,0) - z1 * x3 * T(2,2,2);
1198
1199 double lz
1200 = - x1 * z3 * T(0,1,0) + x1 * x3 * T(0,1,2)
1201 - y1 * z3 * T(1,1,0) + y1 * x3 * T(1,1,2)
1202 - z1 * z3 * T(2,1,0) + z1 * x3 * T(2,1,2);
1203
1204 lines.emplace_back(lx, ly, lz);
1205 }
1206
1207 /* 8 */
1208 {
1209 double lx = 0;
1210
1211 double ly
1212 = x1 * z3 * T(0,2,1) - x1 * y3 * T(0,2,2)
1213 + y1 * z3 * T(1,2,1) - y1 * y3 * T(1,2,2)
1214 + z1 * z3 * T(2,2,1) - z1 * y3 * T(2,2,2);
1215
1216 double lz
1217 = - x1 * z3 * T(0,1,1) + x1 * y3 * T(0,1,2)
1218 - y1 * z3 * T(1,1,1) + y1 * y3 * T(1,1,2)
1219 - z1 * z3 * T(2,1,1) + z1 * y3 * T(2,1,2);
1220
1221 lines.emplace_back(lx, ly, lz);
1222 }
1223 }
1224
get_constraint_lines_image2(HomgPoint2D const & p1,HomgPoint2D const & p3,std::vector<HomgLine2D> * lines) const1225 void TriTensor::get_constraint_lines_image2(HomgPoint2D const& p1,
1226 HomgPoint2D const& p3,
1227 std::vector<HomgLine2D>* lines) const
1228 {
1229 double x1 = p1.x();
1230 double y1 = p1.y();
1231 double z1 = p1.w();
1232
1233 double x3 = p3.x();
1234 double y3 = p3.y();
1235 double z3 = p3.w();
1236
1237 lines->resize(0);
1238
1239 /* 0 */
1240 {
1241 double lx
1242 = x1 * y3 * T(0,1,0) - x1 * x3 * T(0,1,1)
1243 + y1 * y3 * T(1,1,0) - y1 * x3 * T(1,1,1)
1244 + z1 * y3 * T(2,1,0) - z1 * x3 * T(2,1,1);
1245
1246 double ly
1247 = - x1 * y3 * T(0,0,0) + x1 * x3 * T(0,0,1)
1248 - y1 * y3 * T(1,0,0) + y1 * x3 * T(1,0,1)
1249 - z1 * y3 * T(2,0,0) + z1 * x3 * T(2,0,1);
1250
1251 double lz = 0;
1252
1253 lines->push_back(HomgLine2D(lx, ly, lz));
1254 }
1255
1256 /* 1 */
1257 {
1258 double lx
1259 = x1 * z3 * T(0,1,0) - x1 * x3 * T(0,1,2)
1260 + y1 * z3 * T(1,1,0) - y1 * x3 * T(1,1,2)
1261 + z1 * z3 * T(2,1,0) - z1 * x3 * T(2,1,2);
1262
1263 double ly
1264 = - x1 * z3 * T(0,0,0) + x1 * x3 * T(0,0,2)
1265 - y1 * z3 * T(1,0,0) + y1 * x3 * T(1,0,2)
1266 - z1 * z3 * T(2,0,0) + z1 * x3 * T(2,0,2);
1267
1268 double lz = 0;
1269
1270 lines->push_back(HomgLine2D(lx, ly, lz));
1271 }
1272
1273 /* 2 */
1274 {
1275 double lx
1276 = x1 * z3 * T(0,1,1) - x1 * y3 * T(0,1,2)
1277 + y1 * z3 * T(1,1,1) - y1 * y3 * T(1,1,2)
1278 + z1 * z3 * T(2,1,1) - z1 * y3 * T(2,1,2);
1279
1280 double ly
1281 = -x1 * z3 * T(0,0,1) + x1 * y3 * T(0,0,2)
1282 - y1 * z3 * T(1,0,1) + y1 * y3 * T(1,0,2)
1283 - z1 * z3 * T(2,0,1) + z1 * y3 * T(2,0,2);
1284
1285 double lz = 0;
1286
1287 lines->push_back(HomgLine2D(lx, ly, lz));
1288 }
1289
1290 /* 3 */
1291 {
1292 double lx
1293 = x1 * y3 * T(0,2,0) - x1 * x3 * T(0,2,1)
1294 + y1 * y3 * T(1,2,0) - y1 * x3 * T(1,2,1)
1295 + z1 * y3 * T(2,2,0) - z1 * x3 * T(2,2,1);
1296
1297 double ly = 0;
1298
1299 double lz
1300 = -x1 * y3 * T(0,0,0) + x1 * x3 * T(0,0,1)
1301 - y1 * y3 * T(1,0,0) + y1 * x3 * T(1,0,1)
1302 - z1 * y3 * T(2,0,0) + z1 * x3 * T(2,0,1);
1303
1304 lines->push_back(HomgLine2D(lx, ly, lz));
1305 }
1306
1307 /* 4 */
1308 {
1309 double lx
1310 = x1 * z3 * T(0,2,0) - x1 * x3 * T(0,2,2)
1311 + y1 * z3 * T(1,2,0) - y1 * x3 * T(1,2,2)
1312 + z1 * z3 * T(2,2,0) - z1 * x3 * T(2,2,2);
1313
1314 double ly = 0;
1315
1316 double lz
1317 = - x1 * z3 * T(0,0,0) + x1 * x3 * T(0,0,2)
1318 - y1 * z3 * T(1,0,0) + y1 * x3 * T(1,0,2)
1319 - z1 * z3 * T(2,0,0) + z1 * x3 * T(2,0,2);
1320
1321 lines->push_back(HomgLine2D(lx, ly, lz));
1322 }
1323
1324 /* 5 */
1325 {
1326 double lx
1327 = x1 * z3 * T(0,2,1) - x1 * y3 * T(0,2,2)
1328 + y1 * z3 * T(1,2,1) - y1 * y3 * T(1,2,2)
1329 + z1 * z3 * T(2,2,1) - z1 * y3 * T(2,2,2);
1330
1331 double ly = 0;
1332
1333 double lz
1334 = - x1 * z3 * T(0,0,1) + x1 * y3 * T(0,0,2)
1335 - y1 * z3 * T(1,0,1) + y1 * y3 * T(1,0,2)
1336 - z1 * z3 * T(2,0,1) + z1 * y3 * T(2,0,2);
1337
1338 lines->push_back(HomgLine2D(lx, ly, lz));
1339 }
1340
1341 /* 6 */
1342 {
1343 double lx = 0;
1344
1345 double ly
1346 = x1 * y3 * T(0,2,0) - x1 * x3 * T(0,2,1)
1347 + y1 * y3 * T(1,2,0) - y1 * x3 * T(1,2,1)
1348 + z1 * y3 * T(2,2,0) - z1 * x3 * T(2,2,1);
1349
1350 double lz
1351 = -x1 * y3 * T(0,1,0) + x1 * x3 * T(0,1,1)
1352 - y1 * y3 * T(1,1,0) + y1 * x3 * T(1,1,1)
1353 - z1 * y3 * T(2,1,0) + z1 * x3 * T(2,1,1);
1354
1355 lines->push_back(HomgLine2D(lx, ly, lz));
1356 }
1357
1358 /* 7 */
1359 {
1360 double lx = 0;
1361
1362 double ly
1363 = x1 * z3 * T(0,2,0) - x1 * x3 * T(0,2,2)
1364 + y1 * z3 * T(1,2,0) - y1 * x3 * T(1,2,2)
1365 + z1 * z3 * T(2,2,0) - z1 * x3 * T(2,2,2);
1366
1367 double lz
1368 = - x1 * z3 * T(0,1,0) + x1 * x3 * T(0,1,2)
1369 - y1 * z3 * T(1,1,0) + y1 * x3 * T(1,1,2)
1370 - z1 * z3 * T(2,1,0) + z1 * x3 * T(2,1,2);
1371
1372 lines->push_back(HomgLine2D(lx, ly, lz));
1373 }
1374
1375 /* 8 */
1376 {
1377 double lx = 0;
1378
1379 double ly
1380 = x1 * z3 * T(0,2,1) - x1 * y3 * T(0,2,2)
1381 + y1 * z3 * T(1,2,1) - y1 * y3 * T(1,2,2)
1382 + z1 * z3 * T(2,2,1) - z1 * y3 * T(2,2,2);
1383
1384 double lz
1385 = - x1 * z3 * T(0,1,1) + x1 * y3 * T(0,1,2)
1386 - y1 * z3 * T(1,1,1) + y1 * y3 * T(1,1,2)
1387 - z1 * z3 * T(2,1,1) + z1 * y3 * T(2,1,2);
1388
1389 lines->push_back(HomgLine2D(lx, ly, lz));
1390 }
1391
1392 // awf removed deconditioning
1393 }
1394
get_constraint_lines_image1(vgl_homg_point_2d<double> const & p2,vgl_homg_point_2d<double> const & p3,std::vector<vgl_homg_line_2d<double>> & lines) const1395 void TriTensor::get_constraint_lines_image1(vgl_homg_point_2d<double> const& p2,
1396 vgl_homg_point_2d<double> const& p3,
1397 std::vector<vgl_homg_line_2d<double> >& lines) const
1398 {
1399 // use the same notation as the output of tr_hartley_equation.
1400
1401 double x2 = p2.x();
1402 double y2 = p2.y();
1403 double z2 = p2.w();
1404
1405 double x3 = p3.x();
1406 double y3 = p3.y();
1407 double z3 = p3.w();
1408
1409 lines.resize(0);
1410
1411 /* 0 */
1412
1413 {
1414 double lx
1415 = x2 * y3 * T(0,1,0)
1416 - y2 * y3 * T(0,0,0)
1417 - x2 * x3 * T(0,1,1)
1418 + y2 * x3 * T(0,0,1);
1419
1420 double ly
1421 = x2 * y3 * T(1,1,0)
1422 - y2 * y3 * T(1,0,0)
1423 - x2 * x3 * T(1,1,1)
1424 + y2 * x3 * T(1,0,1);
1425
1426 double lz
1427 = x2 * y3 * T(2,1,0)
1428 - y2 * y3 * T(2,0,0)
1429 - x2 * x3 * T(2,1,1)
1430 + y2 * x3 * T(2,0,1);
1431
1432 lines.emplace_back(lx, ly, lz);
1433 }
1434
1435 /* 1 */
1436 {
1437 double lx
1438 = x2 * z3 * T(0,1,0)
1439 - y2 * z3 * T(0,0,0)
1440 - x2 * x3 * T(0,1,2)
1441 + y2 * x3 * T(0,0,2);
1442
1443 double ly
1444 = x2 * z3 * T(1,1,0)
1445 - y2 * z3 * T(1,0,0)
1446 - x2 * x3 * T(1,1,2)
1447 + y2 * x3 * T(1,0,2);
1448
1449 double lz
1450 = x2 * z3 * T(2,1,0)
1451 - y2 * z3 * T(2,0,0)
1452 - x2 * x3 * T(2,1,2)
1453 + y2 * x3 * T(2,0,2);
1454
1455 lines.emplace_back(lx, ly, lz);
1456 }
1457
1458 /* 2 */
1459 {
1460 double lx
1461 = x2 * z3 * T(0,1,1)
1462 - y2 * z3 * T(0,0,1)
1463 - x2 * y3 * T(0,1,2)
1464 + y2 * y3 * T(0,0,2);
1465
1466 double ly
1467 = x2 * z3 * T(1,1,1)
1468 - y2 * z3 * T(1,0,1)
1469 - x2 * y3 * T(1,1,2)
1470 + y2 * y3 * T(1,0,2);
1471
1472 double lz
1473 = x2 * z3 * T(2,1,1)
1474 - y2 * z3 * T(2,0,1)
1475 - x2 * y3 * T(2,1,2)
1476 + y2 * y3 * T(2,0,2);
1477
1478 lines.emplace_back(lx, ly, lz);
1479 }
1480
1481 /* 3 */
1482 {
1483 double lx
1484 = x2 * y3 * T(0,2,0)
1485 - z2 * y3 * T(0,0,0)
1486 - x2 * x3 * T(0,2,1)
1487 + z2 * x3 * T(0,0,1);
1488
1489 double ly
1490 = x2 * y3 * T(1,2,0)
1491 - z2 * y3 * T(1,0,0)
1492 - x2 * x3 * T(1,2,1)
1493 + z2 * x3 * T(1,0,1);
1494
1495 double lz
1496 = x2 * y3 * T(2,2,0)
1497 - z2 * y3 * T(2,0,0)
1498 - x2 * x3 * T(2,2,1)
1499 + z2 * x3 * T(2,0,1);
1500
1501 lines.emplace_back(lx, ly, lz);
1502 }
1503
1504 /* 4 */
1505 {
1506 double lx
1507 = x2 * z3 * T(0,2,0)
1508 - z2 * z3 * T(0,0,0)
1509 - x2 * x3 * T(0,2,2)
1510 + z2 * x3 * T(0,0,2);
1511
1512 double ly
1513 = x2 * z3 * T(1,2,0)
1514 - z2 * z3 * T(1,0,0)
1515 - x2 * x3 * T(1,2,2)
1516 + z2 * x3 * T(1,0,2);
1517
1518 double lz
1519 = x2 * z3 * T(2,2,0)
1520 - z2 * z3 * T(2,0,0)
1521 - x2 * x3 * T(2,2,2)
1522 + z2 * x3 * T(2,0,2);
1523
1524 lines.emplace_back(lx, ly, lz);
1525 }
1526
1527 /* 5 */
1528 {
1529 double lx
1530 = x2 * z3 * T(0,2,1)
1531 - z2 * z3 * T(0,0,1)
1532 - x2 * y3 * T(0,2,2)
1533 + z2 * y3 * T(0,0,2);
1534
1535 double ly
1536 = x2 * z3 * T(1,2,1)
1537 - z2 * z3 * T(1,0,1)
1538 - x2 * y3 * T(1,2,2)
1539 + z2 * y3 * T(1,0,2);
1540
1541 double lz
1542 = x2 * z3 * T(2,2,1)
1543 - z2 * z3 * T(2,0,1)
1544 - x2 * y3 * T(2,2,2)
1545 + z2 * y3 * T(2,0,2);
1546
1547 lines.emplace_back(lx, ly, lz);
1548 }
1549
1550 /* 6 */
1551 {
1552 double lx
1553 = y2 * y3 * T(0,2,0)
1554 - z2 * y3 * T(0,1,0)
1555 - y2 * x3 * T(0,2,1)
1556 + z2 * x3 * T(0,1,1);
1557
1558 double ly
1559 = y2 * y3 * T(1,2,0)
1560 - z2 * y3 * T(1,1,0)
1561 - y2 * x3 * T(1,2,1)
1562 + z2 * x3 * T(1,1,1);
1563
1564 double lz
1565 = y2 * y3 * T(2,2,0)
1566 - z2 * y3 * T(2,1,0)
1567 - y2 * x3 * T(2,2,1)
1568 + z2 * x3 * T(2,1,1);
1569
1570 lines.emplace_back(lx, ly, lz);
1571 }
1572
1573 /* 7 */
1574 {
1575 double lx
1576 = y2 * z3 * T(0,2,0)
1577 - z2 * z3 * T(0,1,0)
1578 - y2 * x3 * T(0,2,2)
1579 + z2 * x3 * T(0,1,2);
1580
1581 double ly
1582 = y2 * z3 * T(1,2,0)
1583 - z2 * z3 * T(1,1,0)
1584 - y2 * x3 * T(1,2,2)
1585 + z2 * x3 * T(1,1,2);
1586
1587 double lz
1588 = y2 * z3 * T(2,2,0)
1589 - z2 * z3 * T(2,1,0)
1590 - y2 * x3 * T(2,2,2)
1591 + z2 * x3 * T(2,1,2);
1592
1593 lines.emplace_back(lx, ly, lz);
1594 }
1595
1596 /* 8 */
1597 {
1598 double lx
1599 = y2 * z3 * T(0,2,1)
1600 - z2 * z3 * T(0,1,1)
1601 - y2 * y3 * T(0,2,2)
1602 + z2 * y3 * T(0,1,2);
1603
1604 double ly
1605 = y2 * z3 * T(1,2,1)
1606 - z2 * z3 * T(1,1,1)
1607 - y2 * y3 * T(1,2,2)
1608 + z2 * y3 * T(1,1,2);
1609
1610 double lz
1611 = y2 * z3 * T(2,2,1)
1612 - z2 * z3 * T(2,1,1)
1613 - y2 * y3 * T(2,2,2)
1614 + z2 * y3 * T(2,1,2);
1615
1616 lines.emplace_back(lx, ly, lz);
1617 }
1618 }
1619
get_constraint_lines_image1(HomgPoint2D const & p2,HomgPoint2D const & p3,std::vector<HomgLine2D> * lines) const1620 void TriTensor::get_constraint_lines_image1(HomgPoint2D const& p2,
1621 HomgPoint2D const& p3,
1622 std::vector<HomgLine2D>* lines) const
1623 {
1624 // use the same notation as the output of tr_hartley_equation.
1625
1626 double x2 = p2.x();
1627 double y2 = p2.y();
1628 double z2 = p2.w();
1629
1630 double x3 = p3.x();
1631 double y3 = p3.y();
1632 double z3 = p3.w();
1633
1634 lines->resize(0);
1635
1636 /* 0 */
1637
1638 {
1639 double lx
1640 = x2 * y3 * T(0,1,0)
1641 - y2 * y3 * T(0,0,0)
1642 - x2 * x3 * T(0,1,1)
1643 + y2 * x3 * T(0,0,1);
1644
1645 double ly
1646 = x2 * y3 * T(1,1,0)
1647 - y2 * y3 * T(1,0,0)
1648 - x2 * x3 * T(1,1,1)
1649 + y2 * x3 * T(1,0,1);
1650
1651 double lz
1652 = x2 * y3 * T(2,1,0)
1653 - y2 * y3 * T(2,0,0)
1654 - x2 * x3 * T(2,1,1)
1655 + y2 * x3 * T(2,0,1);
1656
1657 lines->push_back(HomgLine2D(lx, ly, lz));
1658 }
1659
1660 /* 1 */
1661 {
1662 double lx
1663 = x2 * z3 * T(0,1,0)
1664 - y2 * z3 * T(0,0,0)
1665 - x2 * x3 * T(0,1,2)
1666 + y2 * x3 * T(0,0,2);
1667
1668 double ly
1669 = x2 * z3 * T(1,1,0)
1670 - y2 * z3 * T(1,0,0)
1671 - x2 * x3 * T(1,1,2)
1672 + y2 * x3 * T(1,0,2);
1673
1674 double lz
1675 = x2 * z3 * T(2,1,0)
1676 - y2 * z3 * T(2,0,0)
1677 - x2 * x3 * T(2,1,2)
1678 + y2 * x3 * T(2,0,2);
1679
1680 lines->push_back(HomgLine2D(lx, ly, lz));
1681 }
1682
1683 /* 2 */
1684 {
1685 double lx
1686 = x2 * z3 * T(0,1,1)
1687 - y2 * z3 * T(0,0,1)
1688 - x2 * y3 * T(0,1,2)
1689 + y2 * y3 * T(0,0,2);
1690
1691 double ly
1692 = x2 * z3 * T(1,1,1)
1693 - y2 * z3 * T(1,0,1)
1694 - x2 * y3 * T(1,1,2)
1695 + y2 * y3 * T(1,0,2);
1696
1697 double lz
1698 = x2 * z3 * T(2,1,1)
1699 - y2 * z3 * T(2,0,1)
1700 - x2 * y3 * T(2,1,2)
1701 + y2 * y3 * T(2,0,2);
1702
1703 lines->push_back(HomgLine2D(lx, ly, lz));
1704 }
1705
1706 /* 3 */
1707 {
1708 double lx
1709 = x2 * y3 * T(0,2,0)
1710 - z2 * y3 * T(0,0,0)
1711 - x2 * x3 * T(0,2,1)
1712 + z2 * x3 * T(0,0,1);
1713
1714 double ly
1715 = x2 * y3 * T(1,2,0)
1716 - z2 * y3 * T(1,0,0)
1717 - x2 * x3 * T(1,2,1)
1718 + z2 * x3 * T(1,0,1);
1719
1720 double lz
1721 = x2 * y3 * T(2,2,0)
1722 - z2 * y3 * T(2,0,0)
1723 - x2 * x3 * T(2,2,1)
1724 + z2 * x3 * T(2,0,1);
1725
1726 lines->push_back(HomgLine2D(lx, ly, lz));
1727 }
1728
1729 /* 4 */
1730 {
1731 double lx
1732 = x2 * z3 * T(0,2,0)
1733 - z2 * z3 * T(0,0,0)
1734 - x2 * x3 * T(0,2,2)
1735 + z2 * x3 * T(0,0,2);
1736
1737 double ly
1738 = x2 * z3 * T(1,2,0)
1739 - z2 * z3 * T(1,0,0)
1740 - x2 * x3 * T(1,2,2)
1741 + z2 * x3 * T(1,0,2);
1742
1743 double lz
1744 = x2 * z3 * T(2,2,0)
1745 - z2 * z3 * T(2,0,0)
1746 - x2 * x3 * T(2,2,2)
1747 + z2 * x3 * T(2,0,2);
1748
1749 lines->push_back(HomgLine2D(lx, ly, lz));
1750 }
1751
1752 /* 5 */
1753 {
1754 double lx
1755 = x2 * z3 * T(0,2,1)
1756 - z2 * z3 * T(0,0,1)
1757 - x2 * y3 * T(0,2,2)
1758 + z2 * y3 * T(0,0,2);
1759
1760 double ly
1761 = x2 * z3 * T(1,2,1)
1762 - z2 * z3 * T(1,0,1)
1763 - x2 * y3 * T(1,2,2)
1764 + z2 * y3 * T(1,0,2);
1765
1766 double lz
1767 = x2 * z3 * T(2,2,1)
1768 - z2 * z3 * T(2,0,1)
1769 - x2 * y3 * T(2,2,2)
1770 + z2 * y3 * T(2,0,2);
1771
1772 lines->push_back(HomgLine2D(lx, ly, lz));
1773 }
1774
1775 /* 6 */
1776 {
1777 double lx
1778 = y2 * y3 * T(0,2,0)
1779 - z2 * y3 * T(0,1,0)
1780 - y2 * x3 * T(0,2,1)
1781 + z2 * x3 * T(0,1,1);
1782
1783 double ly
1784 = y2 * y3 * T(1,2,0)
1785 - z2 * y3 * T(1,1,0)
1786 - y2 * x3 * T(1,2,1)
1787 + z2 * x3 * T(1,1,1);
1788
1789 double lz
1790 = y2 * y3 * T(2,2,0)
1791 - z2 * y3 * T(2,1,0)
1792 - y2 * x3 * T(2,2,1)
1793 + z2 * x3 * T(2,1,1);
1794
1795 lines->push_back(HomgLine2D(lx, ly, lz));
1796 }
1797
1798 /* 7 */
1799 {
1800 double lx
1801 = y2 * z3 * T(0,2,0)
1802 - z2 * z3 * T(0,1,0)
1803 - y2 * x3 * T(0,2,2)
1804 + z2 * x3 * T(0,1,2);
1805
1806 double ly
1807 = y2 * z3 * T(1,2,0)
1808 - z2 * z3 * T(1,1,0)
1809 - y2 * x3 * T(1,2,2)
1810 + z2 * x3 * T(1,1,2);
1811
1812 double lz
1813 = y2 * z3 * T(2,2,0)
1814 - z2 * z3 * T(2,1,0)
1815 - y2 * x3 * T(2,2,2)
1816 + z2 * x3 * T(2,1,2);
1817
1818 lines->push_back(HomgLine2D(lx, ly, lz));
1819 }
1820
1821 /* 8 */
1822 {
1823 double lx
1824 = y2 * z3 * T(0,2,1)
1825 - z2 * z3 * T(0,1,1)
1826 - y2 * y3 * T(0,2,2)
1827 + z2 * y3 * T(0,1,2);
1828
1829 double ly
1830 = y2 * z3 * T(1,2,1)
1831 - z2 * z3 * T(1,1,1)
1832 - y2 * y3 * T(1,2,2)
1833 + z2 * y3 * T(1,1,2);
1834
1835 double lz
1836 = y2 * z3 * T(2,2,1)
1837 - z2 * z3 * T(2,1,1)
1838 - y2 * y3 * T(2,2,2)
1839 + z2 * y3 * T(2,1,2);
1840
1841 lines->push_back(HomgLine2D(lx, ly, lz));
1842 }
1843
1844 // awf removed deconditioning
1845 }
1846
1847
1848 // == INPUT/OUTPUT ==
1849
1850 //-----------------------------------------------------------------------------
1851 //: Read from ASCII std::istream
operator >>(std::istream & s,TriTensor & T)1852 std::istream& operator>>(std::istream& s, TriTensor& T)
1853 {
1854 for (int i = 0; i < 3; ++i)
1855 for (int j = 0; j < 3; ++j)
1856 for (int k = 0; k < 3; ++k)
1857 s >> T(i,j,k);
1858 return s;
1859 }
1860
1861 //-----------------------------------------------------------------------------
1862 //: Print in ASCII to std::ostream
operator <<(std::ostream & s,const TriTensor & T)1863 std::ostream& operator<<(std::ostream& s, const TriTensor& T)
1864 {
1865 for (int i = 0; i < 3; ++i) {
1866 for (int j = 0; j < 3; ++j) {
1867 for (int k = 0; k < 3; ++k)
1868 vul_printf(s, "%20.16e ", T(i,j,k));
1869 s << std::endl;
1870 }
1871 s << std::endl;
1872 }
1873 return s;
1874 }
1875
1876 struct Column3x3 : public vnl_double_3x3
1877 {
Column3x3Column3x31878 Column3x3(const vnl_vector<double>& v1, const vnl_vector<double>& v2, const vnl_vector<double>& v3)
1879 {
1880 (*this)(0,0) = v1[0]; (*this)(0,1) = v2[0]; (*this)(0,2) = v3[0];
1881 (*this)(1,0) = v1[1]; (*this)(1,1) = v2[1]; (*this)(1,2) = v3[1];
1882 (*this)(2,0) = v1[2]; (*this)(2,1) = v2[2]; (*this)(2,2) = v3[2];
1883 }
1884 };
1885
1886 // == FUNDAMENTAL MATRICES AND EPIPOLES ==
1887
1888 //: Compute and cache the two epipoles from image 1.
compute_epipoles() const1889 bool TriTensor::compute_epipoles() const
1890 {
1891 vnl_double_3x3 T1 = dot1(vnl_double_3(1,0,0).as_ref());
1892 vnl_double_3x3 T2 = dot1(vnl_double_3(0,1,0).as_ref());
1893 vnl_double_3x3 T3 = dot1(vnl_double_3(0,0,1).as_ref());
1894
1895 vnl_svd<double> svd1(T1.as_ref());
1896 vnl_double_3 u1 = svd1.nullvector();
1897 vnl_double_3 v1 = svd1.left_nullvector();
1898
1899 vnl_svd<double> svd2(T2.as_ref());
1900 vnl_double_3 u2 = svd2.nullvector();
1901 vnl_double_3 v2 = svd2.left_nullvector();
1902
1903 vnl_svd<double> svd3(T3.as_ref());
1904 vnl_double_3 u3 = svd3.nullvector();
1905 vnl_double_3 v3 = svd3.left_nullvector();
1906
1907 vnl_double_3x3 V;
1908 V(0,0) = v1[0]; V(0,1) = v2[0]; V(0,2) = v3[0];
1909 V(1,0) = v1[1]; V(1,1) = v2[1]; V(1,2) = v3[1];
1910 V(2,0) = v1[2]; V(2,1) = v2[2]; V(2,2) = v3[2];
1911
1912 vnl_svd<double> svdv(V.as_ref());
1913
1914 delete e12_;
1915 e12_ = new HomgPoint2D(svdv.left_nullvector());
1916
1917 vnl_double_3x3 U;
1918 U(0,0) = u1[0]; U(0,1) = u2[0]; U(0,2) = u3[0];
1919 U(1,0) = u1[1]; U(1,1) = u2[1]; U(1,2) = u3[1];
1920 U(2,0) = u1[2]; U(2,1) = u2[2]; U(2,2) = u3[2];
1921
1922 vnl_svd<double> svdu(U.as_ref());
1923
1924 delete e13_;
1925 e13_ = new HomgPoint2D(svdu.left_nullvector());
1926
1927 return e12_!=nullptr && e13_!=nullptr;
1928 }
1929
1930 //: Return epipoles e2 and e3, from image 1 into images 2 and 3 respectively.
get_epipoles(vgl_homg_point_2d<double> & e2,vgl_homg_point_2d<double> & e3) const1931 bool TriTensor::get_epipoles(vgl_homg_point_2d<double>& e2,
1932 vgl_homg_point_2d<double>& e3) const
1933 {
1934 // Check if cached.
1935 if (!e12_ || !e13_)
1936 compute_epipoles();
1937
1938 e2.set(e12_->x(),e12_->y(),e12_->w());
1939 e3.set(e13_->x(),e13_->y(),e13_->w());
1940 return true;
1941 }
1942
get_epipoles(HomgPoint2D * e2,HomgPoint2D * e3) const1943 bool TriTensor::get_epipoles(HomgPoint2D* e2,
1944 HomgPoint2D* e3) const
1945 {
1946 // Check if cached.
1947 if (!e12_ || !e13_)
1948 compute_epipoles();
1949
1950 if (e2) *e2 = *e12_;
1951 if (e3) *e3 = *e13_;
1952 return true;
1953 }
1954
1955 //: Return epipole12.
get_epipole_12() const1956 HomgPoint2D TriTensor::get_epipole_12() const
1957 {
1958 get_epipoles(nullptr,nullptr);
1959 return *e12_;
1960 }
1961
1962 //: Return epipole13.
get_epipole_13() const1963 HomgPoint2D TriTensor::get_epipole_13() const
1964 {
1965 get_epipoles(nullptr,nullptr);
1966 return *e13_;
1967 }
1968
1969 //: Return F12, the fundamental matrix between images 1 and 2
get_fmatrix_12() const1970 FMatrix TriTensor::get_fmatrix_12() const
1971 {
1972 get_epipoles(nullptr,nullptr);
1973 return FMatrix(vnl_cross_product_matrix(e12_->get_vector()) * dot3(e13_->get_vector().as_ref()).transpose().as_ref());
1974 }
1975
1976 //: Return F13, the fundamental matrix between images 1 and 3
get_fmatrix_13() const1977 FMatrix TriTensor::get_fmatrix_13() const
1978 {
1979 get_epipoles(nullptr,nullptr);
1980 return FMatrix(vnl_cross_product_matrix(e13_->get_vector()) * dot2(e12_->get_vector().as_ref()).transpose().as_ref());
1981 }
1982
compute_fmatrix_23() const1983 FMatrix TriTensor::compute_fmatrix_23() const
1984 {
1985 PMatrix P2, P3;
1986 compute_P_matrices(&P2, &P3);
1987 return FMatrix(P2, P3);
1988 }
1989
1990 //: Return a manifold-projector for the Fundamental matrix between 1 and 2.
1991 // The projector is cached until the next time T is changed.
get_fmp12() const1992 const FManifoldProject* TriTensor::get_fmp12() const
1993 {
1994 // If not cached, compute it.
1995 if (!fmp12_) fmp12_ = new FManifoldProject(get_fmatrix_12());
1996 return fmp12_;
1997 }
1998
1999 //: Return a manifold-projector as above, between 1 and 3.
get_fmp13() const2000 const FManifoldProject* TriTensor::get_fmp13() const
2001 {
2002 // If not cached, compute it.
2003 if (!fmp13_) fmp13_ = new FManifoldProject(get_fmatrix_13());
2004 return fmp13_;
2005 }
2006
2007 //: Return a manifold-projector as above, between 2 and 3.
get_fmp23() const2008 const FManifoldProject* TriTensor::get_fmp23() const
2009 {
2010 // If not cached, compute it.
2011 if (!fmp23_) {
2012 // Need to get FMatrix 23
2013 PMatrix P2;
2014 PMatrix P3;
2015 compute_P_matrices(&P2, &P3);
2016 FMatrix f23(P2,P3);
2017
2018 fmp23_ = new FManifoldProject(f23);
2019 }
2020 return fmp23_;
2021 }
2022
2023 //: Compute one of the family of P matrix triplets consistent with this T
compute_P_matrices(const vnl_double_3 & x,double alpha,double beta,PMatrix * P2,PMatrix * P3) const2024 void TriTensor::compute_P_matrices(const vnl_double_3& x, double alpha, double beta, PMatrix* P2, PMatrix* P3) const
2025 {
2026 HomgPoint2D e2 = get_epipole_12();
2027 HomgPoint2D e3 = get_epipole_13();
2028
2029 vnl_double_3x3 Te3 = dot3t(e3.as_ref());
2030 vnl_double_3x3 TTe2 = dot2t(e2.as_ref());
2031
2032 MATLABPRINT((vnl_matrix<double> const&/*2.7*/)Te3);
2033 MATLABPRINT((vnl_matrix<double> const&/*2.7*/)TTe2);
2034
2035 vnl_double_3x3 M = vnl_identity_3x3() - OuterProduct3x3(e3,e3);
2036
2037 vnl_double_3x3 B0 = -M * TTe2;
2038
2039 vnl_double_3x3 DIFF = B0 + TTe2 - OuterProduct3x3(e3, TTe2.transpose()*e3);
2040 double diffmag = DIFF.fro_norm();
2041 if (diffmag > 1e-12) {
2042 std::cerr << "TriTensor::compute_P_matrices: DIFF = " << DIFF << '\n';
2043 }
2044
2045 vnl_double_3x3& A0 = Te3;
2046
2047 // P1 = [I O];
2048 //P2 = [A0 + e2 * x(:)' , beta*e2];
2049 //P3 = [B0 + e3 * x(:)' , alpha*e3 ];
2050
2051 P2->set(A0 + OuterProduct3x3(e2, x), beta*e2);
2052 P3->set(B0 + OuterProduct3x3(e3, x), alpha*e3);
2053
2054 std::cerr << *P2 << '\n'
2055 << *P3 << '\n';
2056
2057 // Check
2058 this->check_equal_up_to_scale(TriTensor(*P2, *P3));
2059 }
2060
2061 struct maxabs
2062 {
2063 int i, j, k;
2064 double maxval;
2065 maxabs(const TriTensor& T);
2066 };
2067
maxabs(const TriTensor & T)2068 maxabs::maxabs(const TriTensor& T) : i(0), j(0), k(0), maxval(0.0)
2069 {
2070 for (int i = 0; i < 3; ++i)
2071 for (int j = 0; j < 3; ++j)
2072 for (int k = 0; k < 3; ++k) {
2073 double v = std::fabs(T(i,j,k));
2074 if (v >= maxval) {
2075 maxval = v;
2076 this->i = i;
2077 this->j = j;
2078 this->k = k;
2079 }
2080 }
2081 }
2082
check_same(const TriTensor & T1,const TriTensor & T2)2083 static bool check_same(const TriTensor& T1, const TriTensor& T2)
2084 {
2085 maxabs m1(T1);
2086 double scale1 = 1/m1.maxval;
2087 double scale2 = 1/std::fabs(T2(m1.i,m1.j,m1.k));
2088
2089 double rms = 0;
2090 for (int i = 0; i < 3; ++i)
2091 for (int j = 0; j < 3; ++j)
2092 for (int k = 0; k < 3; ++k) {
2093 double d = T1(i,j,k)*scale1 - T2(i,j,k) * scale2;
2094 rms += d*d;
2095 }
2096
2097 rms /= 27;
2098
2099 if (rms > 1e-15) {
2100 std::cerr << "check_same: different TriTensors\n"
2101 << "T1 =\n" << T1
2102 << "T2 =\n" << T2;
2103 return false;
2104 }
2105
2106 return true;
2107 }
2108
2109 //: Check that another trifocal tensor is equal to this one up to scale.
2110 // Finds largest component of this, scales both tritensors so that this
2111 // component is one, and checks that fronorm of difference is small.
2112 // Prints a message if not.
check_equal_up_to_scale(const TriTensor & that) const2113 bool TriTensor::check_equal_up_to_scale(const TriTensor& that) const
2114 {
2115 return check_same(*this, that);
2116 }
2117