1 // This is core/vpgl/vpgl_proj_camera.hxx
2 #ifndef vpgl_tri_focal_tensor_hxx_
3 #define vpgl_tri_focal_tensor_hxx_
4
5 #include "vpgl_tri_focal_tensor.h"
6 #include <vul/vul_printf.h>
7 #include <vgl/vgl_homg_point_2d.h>
8 #include <vgl/vgl_homg_line_2d.h>
9 #include <vgl/vgl_tolerance.h>
10 #include <vgl/algo/vgl_homg_operators_2d.h>
11 #include <vnl/vnl_inverse.h>
12 #include <vnl/vnl_cross_product_matrix.h>
epsilon(size_t i,size_t j,size_t k)13 static int epsilon(size_t i, size_t j, size_t k){
14 if(i==j||j==k||k==i)
15 return 0;
16 if(i==0&&j==1&&k==2)
17 return 1;
18 if(i==1&&j==2&&k==0)
19 return 1;
20 if(i==2&&j==0&&k==1)
21 return 1;
22 return -1;
23 }
24 template <class Type>
init()25 void vpgl_tri_focal_tensor<Type>::init(){
26
27 // epipoles
28 epipoles_valid_ = false;
29 e12_.set(Type(0),Type(0),Type(0));
30 e13_.set(Type(0),Type(0),Type(0));
31
32 // cameras
33 if(!cameras_valid_){
34 vnl_matrix_fixed<Type, 3, 4> c_invalid(Type(0));
35 c1_.set_matrix(c_invalid);
36 c2_.set_matrix(c_invalid);
37 c3_.set_matrix(c_invalid);
38 }
39 // fundamental matrices
40 f_matrices_1213_valid_ = false;
41 vnl_matrix_fixed<Type,3,3> f_invalid(Type(0));
42 f12_.set_matrix(f_invalid);
43 f13_.set_matrix(f_invalid);
44
45 f_matrix_23_valid_ = false;
46 f23_.set_matrix(f_invalid);
47 }
48 template <class Type>
normalize()49 void vpgl_tri_focal_tensor<Type>::normalize(){
50 Type sum_sqrs = Type(0);
51 for (size_t i = 0; i < 3; ++i)
52 for (size_t j = 0; j < 3; ++j)
53 for (size_t k = 0; k < 3; ++k){
54 Type t = T_(i,j,k);
55 sum_sqrs += t*t;
56 }
57 sum_sqrs/=Type(27);
58 Type s = sqrt(sum_sqrs);
59 Type ptol = vgl_tolerance<Type>::position;
60 if(s<ptol){
61 std::cout << " Frobenius norm too low - " << s<< " < " << ptol << " can't normalize" << std::endl;
62 return;
63 }
64 for (size_t i = 0; i < 3; ++i)
65 for (size_t j = 0; j < 3; ++j)
66 for (size_t k = 0; k < 3; ++k)
67 T_(i,j,k) /= s;
68 }
69
70 template <class Type>
compute_proj_cameras()71 bool vpgl_tri_focal_tensor<Type>::compute_proj_cameras(){
72 if(cameras_valid_)
73 return true;
74 if(!epipoles_valid_)
75 this->compute_epipoles();
76 if(!epipoles_valid_)
77 return false;
78 c1_ = vpgl_proj_camera<Type>(); // canonical camera
79 vnl_vector_fixed<Type,3> x(Type(1),Type(1),Type(1));
80 Type alpha = Type(1), beta = Type(1);
81 vnl_vector_fixed<Type, 3> e12(e12_.x(), e12_.y(), e12_.w()), e13(e13_.x(), e13_.y(), e13_.w());
82 vnl_matrix_fixed<Type, 3, 3> Te3 = dot3t(e13.as_ref());
83 vnl_matrix_fixed<Type, 3, 3> TTe2 = dot2t(e12.as_ref());
84 vnl_matrix_fixed<Type, 3, 3> M; M.set_identity();
85 M -= outer_product(e13,e13);
86 vnl_matrix_fixed<Type, 3, 3> B0 = -M*TTe2;
87 vnl_matrix_fixed<Type, 3, 3> DIFF = B0 + TTe2 - outer_product(e13, TTe2.transpose()*e13);
88 double diffmag = DIFF.fro_norm();
89 if(diffmag>Type(1e-12))
90 return false;
91 vnl_matrix_fixed<Type, 3, 3> A0 = Te3;
92 c2_.set_matrix(A0 + outer_product(e12, x), beta*e12);
93 c3_.set_matrix(B0 + outer_product(e13, x), beta*e13);
94 // check
95 if (!within_scale(*this, vpgl_tri_focal_tensor<Type>(c2_, c3_)))
96 return false;
97 cameras_valid_ = true;
98 return true;
99 }
100 template <class Type>
set(const vpgl_proj_camera<Type> & c1,const vpgl_proj_camera<Type> & c2,const vpgl_proj_camera<Type> & c3)101 void vpgl_tri_focal_tensor<Type>::set(const vpgl_proj_camera<Type>& c1, const vpgl_proj_camera<Type>& c2, const vpgl_proj_camera<Type>& c3){
102 vnl_matrix_fixed<Type, 3, 3> M2, M3;
103 vnl_vector_fixed<Type, 3> p2, p3;
104 cameras_valid_ = true;
105 bool c1_is_can = c1.is_canonical(static_cast<Type>(1.0e-6));//may need to be adjusted if noisy data? FIXME- JLM
106 if(c1_is_can){
107 c2.decompose(M2, p2);
108 c3.decompose(M3, p3);
109 c1_ = c1; c2_ = c2; c3_ =c3;
110 }else{
111
112 vgl_h_matrix_3d<Type> hc = get_canonical_h(c1);
113 vpgl_proj_camera<Type> c1_can(c1.get_matrix()*hc.get_matrix());
114 vpgl_proj_camera<Type> c2_can(c2.get_matrix()*hc.get_matrix());
115 vpgl_proj_camera<Type> c3_can(c3.get_matrix()*hc.get_matrix());
116 c1_ = c1_can; c2_ = c2_can; c3_ =c3_can;
117 c2_can.decompose(M2, p2);
118 c3_can.decompose(M3, p3);
119 }
120 for (size_t i = 0; i < 3; ++i)
121 for (size_t j = 0; j < 3; ++j)
122 for (size_t k = 0; k < 3; ++k)
123 T_(i,j,k) = (M2(j,i) * p3[k] - M3(k,i) * p2[j]);
124 this->normalize();
125 }
126
127 // == CONTRACTION WITH VECTORS ==
128
129 //: Compute ${\tt M}_{jk} = T_{ijk} v_i$.
130 template <class Type>
dot1(const vnl_vector_fixed<Type,3> & v) const131 vnl_matrix_fixed<Type,3,3> vpgl_tri_focal_tensor<Type>::dot1(const vnl_vector_fixed<Type, 3>& v) const
132 {
133 vnl_matrix_fixed<Type,3,3> answer; answer.fill(0.0);
134 for (int i = 0; i < 3; i++)
135 for (int j = 0; j < 3; j++)
136 for (int k = 0; k < 3; k++)
137 answer(j,k) += v[i] * T_(i,j,k);
138 return answer;
139 }
140
141 //: Compute ${\tt M}_{ik} = T_{ijk} v_j$.
142 template <class Type>
dot2(const vnl_vector_fixed<Type,3> & v) const143 vnl_matrix_fixed<Type,3,3> vpgl_tri_focal_tensor<Type>::dot2(const vnl_vector_fixed<Type, 3>& v) const
144 {
145 vnl_matrix_fixed<Type,3,3> answer; answer.fill(0.0);
146 for (int i = 0; i < 3; i++)
147 for (int j = 0; j < 3; j++)
148 for (int k = 0; k < 3; k++)
149 answer(i,k) += v[j] * T_(i,j,k);
150 return answer;
151 }
152
153 //: Compute ${\tt M}_{ij} = T_{ijk} v_k$.
154 template <class Type>
dot3(const vnl_vector_fixed<Type,3> & v) const155 vnl_matrix_fixed<Type,3,3> vpgl_tri_focal_tensor<Type>::dot3(const vnl_vector_fixed<Type, 3>& v) const
156 {
157 vnl_matrix_fixed<Type,3,3> answer; answer.fill(0.0);
158 for (int i = 0; i < 3; i++)
159 for (int j = 0; j < 3; j++)
160 for (int k = 0; k < 3; k++)
161 answer(i,j) += v[k] * T_(i,j,k);
162 return answer;
163 }
164
165 //: Compute ${\tt M}_{kj} = T_{ijk} v_i$. (The transpose of dot1).
166 template <class Type>
dot1t(const vnl_vector_fixed<Type,3> & v) const167 vnl_matrix_fixed<Type,3,3> vpgl_tri_focal_tensor<Type>::dot1t(const vnl_vector_fixed<Type, 3>& v) const
168 {
169 vnl_matrix_fixed<Type,3,3> answer; answer.fill(0.0);
170 for (int i = 0; i < 3; i++)
171 for (int j = 0; j < 3; j++)
172 for (int k = 0; k < 3; k++)
173 answer(k,j) += v[i] * T_(i,j,k);
174 return answer;
175 }
176
177 //: Compute ${\tt M}_{ki} = T_{ijk} v_j$.
178 template <class Type>
dot2t(const vnl_vector_fixed<Type,3> & v) const179 vnl_matrix_fixed<Type,3,3> vpgl_tri_focal_tensor<Type>::dot2t(const vnl_vector_fixed<Type, 3>& v) const
180 {
181 vnl_matrix_fixed<Type,3,3> answer; answer.fill(0.0);
182 for (int i = 0; i < 3; i++)
183 for (int j = 0; j < 3; j++)
184 for (int k = 0; k < 3; k++)
185 answer(k,i) += v[j] * T_(i,j,k);
186 return answer;
187 }
188
189 //: Compute ${\tt M}_{ji} = T_{ijk} v_k$.
190 template <class Type>
dot3t(const vnl_vector_fixed<Type,3> & v) const191 vnl_matrix_fixed<Type,3,3> vpgl_tri_focal_tensor<Type>::dot3t(const vnl_vector_fixed<Type, 3>& v) const
192 {
193 vnl_matrix_fixed<Type,3,3> answer; answer.fill(0.0);
194 for (int i = 0; i < 3; i++)
195 for (int j = 0; j < 3; j++)
196 for (int k = 0; k < 3; k++)
197 answer(j,i) += v[k] * T_(i,j,k);
198 return answer;
199 }
200
201 //: Compute ${\tt M}_{rs} = x_i(x'_j epsilon_{jpr}) (x''_k epsilon+{kqs})T_{ipq)$.
202 template <class Type>
203 vnl_matrix_fixed<Type, 3,3> vpgl_tri_focal_tensor<Type>::
point_constraint_3x3(vgl_homg_point_2d<Type> const & point1,vgl_homg_point_2d<Type> const & point2,vgl_homg_point_2d<Type> const & point3)204 point_constraint_3x3(vgl_homg_point_2d<Type> const& point1,
205 vgl_homg_point_2d<Type> const& point2,
206 vgl_homg_point_2d<Type> const& point3){
207 Type z = Type(0);
208 vnl_vector_fixed<Type, 3> x(z), xp(z), xpp(z);
209 x[0] = point1.x(); x[1] = point1.y(); x[2] = point1.w();
210 xp[0] = point2.x(); xp[1] = point2.y(); xp[2] = point2.w();
211 xpp[0] = point3.x(); xpp[1] = point3.y(); xpp[2] = point3.w();
212 vnl_matrix_fixed<Type,3,3> zero(0.0);
213 for(size_t r = 0; r<3; ++r)
214 for(size_t s = 0; s<3; ++s){
215 Type sum_rs = Type(0);
216 Type n = Type(0);
217 for (size_t i = 0; i < 3; i++)
218 for (size_t p = 0; p < 3; p++)
219 for (size_t q = 0; q < 3; q++){
220 Type sum_j = Type(0), sum_k = Type(0);
221
222 for(size_t j = 0; j < 3; ++j)
223 sum_j += xp[j]*epsilon(j,p,r);
224
225 for(size_t k = 0;k < 3; ++k)
226 sum_k += xpp[k]*epsilon(k,q,s);
227
228 sum_rs += x[i]*sum_j*sum_k*T_(i,p,q);
229 n += Type(1);
230 }
231 zero[r][s] = sum_rs/n;
232 }
233 return zero;
234 }
235 template <class Type>
point_constraint(vgl_homg_point_2d<Type> const & point1,vgl_homg_point_2d<Type> const & point2,vgl_homg_point_2d<Type> const & point3)236 Type vpgl_tri_focal_tensor<Type>::point_constraint(vgl_homg_point_2d<Type> const& point1,
237 vgl_homg_point_2d<Type> const& point2,
238 vgl_homg_point_2d<Type> const& point3){
239 Type z = Type(0);
240 vnl_vector_fixed<Type, 3> m(z), mp(z), mpp(z);
241 m[0] = point1.x(); m[1] = point1.y(); m[2] = point1.w();
242 mp[0] = point2.x(); mp[1] = point2.y(); mp[2] = point2.w();
243 mpp[0] = point3.x(); mpp[1] = point3.y(); mpp[2] = point3.w();
244 Type total_sum = z;
245 Type n_terms = z;
246 for (size_t i = 0; i < 3; i++){
247 Type sum_jk = z;
248 for(size_t j = 0; j < 3; ++j)
249 for(size_t k = 0; k < 3; ++k){
250 sum_jk += mp[j]*mpp[k]*T_(i,2,2);
251 sum_jk -= mpp[k]*T_(i,j,2);
252 sum_jk -= mp[j]*T_(i,2,k);
253 sum_jk += T_(i,j,k);
254 n_terms += Type(1);
255 }
256 total_sum += m[i]*sum_jk;
257 }
258 return total_sum/n_terms;
259 }
260 template <class Type>
line_constraint_3(vgl_homg_line_2d<Type> const & line1,vgl_homg_line_2d<Type> const & line2,vgl_homg_line_2d<Type> const & line3)261 vnl_vector_fixed<Type, 3> vpgl_tri_focal_tensor<Type>::line_constraint_3(vgl_homg_line_2d<Type> const& line1,
262 vgl_homg_line_2d<Type> const& line2,
263 vgl_homg_line_2d<Type> const& line3){
264 Type z = Type(0);
265 vnl_vector_fixed<Type, 3> l(z), lp(z), lpp(z);
266 l[0] = line1.a(); l[1] = line1.b(); l[2] = line1.c();
267 lp[0] = line2.a(); lp[1] = line2.b(); lp[2] = line2.c();
268 lpp[0] = line3.a(); lpp[1] = line3.b(); lpp[2] = line3.c();
269 vnl_vector_fixed<Type,3> zero_s(0.0);
270 for(size_t s = 0; s<3; ++s){
271 Type sum_r = Type(0);
272 Type n = Type(0);
273 for(size_t r = 0; r<3; ++r){
274 for (size_t i = 0; i < 3; i++)
275 for (size_t j = 0; j < 3; j++)
276 for (size_t k = 0; k < 3; k++){
277 n += Type(1);
278 sum_r += l[r]*epsilon(r, i, s)*lp[j]*lpp[k]*T_(i,j,k);
279 }
280 zero_s[s] = sum_r/n;
281 }
282 }
283 return zero_s;
284 }
285
286 template <class Type>
hmatrix_13(vgl_homg_line_2d<Type> const & line2) const287 vgl_h_matrix_2d<Type> vpgl_tri_focal_tensor<Type>::hmatrix_13(vgl_homg_line_2d<Type> const& line2) const{
288 vnl_vector_fixed<Type,3> l2(line2.a(),line2.b(),line2.c());
289 return vgl_h_matrix_2d<Type>(dot2t(l2));
290 }
291
292 template <class Type>
hmatrix_12(vgl_homg_line_2d<Type> const & line3) const293 vgl_h_matrix_2d<Type> vpgl_tri_focal_tensor<Type>::hmatrix_12(vgl_homg_line_2d<Type> const& line3) const{
294 vnl_vector_fixed<Type,3> l3(line3.a(),line3.b(),line3.c());
295 return vgl_h_matrix_2d<Type>(dot3t(l3));//do3->dot3t from oxl/mvl/TriTensor
296 }
297
298 //: Contract tensor axis tensor_axis with first component of matrix $M$.
299 // That is, where $S$ is the result of the operation:
300 //
301 // - For tensor_axis = 1, compute $S_{ijk} = T_{pjk} M_{pi}$
302 // - For tensor_axis = 2, compute $S_{ijk} = T_{ipk} M_{pj}$
303 // - For tensor_axis = 3, compute $S_{ijk} = T_{ijp} M_{pk}$
304 template <class Type>
postmultiply(size_t tensor_axis,const vnl_matrix<Type> & M) const305 vpgl_tri_focal_tensor<Type> vpgl_tri_focal_tensor<Type>::postmultiply(size_t tensor_axis, const vnl_matrix<Type>& M) const{
306 switch (tensor_axis) {
307 case 1: return postmultiply1(M);
308 case 2: return postmultiply2(M);
309 case 3: return postmultiply3(M);
310 default: throw std::invalid_argument("unexpected tensor_axis");
311 }
312 }
313
314 //: Contract tensor axis tensor_axis with second component of matrix $M$.
315 // That is, where $S$ is the result of the operation:
316 //
317 // - For tensor_axis = 1, compute $S_{ijk} = M_{ip} T_{pjk}$
318 // - For tensor_axis = 2, compute $S_{ijk} = M_{jp} T_{ipk}$
319 // - For tensor_axis = 3, compute $S_{ijk} = M_{kp} T_{ijp}$
320 template <class Type>
premultiply(size_t tensor_axis,const vnl_matrix<Type> & M) const321 vpgl_tri_focal_tensor<Type> vpgl_tri_focal_tensor<Type>::premultiply(size_t tensor_axis, const vnl_matrix<Type>& M) const{
322 switch (tensor_axis) {
323 case 1: return premultiply1(M);
324 case 2: return premultiply2(M);
325 case 3: return premultiply3(M);
326 default: throw std::invalid_argument("unexpected tensor_axis");
327 }
328 }
329 //: Compute $ S_{ijk} = T_{pjk} M_{pi} $.
330 template <class Type>
postmultiply1(const vnl_matrix<Type> & M) const331 vpgl_tri_focal_tensor<Type> vpgl_tri_focal_tensor<Type>::postmultiply1(const vnl_matrix<Type>& M) const{
332 vpgl_tri_focal_tensor<Type> S;
333 for (int i = 0; i < 3; ++i)
334 for (int j = 0; j < 3; ++j)
335 for (int k = 0; k < 3; ++k) {
336 double v = 0;
337 for (int p = 0; p < 3; ++p)
338 v += T_(p,j,k) * M(p,i);
339 S(i,j,k) = v;
340 }
341 return S;
342 }
343
344
345 //: Compute $ S_{ijk} = T_{ipk} M_{pj} $.
346 template <class Type>
postmultiply2(const vnl_matrix<Type> & M) const347 vpgl_tri_focal_tensor<Type> vpgl_tri_focal_tensor<Type>::postmultiply2(const vnl_matrix<Type>& M) const{
348 vpgl_tri_focal_tensor<Type> S;
349 for (int i = 0; i < 3; ++i)
350 for (int j = 0; j < 3; ++j)
351 for (int k = 0; k < 3; ++k) {
352 double v = 0;
353 for (int p = 0; p < 3; ++p)
354 v += T_(i,p,k) * M(p,j);
355 S(i,j,k) = v;
356 }
357 return S;
358 }
359 //: Compute $ S_{ijk} = T_{ijp} M_{pk} $.
360 template <class Type>
postmultiply3(const vnl_matrix<Type> & M) const361 vpgl_tri_focal_tensor<Type> vpgl_tri_focal_tensor<Type>::postmultiply3(const vnl_matrix<Type>& M) const{
362 vpgl_tri_focal_tensor<Type> S;
363 for (int i = 0; i < 3; ++i)
364 for (int j = 0; j < 3; ++j)
365 for (int k = 0; k < 3; ++k) {
366 double v = 0;
367 for (int p = 0; p < 3; ++p)
368 v += T_(i,j,p) * M(p,k);
369 S(i,j,k) = v;
370 }
371 return S;
372 }
373 //: Compute $ S_{ijk} = M_{ip} T_{pjk} $.
374 template <class Type>
premultiply1(const vnl_matrix<Type> & M) const375 vpgl_tri_focal_tensor<Type> vpgl_tri_focal_tensor<Type>::premultiply1(const vnl_matrix<Type>& M) const{
376 vpgl_tri_focal_tensor<Type> S;
377 for (int i = 0; i < 3; ++i)
378 for (int j = 0; j < 3; ++j)
379 for (int k = 0; k < 3; ++k) {
380 double v = 0;
381 for (int p = 0; p < 3; ++p)
382 v += M(i,p) * T_(p,j,k);
383 S(i,j,k) = v;
384 }
385 return S;
386 }
387 //: Compute $ S_{ijk} = M_{jp} T_{ipk} $.
388 template <class Type>
premultiply2(const vnl_matrix<Type> & M) const389 vpgl_tri_focal_tensor<Type> vpgl_tri_focal_tensor<Type>::premultiply2(const vnl_matrix<Type>& M) const{
390 vpgl_tri_focal_tensor<Type> S;
391 for (int i = 0; i < 3; ++i)
392 for (int j = 0; j < 3; ++j)
393 for (int k = 0; k < 3; ++k) {
394 double v = 0;
395 for (int p = 0; p < 3; ++p)
396 v += M(j,p) * T_(i,p,k);
397 S(i,j,k) = v;
398 }
399 return S;
400 }
401 //: Compute $ S_{ijk} = M_{kp} T_{ijp} $.
402 template <class Type>
premultiply3(const vnl_matrix<Type> & M) const403 vpgl_tri_focal_tensor<Type> vpgl_tri_focal_tensor<Type>::premultiply3(const vnl_matrix<Type>& M) const{
404 vpgl_tri_focal_tensor<Type> S;
405 for (int i = 0; i < 3; ++i)
406 for (int j = 0; j < 3; ++j)
407 for (int k = 0; k < 3; ++k) {
408 double v = 0;
409 for (int p = 0; p < 3; ++p)
410 v += M(k,p) * T_(i,j,p);
411 S(i,j,k) = v;
412 }
413 return S;
414 }
415 template <class Type>
image1_transfer(vgl_homg_line_2d<Type> const & line2,vgl_homg_line_2d<Type> const & line3) const416 vgl_homg_line_2d<Type> vpgl_tri_focal_tensor<Type>::image1_transfer(vgl_homg_line_2d<Type> const& line2,
417 vgl_homg_line_2d<Type> const& line3) const
418 {
419 vnl_vector_fixed<Type, 3> l1(0.0, 0.0, 0.0);
420 vnl_vector_fixed<Type, 3> l2(line2.a(), line2.b(), line2.c());
421 vnl_vector_fixed<Type, 3> l3(line3.a(), line3.b(), line3.c());
422
423 for (int i = 0; i < 3; i++)
424 for (int j = 0; j < 3; j++)
425 for (int k = 0; k < 3; k++)
426 l1[i] += T_(i, j, k) * l2[j] * l3[k];
427
428 return vgl_homg_line_2d<Type>(l1[0], l1[1], l1[2]);
429
430 }
431
432 template <class Type>
image2_transfer(vgl_homg_line_2d<Type> const & line1,vgl_homg_line_2d<Type> const & line3) const433 vgl_homg_line_2d<Type> vpgl_tri_focal_tensor<Type>::image2_transfer(vgl_homg_line_2d<Type> const& line1,
434 vgl_homg_line_2d<Type> const& line3) const
435 {
436 vnl_vector_fixed<Type,3> l1(line1.a(),line1.b(),line1.c());
437 vnl_vector_fixed<Type,3> l3(line3.a(),line3.b(),line3.c());
438 vnl_vector_fixed<Type,3> l = vnl_inverse(dot3(l3)) * l1;
439 return vgl_homg_line_2d<Type>(l[0],l[1],l[2]);
440 }
441
442 template <class Type>
image3_transfer(vgl_homg_line_2d<Type> const & line1,vgl_homg_line_2d<Type> const & line2) const443 vgl_homg_line_2d<Type> vpgl_tri_focal_tensor<Type>::image3_transfer(vgl_homg_line_2d<Type> const& line1,
444 vgl_homg_line_2d<Type> const& line2) const
445 {
446 vnl_vector_fixed<Type,3> l1(line1.a(),line1.b(),line1.c());
447 vnl_vector_fixed<Type,3> l2(line2.a(),line2.b(),line2.c());
448 vnl_vector_fixed<Type,3> l = vnl_inverse(dot2(l2)) * l1;
449 return vgl_homg_line_2d<Type>(l[0],l[1],l[2]);
450 }
451
452 template <class Type>
get_constraint_lines_image1(vgl_homg_point_2d<Type> const & p2,vgl_homg_point_2d<Type> const & p3,std::vector<vgl_homg_line_2d<Type>> & lines) const453 void vpgl_tri_focal_tensor<Type>::get_constraint_lines_image1(vgl_homg_point_2d<Type> const& p2,
454 vgl_homg_point_2d<Type> const& p3,
455 std::vector<vgl_homg_line_2d<Type> >& lines) const {
456 // use the same notation as the output of tr_hartley_equation.
457
458 Type x2 = p2.x();
459 Type y2 = p2.y();
460 Type z2 = p2.w();
461
462 Type x3 = p3.x();
463 Type y3 = p3.y();
464 Type z3 = p3.w();
465
466 lines.resize(0);
467
468 /* 0 */
469
470 {
471 Type lx
472 = x2 * y3 * T_(0, 1, 0)
473 - y2 * y3 * T_(0, 0, 0)
474 - x2 * x3 * T_(0, 1, 1)
475 + y2 * x3 * T_(0, 0, 1);
476
477 Type ly
478 = x2 * y3 * T_(1, 1, 0)
479 - y2 * y3 * T_(1, 0, 0)
480 - x2 * x3 * T_(1, 1, 1)
481 + y2 * x3 * T_(1, 0, 1);
482
483 Type lz
484 = x2 * y3 * T_(2, 1, 0)
485 - y2 * y3 * T_(2, 0, 0)
486 - x2 * x3 * T_(2, 1, 1)
487 + y2 * x3 * T_(2, 0, 1);
488
489 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
490 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
491 }
492
493 /* 1 */
494 {
495 Type lx
496 = x2 * z3 * T_(0, 1, 0)
497 - y2 * z3 * T_(0, 0, 0)
498 - x2 * x3 * T_(0, 1, 2)
499 + y2 * x3 * T_(0, 0, 2);
500
501 Type ly
502 = x2 * z3 * T_(1, 1, 0)
503 - y2 * z3 * T_(1, 0, 0)
504 - x2 * x3 * T_(1, 1, 2)
505 + y2 * x3 * T_(1, 0, 2);
506
507 Type lz
508 = x2 * z3 * T_(2, 1, 0)
509 - y2 * z3 * T_(2, 0, 0)
510 - x2 * x3 * T_(2, 1, 2)
511 + y2 * x3 * T_(2, 0, 2);
512
513 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
514 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
515 }
516
517 /* 2 */
518 {
519 Type lx
520 = x2 * z3 * T_(0, 1, 1)
521 - y2 * z3 * T_(0, 0, 1)
522 - x2 * y3 * T_(0, 1, 2)
523 + y2 * y3 * T_(0, 0, 2);
524
525 Type ly
526 = x2 * z3 * T_(1, 1, 1)
527 - y2 * z3 * T_(1, 0, 1)
528 - x2 * y3 * T_(1, 1, 2)
529 + y2 * y3 * T_(1, 0, 2);
530
531 Type lz
532 = x2 * z3 * T_(2, 1, 1)
533 - y2 * z3 * T_(2, 0, 1)
534 - x2 * y3 * T_(2, 1, 2)
535 + y2 * y3 * T_(2, 0, 2);
536 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
537 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
538 }
539
540 /* 3 */
541 {
542 Type lx
543 = x2 * y3 * T_(0, 2, 0)
544 - z2 * y3 * T_(0, 0, 0)
545 - x2 * x3 * T_(0, 2, 1)
546 + z2 * x3 * T_(0, 0, 1);
547
548 Type ly
549 = x2 * y3 * T_(1, 2, 0)
550 - z2 * y3 * T_(1, 0, 0)
551 - x2 * x3 * T_(1, 2, 1)
552 + z2 * x3 * T_(1, 0, 1);
553
554 Type lz
555 = x2 * y3 * T_(2, 2, 0)
556 - z2 * y3 * T_(2, 0, 0)
557 - x2 * x3 * T_(2, 2, 1)
558 + z2 * x3 * T_(2, 0, 1);
559 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
560 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
561 }
562
563 /* 4 */
564 {
565 Type lx
566 = x2 * z3 * T_(0, 2, 0)
567 - z2 * z3 * T_(0, 0, 0)
568 - x2 * x3 * T_(0, 2, 2)
569 + z2 * x3 * T_(0, 0, 2);
570
571 Type ly
572 = x2 * z3 * T_(1, 2, 0)
573 - z2 * z3 * T_(1, 0, 0)
574 - x2 * x3 * T_(1, 2, 2)
575 + z2 * x3 * T_(1, 0, 2);
576
577 Type lz
578 = x2 * z3 * T_(2, 2, 0)
579 - z2 * z3 * T_(2, 0, 0)
580 - x2 * x3 * T_(2, 2, 2)
581 + z2 * x3 * T_(2, 0, 2);
582 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
583 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
584 }
585
586 /* 5 */
587 {
588 Type lx
589 = x2 * z3 * T_(0, 2, 1)
590 - z2 * z3 * T_(0, 0, 1)
591 - x2 * y3 * T_(0, 2, 2)
592 + z2 * y3 * T_(0, 0, 2);
593
594 Type ly
595 = x2 * z3 * T_(1, 2, 1)
596 - z2 * z3 * T_(1, 0, 1)
597 - x2 * y3 * T_(1, 2, 2)
598 + z2 * y3 * T_(1, 0, 2);
599
600 Type lz
601 = x2 * z3 * T_(2, 2, 1)
602 - z2 * z3 * T_(2, 0, 1)
603 - x2 * y3 * T_(2, 2, 2)
604 + z2 * y3 * T_(2, 0, 2);
605 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
606 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
607 }
608
609 /* 6 */
610 {
611 Type lx
612 = y2 * y3 * T_(0, 2, 0)
613 - z2 * y3 * T_(0, 1, 0)
614 - y2 * x3 * T_(0, 2, 1)
615 + z2 * x3 * T_(0, 1, 1);
616
617 Type ly
618 = y2 * y3 * T_(1, 2, 0)
619 - z2 * y3 * T_(1, 1, 0)
620 - y2 * x3 * T_(1, 2, 1)
621 + z2 * x3 * T_(1, 1, 1);
622
623 Type lz
624 = y2 * y3 * T_(2, 2, 0)
625 - z2 * y3 * T_(2, 1, 0)
626 - y2 * x3 * T_(2, 2, 1)
627 + z2 * x3 * T_(2, 1, 1);
628 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
629 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
630 }
631
632 /* 7 */
633 {
634 Type lx
635 = y2 * z3 * T_(0, 2, 0)
636 - z2 * z3 * T_(0, 1, 0)
637 - y2 * x3 * T_(0, 2, 2)
638 + z2 * x3 * T_(0, 1, 2);
639
640 Type ly
641 = y2 * z3 * T_(1, 2, 0)
642 - z2 * z3 * T_(1, 1, 0)
643 - y2 * x3 * T_(1, 2, 2)
644 + z2 * x3 * T_(1, 1, 2);
645
646 Type lz
647 = y2 * z3 * T_(2, 2, 0)
648 - z2 * z3 * T_(2, 1, 0)
649 - y2 * x3 * T_(2, 2, 2)
650 + z2 * x3 * T_(2, 1, 2);
651 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
652 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
653 }
654
655 /* 8 */
656 {
657 Type lx
658 = y2 * z3 * T_(0, 2, 1)
659 - z2 * z3 * T_(0, 1, 1)
660 - y2 * y3 * T_(0, 2, 2)
661 + z2 * y3 * T_(0, 1, 2);
662
663 Type ly
664 = y2 * z3 * T_(1, 2, 1)
665 - z2 * z3 * T_(1, 1, 1)
666 - y2 * y3 * T_(1, 2, 2)
667 + z2 * y3 * T_(1, 1, 2);
668
669 Type lz
670 = y2 * z3 * T_(2, 2, 1)
671 - z2 * z3 * T_(2, 1, 1)
672 - y2 * y3 * T_(2, 2, 2)
673 + z2 * y3 * T_(2, 1, 2);
674 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
675 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
676 }
677 }
678 template <class Type>
get_constraint_lines_image2(vgl_homg_point_2d<Type> const & p1,vgl_homg_point_2d<Type> const & p3,std::vector<vgl_homg_line_2d<Type>> & lines) const679 void vpgl_tri_focal_tensor<Type>::get_constraint_lines_image2(vgl_homg_point_2d<Type> const& p1,
680 vgl_homg_point_2d<Type> const& p3,
681 std::vector<vgl_homg_line_2d<Type> >& lines) const{
682 Type x1 = p1.x();
683 Type y1 = p1.y();
684 Type z1 = p1.w();
685
686 Type x3 = p3.x();
687 Type y3 = p3.y();
688 Type z3 = p3.w();
689
690 lines.resize(0);
691
692 /* 0 */
693 {
694 Type lx
695 = x1 * y3 * T_(0,1,0) - x1 * x3 * T_(0,1,1)
696 + y1 * y3 * T_(1,1,0) - y1 * x3 * T_(1,1,1)
697 + z1 * y3 * T_(2,1,0) - z1 * x3 * T_(2,1,1);
698
699 Type ly
700 = - x1 * y3 * T_(0,0,0) + x1 * x3 * T_(0,0,1)
701 - y1 * y3 * T_(1,0,0) + y1 * x3 * T_(1,0,1)
702 - z1 * y3 * T_(2,0,0) + z1 * x3 * T_(2,0,1);
703
704 Type lz = 0;
705
706 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
707 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
708 }
709
710 /* 1 */
711 {
712 Type lx
713 = x1 * z3 * T_(0,1,0) - x1 * x3 * T_(0,1,2)
714 + y1 * z3 * T_(1,1,0) - y1 * x3 * T_(1,1,2)
715 + z1 * z3 * T_(2,1,0) - z1 * x3 * T_(2,1,2);
716
717 Type ly
718 = - x1 * z3 * T_(0,0,0) + x1 * x3 * T_(0,0,2)
719 - y1 * z3 * T_(1,0,0) + y1 * x3 * T_(1,0,2)
720 - z1 * z3 * T_(2,0,0) + z1 * x3 * T_(2,0,2);
721
722 Type lz = 0;
723 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
724 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
725 }
726
727 /* 2 */
728 {
729 Type lx
730 = x1 * z3 * T_(0,1,1) - x1 * y3 * T_(0,1,2)
731 + y1 * z3 * T_(1,1,1) - y1 * y3 * T_(1,1,2)
732 + z1 * z3 * T_(2,1,1) - z1 * y3 * T_(2,1,2);
733
734 Type ly
735 = -x1 * z3 * T_(0,0,1) + x1 * y3 * T_(0,0,2)
736 - y1 * z3 * T_(1,0,1) + y1 * y3 * T_(1,0,2)
737 - z1 * z3 * T_(2,0,1) + z1 * y3 * T_(2,0,2);
738
739 Type lz = 0;
740
741 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
742 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
743 }
744
745 /* 3 */
746 {
747 Type lx
748 = x1 * y3 * T_(0,2,0) - x1 * x3 * T_(0,2,1)
749 + y1 * y3 * T_(1,2,0) - y1 * x3 * T_(1,2,1)
750 + z1 * y3 * T_(2,2,0) - z1 * x3 * T_(2,2,1);
751
752 Type ly = 0;
753
754 Type lz
755 = -x1 * y3 * T_(0,0,0) + x1 * x3 * T_(0,0,1)
756 - y1 * y3 * T_(1,0,0) + y1 * x3 * T_(1,0,1)
757 - z1 * y3 * T_(2,0,0) + z1 * x3 * T_(2,0,1);
758
759 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
760 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
761 }
762
763 /* 4 */
764 {
765 Type lx
766 = x1 * z3 * T_(0,2,0) - x1 * x3 * T_(0,2,2)
767 + y1 * z3 * T_(1,2,0) - y1 * x3 * T_(1,2,2)
768 + z1 * z3 * T_(2,2,0) - z1 * x3 * T_(2,2,2);
769
770 Type ly = 0;
771
772 Type lz
773 = - x1 * z3 * T_(0,0,0) + x1 * x3 * T_(0,0,2)
774 - y1 * z3 * T_(1,0,0) + y1 * x3 * T_(1,0,2)
775 - z1 * z3 * T_(2,0,0) + z1 * x3 * T_(2,0,2);
776
777 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
778 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
779 }
780
781 /* 5 */
782 {
783 Type lx
784 = x1 * z3 * T_(0,2,1) - x1 * y3 * T_(0,2,2)
785 + y1 * z3 * T_(1,2,1) - y1 * y3 * T_(1,2,2)
786 + z1 * z3 * T_(2,2,1) - z1 * y3 * T_(2,2,2);
787
788 Type ly = 0;
789
790 Type lz
791 = - x1 * z3 * T_(0,0,1) + x1 * y3 * T_(0,0,2)
792 - y1 * z3 * T_(1,0,1) + y1 * y3 * T_(1,0,2)
793 - z1 * z3 * T_(2,0,1) + z1 * y3 * T_(2,0,2);
794
795 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
796 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
797 }
798
799 /* 6 */
800 {
801 Type lx = 0;
802
803 Type ly
804 = x1 * y3 * T_(0,2,0) - x1 * x3 * T_(0,2,1)
805 + y1 * y3 * T_(1,2,0) - y1 * x3 * T_(1,2,1)
806 + z1 * y3 * T_(2,2,0) - z1 * x3 * T_(2,2,1);
807
808 Type lz
809 = -x1 * y3 * T_(0,1,0) + x1 * x3 * T_(0,1,1)
810 - y1 * y3 * T_(1,1,0) + y1 * x3 * T_(1,1,1)
811 - z1 * y3 * T_(2,1,0) + z1 * x3 * T_(2,1,1);
812 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
813 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
814 }
815
816 /* 7 */
817 {
818 Type lx = 0;
819
820 Type ly
821 = x1 * z3 * T_(0,2,0) - x1 * x3 * T_(0,2,2)
822 + y1 * z3 * T_(1,2,0) - y1 * x3 * T_(1,2,2)
823 + z1 * z3 * T_(2,2,0) - z1 * x3 * T_(2,2,2);
824
825 Type lz
826 = - x1 * z3 * T_(0,1,0) + x1 * x3 * T_(0,1,2)
827 - y1 * z3 * T_(1,1,0) + y1 * x3 * T_(1,1,2)
828 - z1 * z3 * T_(2,1,0) + z1 * x3 * T_(2,1,2);
829 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
830 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
831 }
832
833 /* 8 */
834 {
835 Type lx = 0;
836
837 Type ly
838 = x1 * z3 * T_(0,2,1) - x1 * y3 * T_(0,2,2)
839 + y1 * z3 * T_(1,2,1) - y1 * y3 * T_(1,2,2)
840 + z1 * z3 * T_(2,2,1) - z1 * y3 * T_(2,2,2);
841
842 Type lz
843 = - x1 * z3 * T_(0,1,1) + x1 * y3 * T_(0,1,2)
844 - y1 * z3 * T_(1,1,1) + y1 * y3 * T_(1,1,2)
845 - z1 * z3 * T_(2,1,1) + z1 * y3 * T_(2,1,2);
846
847 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
848 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
849 }
850 }
851
852 template <class Type>
get_constraint_lines_image3(vgl_homg_point_2d<Type> const & p1,vgl_homg_point_2d<Type> const & p2,std::vector<vgl_homg_line_2d<Type>> & lines) const853 void vpgl_tri_focal_tensor<Type>::get_constraint_lines_image3(vgl_homg_point_2d<Type> const& p1,
854 vgl_homg_point_2d<Type> const& p2,
855 std::vector<vgl_homg_line_2d<Type> >& lines) const
856 {
857 // use the same notation as the output of tr_hartley_equation.
858 Type x1 = p1.x();
859 Type y1 = p1.y();
860 Type z1 = p1.w();
861
862 Type x2 = p2.x();
863 Type y2 = p2.y();
864 Type z2 = p2.w();
865
866 lines.clear();
867
868 /* 0 */
869 {
870 Type lx =
871 -x1 * x2 * T_(0,1,1) + x1 * y2 * T_(0,0,1)
872 -y1 * x2 * T_(1,1,1) + y1 * y2 * T_(1,0,1)
873 -z1 * x2 * T_(2,1,1) + z1 * y2 * T_(2,0,1);
874
875 Type ly =
876 x1 * x2 * T_(0,1,0) - x1 * y2 * T_(0,0,0) +
877 y1 * x2 * T_(1,1,0) - y1 * y2 * T_(1,0,0) +
878 z1 * x2 * T_(2,1,0) - z1 * y2 * T_(2,0,0);
879
880 Type lz = 0;
881 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
882 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
883 }
884
885 /* 1 */
886 {
887 Type lx =
888 -x1 * x2 * T_(0,1,2) + x1 * y2 * T_(0,0,2)
889 - y1 * x2 * T_(1,1,2) + y1 * y2 * T_(1,0,2)
890 - z1 * x2 * T_(2,1,2) + z1 * y2 * T_(2,0,2);
891
892 Type ly = 0;
893
894 Type lz
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 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
900 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
901 }
902
903 /* 2 */
904 {
905 Type lx =
906 0;
907
908 Type ly =
909 -x1 * x2 * T_(0,1,2) + x1 * y2 * T_(0,0,2)
910 -y1 * x2 * T_(1,1,2) + y1 * y2 * T_(1,0,2)
911 -z1 * x2 * T_(2,1,2) + z1 * y2 * T_(2,0,2);
912
913 Type lz =
914 x1 * x2 * T_(0,1,1) - x1 * y2 * T_(0,0,1)
915 + y1 * x2 * T_(1,1,1) - y1 * y2 * T_(1,0,1)
916 + z1 * x2 * T_(2,1,1) - z1 * y2 * T_(2,0,1);
917 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
918 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
919 }
920
921 /* 3 */
922 {
923 Type lx =
924 -x1 * x2 * T_(0,2,1) + x1 * z2 * T_(0,0,1)
925 - y1 * x2 * T_(1,2,1) + y1 * z2 * T_(1,0,1)
926 - z1 * x2 * T_(2,2,1) + z1 * z2 * T_(2,0,1);
927
928 Type ly =
929 x1 * x2 * T_(0,2,0) - x1 * z2 * T_(0,0,0)
930 + y1 * x2 * T_(1,2,0) - y1 * z2 * T_(1,0,0)
931 + z1 * x2 * T_(2,2,0) - z1 * z2 * T_(2,0,0);
932
933 Type lz = 0;
934 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
935 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
936 }
937
938 /* 4 */
939 {
940 Type lx =
941 -x1 * x2 * T_(0,2,2) + x1 * z2 * T_(0,0,2)
942 -y1 * x2 * T_(1,2,2) + y1 * z2 * T_(1,0,2)
943 -z1 * x2 * T_(2,2,2) + z1 * z2 * T_(2,0,2);
944
945 Type ly = 0;
946
947 Type lz =
948 x1 * x2 * T_(0,2,0) - x1 * z2 * T_(0,0,0) +
949 y1 * x2 * T_(1,2,0) - y1 * z2 * T_(1,0,0) +
950 z1 * x2 * T_(2,2,0) - z1 * z2 * T_(2,0,0);
951 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
952 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
953 }
954
955 /* 5 */
956 {
957 Type lx = 0;
958
959 Type ly =
960 -x1 * x2 * T_(0,2,2) + x1 * z2 * T_(0,0,2)
961 -y1 * x2 * T_(1,2,2) + y1 * z2 * T_(1,0,2)
962 -z1 * x2 * T_(2,2,2) + z1 * z2 * T_(2,0,2);
963
964 Type lz
965 = x1 * x2 * T_(0,2,1) - x1 * z2 * T_(0,0,1)
966 + y1 * x2 * T_(1,2,1) - y1 * z2 * T_(1,0,1)
967 + z1 * x2 * T_(2,2,1) - z1 * z2 * T_(2,0,1);
968 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
969 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
970 }
971
972 /* 6 */
973 {
974 Type lx
975 = - x1 * y2 * T_(0,2,1) + x1 * z2 * T_(0,1,1)
976 - y1 * y2 * T_(1,2,1) + y1 * z2 * T_(1,1,1)
977 - z1 * y2 * T_(2,2,1) + z1 * z2 * T_(2,1,1);
978
979 Type ly
980 = x1 * y2 * T_(0,2,0) - x1 * z2 * T_(0,1,0)
981 + y1 * y2 * T_(1,2,0) - y1 * z2 * T_(1,1,0)
982 + z1 * y2 * T_(2,2,0) - z1 * z2 * T_(2,1,0);
983
984 Type lz = 0;
985 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
986 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
987 }
988
989 /* 7 */
990 {
991 Type lx
992 = -x1 * y2 * T_(0,2,2) + x1 * z2 * T_(0,1,2)
993 - y1 * y2 * T_(1,2,2) + y1 * z2 * T_(1,1,2)
994 - z1 * y2 * T_(2,2,2) + z1 * z2 * T_(2,1,2);
995
996 Type ly = 0;
997
998 Type lz
999 = x1 * y2 * T_(0,2,0) - x1 * z2 * T_(0,1,0)
1000 + y1 * y2 * T_(1,2,0) - y1 * z2 * T_(1,1,0)
1001 + z1 * y2 * T_(2,2,0) - z1 * z2 * T_(2,1,0);
1002
1003 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
1004 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
1005 }
1006
1007 /* 8 */
1008 {
1009 Type lx = 0;
1010
1011 Type ly
1012 = -x1 * y2 * T_(0,2,2) + x1 * z2 * T_(0,1,2)
1013 - y1 * y2 * T_(1,2,2) + y1 * z2 * T_(1,1,2)
1014 - z1 * y2 * T_(2,2,2) + z1 * z2 * T_(2,1,2);
1015
1016 Type lz
1017 = x1 * y2 * T_(0,2,1) - x1 * z2 * T_(0,1,1)
1018 + y1 * y2 * T_(1,2,1) - y1 * z2 * T_(1,1,1)
1019 + z1 * y2 * T_(2,2,1) - z1 * z2 * T_(2,1,1);
1020 if(!(lx == Type(0)&&ly== Type(0)&&lz == Type(0)))
1021 lines.push_back(vgl_homg_line_2d<Type>(lx, ly, lz));
1022 }
1023 }
1024
1025 template <class Type>
image1_transfer(vgl_homg_point_2d<Type> const & point2,vgl_homg_point_2d<Type> const & point3) const1026 vgl_homg_point_2d<Type> vpgl_tri_focal_tensor<Type>::image1_transfer(vgl_homg_point_2d<Type> const& point2,
1027 vgl_homg_point_2d<Type> const& point3) const{
1028 std::vector<vgl_homg_line_2d<Type> > constraint_lines(9);
1029 get_constraint_lines_image1(point2, point3, constraint_lines);
1030 return vgl_homg_operators_2d<Type>::lines_to_point(constraint_lines);
1031 }
1032 template <class Type>
image2_transfer(vgl_homg_point_2d<Type> const & point1,vgl_homg_point_2d<Type> const & point3) const1033 vgl_homg_point_2d<Type> vpgl_tri_focal_tensor<Type>::image2_transfer(vgl_homg_point_2d<Type> const& point1,
1034 vgl_homg_point_2d<Type> const& point3) const{
1035 std::vector<vgl_homg_line_2d<Type> > constraint_lines(9);
1036 get_constraint_lines_image2(point1, point3, constraint_lines);
1037 return vgl_homg_operators_2d<Type>::lines_to_point(constraint_lines);
1038 }
1039 template <class Type>
image3_transfer(vgl_homg_point_2d<Type> const & point1,vgl_homg_point_2d<Type> const & point2) const1040 vgl_homg_point_2d<Type> vpgl_tri_focal_tensor<Type>::image3_transfer(vgl_homg_point_2d<Type> const& point1,
1041 vgl_homg_point_2d<Type> const& point2) const{
1042 std::vector<vgl_homg_line_2d<Type> > constraint_lines(9);
1043 get_constraint_lines_image3(point1, point2, constraint_lines);
1044 return vgl_homg_operators_2d<Type>::lines_to_point(constraint_lines);
1045 }
1046 template <class Type>
compute_epipoles()1047 bool vpgl_tri_focal_tensor<Type>::compute_epipoles(){
1048 if(epipoles_valid_)
1049 return true;
1050
1051 Type tol = vgl_tolerance<Type>::position;
1052 vnl_matrix_fixed<Type,3,3> T1 = dot1(vnl_vector_fixed<Type,3>(1,0,0).as_ref());
1053 vnl_matrix_fixed<Type,3,3> T2 = dot1(vnl_vector_fixed<Type,3>(0,1,0).as_ref());
1054 vnl_matrix_fixed<Type,3,3> T3 = dot1(vnl_vector_fixed<Type,3>(0,0,1).as_ref());
1055
1056 vnl_svd<Type> svd1(T1.as_ref());
1057
1058 vnl_vector_fixed<Type,3> u1 = svd1.nullvector();
1059 vnl_vector_fixed<Type,3> v1 = svd1.left_nullvector();
1060
1061 vnl_svd<Type> svd2(T2.as_ref());
1062 vnl_vector_fixed<Type,3> u2 = svd2.nullvector();
1063 vnl_vector_fixed<Type,3> v2 = svd2.left_nullvector();
1064
1065 vnl_svd<Type> svd3(T3.as_ref());
1066 vnl_vector_fixed<Type,3> u3 = svd3.nullvector();
1067 vnl_vector_fixed<Type,3> v3 = svd3.left_nullvector();
1068
1069 vnl_matrix_fixed<Type,3,3> V;
1070 V(0,0) = v1[0]; V(0,1) = v2[0]; V(0,2) = v3[0];
1071 V(1,0) = v1[1]; V(1,1) = v2[1]; V(1,2) = v3[1];
1072 V(2,0) = v1[2]; V(2,1) = v2[2]; V(2,2) = v3[2];
1073
1074 vnl_svd<Type> svdv(V.as_ref());
1075 vnl_vector<Type> left_nvvec = svdv.left_nullvector();
1076 e12_.set(left_nvvec[0],left_nvvec[1],left_nvvec[2]);
1077
1078 if(fabs(e12_.x())<tol && fabs(e12_.y())<tol && fabs(e12_.w())<tol){
1079 std::cout << "null e12 - fatal" << std::endl;
1080 return false;
1081 }
1082
1083 vnl_matrix_fixed<Type,3,3> U;
1084 U(0,0) = u1[0]; U(0,1) = u2[0]; U(0,2) = u3[0];
1085 U(1,0) = u1[1]; U(1,1) = u2[1]; U(1,2) = u3[1];
1086 U(2,0) = u1[2]; U(2,1) = u2[2]; U(2,2) = u3[2];
1087
1088 vnl_svd<Type> svdu(U.as_ref());
1089 vnl_vector<Type> left_nuvec = svdu.left_nullvector();
1090 e13_.set(left_nuvec[0], left_nuvec[1], left_nuvec[2]);
1091 if(fabs(e13_.x())<tol && fabs(e13_.y())<tol && fabs(e13_.w())<tol){
1092 std::cout << "null e13 - fatal" << std::endl;
1093 return false;
1094 }
1095 epipoles_valid_ = true;
1096 return true;
1097 }
1098
1099
1100 template <class Type>
compute_f_matrices()1101 bool vpgl_tri_focal_tensor<Type>::compute_f_matrices(){
1102 if(f_matrices_1213_valid_)
1103 return true;
1104 if(!epipoles_valid_) compute_epipoles();
1105 if(!epipoles_valid_){
1106 std::cout << "Can't compute f matrices - epipoles not valid" << std::endl;
1107 return false;
1108 }
1109
1110 vnl_vector_fixed<Type,3> ev12(e12_.x(), e12_.y(), e12_.w()), ev13(e13_.x(), e13_.y(), e13_.w());
1111
1112 // bit of a pain since cross product matrix isn't defined for a generic type
1113 // so must convert to double
1114 vnl_vector_fixed<double,3> ev12_d(ev12[0], ev12[1], ev12[2]), ev13_d(ev13[0], ev13[1], ev13[2]);
1115
1116 vnl_matrix_fixed<double, 3, 3> e12x = vnl_cross_product_matrix(ev12_d);
1117 vnl_matrix_fixed<double, 3, 3> e13x = vnl_cross_product_matrix(ev13_d);
1118
1119 vnl_matrix_fixed<Type, 3,3> temp12, temp13, F12_t, F13_t;
1120 vnl_matrix_fixed<double, 3,3> temp12_d, temp13_d, cp12, cp13;
1121 temp12 = dot3(ev13);
1122 temp13 = dot2(ev12);
1123 //transpose required
1124 for(size_t r = 0; r<3; ++r)
1125 for(size_t c = 0; c<3; ++c){
1126 temp12_d[r][c] = temp12[c][r];//transpose
1127 temp13_d[r][c] = temp13[c][r];//transpose
1128 }
1129 cp12 = e12x*temp12_d; cp13 = e13x*temp13_d;
1130 for(size_t r = 0; r<3; ++r)
1131 for(size_t c = 0; c<3; ++c){
1132 F12_t[r][c] = cp12[r][c];
1133 F13_t[r][c] = cp13[r][c];
1134 }
1135 f12_.set_matrix(F12_t);
1136 f13_.set_matrix(F13_t);
1137 f_matrices_1213_valid_ = true;
1138 return true;
1139 }
1140 template <class Type>
compute_f_matrix_23()1141 bool vpgl_tri_focal_tensor<Type>::compute_f_matrix_23(){
1142 if(f_matrix_23_valid_)
1143 return true;
1144 if(!cameras_valid_)
1145 return false;
1146 vpgl_fundamental_matrix<Type> f23(c2_, c3_);
1147 f23_ = f23;
1148 f_matrix_23_valid_ = true;
1149 return true;
1150
1151 }
1152 //-----------------------------------------------------------------------------
1153 //: Read from ASCII std::istream
1154 template <class Type>
operator >>(std::istream & s,vpgl_tri_focal_tensor<Type> & T)1155 std::istream& operator>>(std::istream& s, vpgl_tri_focal_tensor<Type>& T)
1156 {
1157 for (int i = 0; i < 3; ++i)
1158 for (int j = 0; j < 3; ++j)
1159 for (int k = 0; k < 3; ++k)
1160 s >> T(i,j,k);
1161 return s;
1162 }
1163
1164 //-----------------------------------------------------------------------------
1165 //: Print in ASCII to std::ostream
1166 template <class Type>
operator <<(std::ostream & s,const vpgl_tri_focal_tensor<Type> & T)1167 std::ostream& operator<<(std::ostream& s, const vpgl_tri_focal_tensor<Type>& T)
1168 {
1169 for (int i = 0; i < 3; ++i) {
1170 for (int j = 0; j < 3; ++j) {
1171 for (int k = 0; k < 3; ++k)
1172 vul_printf(s, "%20.16e ", T(i,j,k));
1173 s << std::endl;
1174 }
1175 s << std::endl;
1176 }
1177 return s;
1178 }
1179 template <class Type>
within_scale(const vpgl_tri_focal_tensor<Type> & T1,const vpgl_tri_focal_tensor<Type> & T2)1180 bool within_scale(const vpgl_tri_focal_tensor<Type>& T1, const vpgl_tri_focal_tensor<Type>& T2){
1181 Type max_abs = Type(0);
1182 size_t max_i = 0, max_j = 0, max_k =0;
1183 for (int i = 0; i < 3; ++i)
1184 for (int j = 0; j < 3; ++j)
1185 for (int k = 0; k < 3; ++k)
1186 if(fabs(T1(i,j,k)) > max_abs){
1187 max_abs = fabs(T1(i,j,k));
1188 max_i = i; max_j =j; max_k = k;
1189 }
1190 Type scale1 = Type(1)/max_abs;
1191 Type scale2 = Type(1)/fabs(T2(max_i, max_j, max_k));
1192 Type rms = Type(0);
1193 for (size_t i = 0; i < 3; ++i)
1194 for (size_t j = 0; j < 3; ++j)
1195 for (size_t k = 0; k < 3; ++k) {
1196 Type d = T1(i,j,k)*scale1 - T2(i,j,k) * scale2;
1197 rms += d*d;
1198 }
1199 rms /= Type(27);
1200
1201 if (rms > Type(1e-15))
1202 return false;
1203
1204 return true;
1205 }
1206 // Code for easy instantiation.
1207 #undef vpgl_TRI_FOCAL_TENSOR_INSTANTIATE
1208 #define vpgl_TRI_FOCAL_TENSOR_INSTANTIATE(Type) \
1209 template class vpgl_tri_focal_tensor<Type >; \
1210 template std::ostream& operator<<(std::ostream&, const vpgl_tri_focal_tensor<Type>&); \
1211 template std::istream& operator>>(std::istream&, vpgl_tri_focal_tensor<Type>&); \
1212 template bool within_scale(const vpgl_tri_focal_tensor<Type>&, const vpgl_tri_focal_tensor<Type>&);
1213 #endif // vpgl_tri_focal_tensor_hxx_
1214