vgl_homg_operators_3d.hxx
Go to the documentation of this file.
1 // This is core/vgl/algo/vgl_homg_operators_3d.hxx
2 #ifndef vgl_homg_operators_3d_hxx_
3 #define vgl_homg_operators_3d_hxx_
4 //:
5 // \file
6 
7 #include <iostream>
8 #include <vector>
9 #include <cmath>
10 #include "vgl_homg_operators_3d.h"
11 //
12 #ifdef _MSC_VER
13 # include <vcl_msvc_warnings.h>
14 #endif
15 #include <cassert>
16 
17 #include <vnl/vnl_vector_fixed.h>
18 #include <vnl/vnl_matrix_fixed.h>
19 #include <vnl/vnl_matrix.h>
20 #include <vnl/algo/vnl_svd.h>
21 
22 #include <vgl/vgl_homg_point_3d.h>
23 #include <vgl/vgl_homg_plane_3d.h>
24 #include <vgl/vgl_homg_line_2d.h>
25 #include <vgl/vgl_homg_point_2d.h>
26 
27 //-----------------------------------------------------------------------------
28 
29 //: Return the angle between the (oriented) lines (in radians)
30 //
31 template <class Type>
33  const vgl_homg_line_3d& l2)
34 {
35  vgl_homg_point_3d<Type> const& dir1 = l1.point_infinite();
36  vgl_homg_point_3d<Type> const& dir2 = l2.point_infinite();
37  double n = dir1.x()*dir1.x()+dir1.y()*dir1.y()+dir1.z()*dir1.z();
38  n *= dir2.x()*dir2.x()+dir2.y()*dir2.y()+dir2.z()*dir2.z();
39  // dot product of unit direction vectors:
40  n = (dir1.x()*dir2.x()+dir1.y()*dir2.y()+dir1.z()*dir2.z())/std::sqrt(n);
41  return std::acos(n);
42 }
43 
44 template <class Type>
46  const vgl_homg_point_3d<Type>& point2)
47 {
48  Type mag = 0;
49  Type d;
50 
51  d = point1.x()/point1.w() - point2.x()/point2.w();
52  mag += d*d;
53 
54  d = point1.y()/point1.w() - point2.y()/point2.w();
55  mag += d*d;
56 
57  d = point1.z()/point1.w() - point2.z()/point2.w();
58  mag += d*d;
59 
60  return mag;
61 }
62 
63 //-----------------------------------------------------------------------------
64 
65 //: Return the Euclidean distance between the points
66 //
67 template <class Type>
69  const vgl_homg_point_3d<Type>&point2)
70 {
71  return std::sqrt(vgl_homg_operators_3d<Type>::distance_squared(point1,point2));
72 }
73 
74 //-----------------------------------------------------------------------------
75 
76 //: Return the intersection point of the line and plane
77 //
78 template <class Type>
80  const vgl_homg_line_3d &line,
81  const vgl_homg_plane_3d<Type>& plane)
82 {
83  // use P.(S + lambda D) = 0 to find lambda, and hence a point on the plane.
84 
85  const vnl_vector_fixed<Type,4> x1 = get_vector(line.point_finite());
86  const vnl_vector_fixed<Type,4> x2 = get_vector(line.point_infinite());
87  const vnl_vector_fixed<Type,4> p = get_vector(plane);
88 
89  // FIXME: this works for double and smaller, but not complex. it might happen.
90 
91  double numerator = -dot_product(x1, p); // find out if dot_product is ok
92  double denominator = dot_product(x2, p);
93 
94  // Scale for conditioning
95  double scale;
96  if ( numerator + denominator != 0 )
97  scale = 1.0/(numerator + denominator);
98  else
99  scale = 1.0/numerator;
100  numerator *= scale;
101  denominator *= scale;
102 
103  vnl_vector_fixed<Type,4> r = x1 * Type(denominator) + x2 * Type(numerator);
104  return vgl_homg_point_3d<Type>(r[0], r[1], r[2], r[3]);
105 }
106 
107 //-----------------------------------------------------------------------------
108 //
109 // Compute the intersection point of the lines, or the mid-point
110 // of the common perpendicular if the lines are skew
111 //
112 #if 0 // linker error better than run-time error.
113 template <class Type>
115 vgl_homg_operators_3d<Type>::lines_to_point(const vgl_homg_line_3d& ,
116  const vgl_homg_line_3d& )
117 {
118  std::cerr << "Warning: vgl_homg_operators_3d<Type>::lines_to_point() not yet implemented\n";
119  return vgl_homg_point_3d<Type>();
120 }
121 #endif
122 
123 //-----------------------------------------------------------------------------
124 //
125 // - Compute the best fit intersection point of the lines
126 //
127 #if 0 // linker error better than run-time error.
128 template <class Type>
130 vgl_homg_operators_3d<Type>::lines_to_point(const std::vector<vgl_homg_line_3d >& )
131 {
132  std::cerr << "Warning: vgl_homg_operators_3d<Type>::lines_to_point() not yet implemented\n";
133  return vgl_homg_point_3d<Type>();
134 }
135 #endif
136 
137 //-----------------------------------------------------------------------------
138 
139 //: Return the squared perpendicular distance between the line and point
140 //
141 template <class Type>
142 double
144  const vgl_homg_point_3d<Type>& p)
145 {
148 }
149 
150 //-----------------------------------------------------------------------------
151 
152 //: Return the line which is perpendicular to l and passes through p.
153 //
154 template <class Type>
157  const vgl_homg_point_3d<Type>& p)
158 {
159  if (p.ideal())
160  {
161  // this only works if p is not on l; and since the implementation below
162  // only works when p is a finite point, use this one when p is at infinity.
164  if (get_vector(p)==get_vector(perp_dirn))
165  std::cerr << "Warning: perp_line_through_point() makes no sense if the point is the infinity point of the line\n";
166  return vgl_homg_line_3d(p, perp_dirn);
167  }
168  else // by Brendan McCane
169  {
170  // OK this is a better version because it works even if the point
171  // is on the line. It does this simply by creating a direction
172  // perpendicular to the current line and then creating a new
173  // line with the passed in pt and the perpendicular direction.
174  vgl_homg_point_3d<Type> dirn = l.point_infinite();
175  vgl_homg_point_3d<Type> perp_dirn(Type(1)/dirn.x(), (-dirn.z()-1)/dirn.y(), Type(1), Type(0));
176  // should put an assert in here making sure that the dot product
177  // is zero (or close to), but I don't know how to do that with
178  // templates eg complex<1e-8 probably doesn't make sense.
179  return vgl_homg_line_3d(p, perp_dirn);
180  }
181 }
182 
183 
184 //-----------------------------------------------------------------------------
185 
186 //: Compute the perpendicular projection point of p onto l.
187 //
188 template <class Type>
191  const vgl_homg_point_3d<Type>& p)
192 {
193  vgl_homg_point_3d<Type> const& q = l.point_finite();
194  Type a[3] = { q.x()/q.w(), q.y()/q.w(), q.z()/q.w() };
195  Type b[3] = { p.x()/p.w()-a[0], p.y()/p.w()-a[1], p.z()/p.w()-a[2] };
196 
197  vgl_homg_point_3d<Type> const& i = l.point_infinite();
198  Type dp = i.x()*i.x()+i.y()*i.y()+i.z()*i.z();
199  dp = (b[0]*i.x() + b[1]*i.y() + b[2]*i.z()) / dp;
200 
201  return vgl_homg_point_3d<Type>(a[0]+dp*i.x(), a[1]+dp*i.y(), a[2]+dp*i.z());
202 }
203 
204 
205 //-----------------------------------------------------------------------------
206 
207 //: Dihedral angle (of intersection) of 2 planes
208 //
209 template <class Type>
211  const vgl_homg_plane_3d<Type>& plane2)
212 {
213  double cosang = dot_product(plane1.normal(), plane2.normal());
214 
215  return (Type)std::acos(cosang);
216 }
217 
218 //-----------------------------------------------------------------------------
219 
220 //: Return the intersection line of the planes
221 //
222 template <class Type>
225  const vgl_homg_plane_3d<Type>& plane2)
226 {
227  // TODO need equivalent of get_vector
228  vnl_matrix_fixed<Type,2,4> M;
229  M.set_row(0, get_vector(plane1));
230  M.set_row(1, get_vector(plane2));
231  vnl_svd<Type> svd(M.as_ref());
232  vnl_matrix<Type> ns = svd.nullspace(2);
233  vnl_vector_fixed<Type,4> r = ns.get_column(0);
234  vgl_homg_point_3d<Type> p1(r[0], r[1], r[2], r[3]);
235  r = ns.get_column(1);
236  vgl_homg_point_3d<Type> p2(r[0], r[1], r[2], r[3]);
237  return vgl_homg_line_3d(p1, p2);
238 }
239 
240 
241 //-----------------------------------------------------------------------------
242 //
243 // - Compute the best-fit intersection line of the planes
244 //
245 #if 0 // linker error better than run-time error.
246 template <class Type>
249 {
250  std::cerr << "Warning: vgl_homg_operators_3d<Type>::planes_to_line() not yet implemented\n";
251  return vgl_homg_line_3d<Type>();
252 }
253 #endif
254 
255 
256 //-----------------------------------------------------------------------------
257 //
258 // - Return the line through the points
259 //
260 #if 0 // linker error better than run-time error.
261 template <class Type>
265 {
266  std::cerr << "Warning: vgl_homg_operators_3d<Type>::points_to_line() not yet implemented\n";
267  return vgl_homg_line_3d<Type>();
268 }
269 #endif
270 
271 //-----------------------------------------------------------------------------
272 //
273 // - Compute the best-fit line through the points
274 //
275 #if 0 // linker error better than run-time error.
276 template <class Type>
279 {
280  std::cerr << "Warning: vgl_homg_operators_3d<Type>::points_to_line() not yet implemented\n";
281  return vgl_homg_line_3d<Type>();
282 }
283 #endif
284 
285 //-----------------------------------------------------------------------------
286 //
287 // - Return the plane through the points
288 //
289 #if 0 // linker error better than run-time error.
290 template <class Type>
295 {
296  std::cerr << "Warning: vgl_homg_operators_3d<Type>::points_to_plane() not yet implemented\n";
297  return vgl_homg_plane_3d<Type>();
298 }
299 #endif
300 
301 
302 //-----------------------------------------------------------------------------
303 //
304 // - Compute the best-fit plane through the points
305 //
306 #if 0 // linker error better than run-time error.
307 template <class Type>
310 {
311  std::cerr << "Warning: vgl_homg_operators_3d<Type>::points_to_plane() not yet implemented\n";
312  return vgl_homg_plane_3d<Type>();
313 }
314 #endif
315 
316 //-----------------------------------------------------------------------------
317 
318 //: Compute best-fit intersection of planes in a point.
319 //
320 template <class Type>
323  const vgl_homg_plane_3d<Type>& p2,
324  const vgl_homg_plane_3d<Type>& p3)
325 {
326  return vgl_homg_point_3d<Type>(p1, p2, p3);
327 }
328 
329 template <class Type>
332 {
333  int n = planes.size();
334  assert(n >= 3);
335  vnl_matrix<Type> A(n, 4);
336 
337  for (int i=0; i<n; ++i) {
338  A(i,0) = planes[i].nx();
339  A(i,1) = planes[i].ny();
340  A(i,2) = planes[i].nz();
341  A(i,3) = planes[i].d();
342  }
343 
344  vnl_svd<Type> svd(A);
345  return vgl_homg_point_3d<Type>(svd.nullvector().begin());
346 }
347 
348 
349 template <class Type>
351 {
352  return vnl_vector_fixed<Type,4>(p.x(),p.y(),p.z(),p.w());
353 }
354 
355 template <class Type>
357 {
358  return vnl_vector_fixed<Type,4>(p.nx(),p.ny(),p.nz(),p.d());
359 }
360 
361 template <class Type>
363 {
364  double norm = a.x()*a.x() + a.y()*a.y() + a.z()*a.z() + a.w()*a.w();
365 
366  if (norm == 0.0) {
367  std::cerr << "vgl_homg_operators_3d<Type>::unitize() -- Zero length vector\n";
368  return;
369  }
370  norm = 1.0/std::sqrt(norm);
371  a.set(Type(a.x()*norm), Type(a.y()*norm), Type(a.z()*norm), Type(a.w()*norm));
372 }
373 
374 //: Return the midpoint of the line joining two homogeneous points
375 // When one of the points is at infinity, that point is returned.
376 // When both points are at infinity, the invalid point (0,0,0,0) is returned.
377 template <class Type>
380  const vgl_homg_point_3d<Type>& p2)
381 {
382  Type x = p1.x() * p2.w() + p2.x() * p1.w();
383  Type y = p1.y() * p2.w() + p2.y() * p1.w();
384  Type z = p1.z() * p2.w() + p2.z() * p1.w();
385  Type w = p1.w() * p2.w() + p2.w() * p1.w();
386 
387  return vgl_homg_point_3d<Type>(x,y,z,w);
388 }
389 
390 //: Intersect a set of 3D planes to find the least-square point of intersection.
391 // This finds the point $\bf x$ that minimizes $\|\tt L \bf x\|$, where $\tt L$
392 // is the matrix whose rows are the planes. The implementation uses vnl_svd
393 // to accumulate and compute the nullspace of $\tt L^\top \tt L$.
394 template <class Type>
397 {
398  assert(planes.size() >= 3);
399 
400  vnl_vector_fixed<Type,4> mov = most_orthogonal_vector_svd(planes);
401  return vgl_homg_point_3d<Type>(mov[0], mov[1], mov[2], mov[3]);
402 }
403 
404 template <class Type>
406  const vgl_homg_point_3d<Type>& b,
407  const vgl_homg_point_3d<Type>& c,
408  const vgl_homg_point_3d<Type>& d)
409 {
410  double x1 = a.x(), y1 = a.y(), z1 = a.z(), w1 = a.w();
411  double x2 = b.x(), y2 = b.y(), z2 = b.z(), w2 = b.w();
412  double x3 = c.x(), y3 = c.y(), z3 = c.z(), w3 = c.w();
413  double x4 = d.x(), y4 = d.y(), z4 = d.z(), w4 = d.w();
414  double x = x1 - x2; if (x<0) x = -x; // assuming a != b ;-)
415  double y = y1 - y2; if (y<0) y = -y;
416  double z = z1 - z2; if (z<0) z = -z;
417  double n = (x>y && x>z) ? (x1*w3-x3*w1)*(x2*w4-x4*w2) :
418  (y>z) ? (y1*w3-y3*w1)*(y2*w4-y4*w2) :
419  (z1*w3-z3*w1)*(z2*w4-z4*w2);
420  double m = (x>y && x>z) ? (x1*w4-x4*w1)*(x2*w3-x3*w2) :
421  (y>z) ? (y1*w4-y4*w1)*(y2*w3-y3*w2) :
422  (z1*w4-z4*w1)*(z2*w3-z3*w2);
423  if (n == 0 && m == 0)
424  std::cerr << "cross ratio not defined: three of the given points coincide\n";
425  return n/m;
426 }
427 
428 template <class Type>
430  const vgl_homg_plane_3d<Type>& b,
431  const vgl_homg_plane_3d<Type>& c,
432  const vgl_homg_plane_3d<Type>& d)
433 {
434  double x1 = a.a(), y1 = a.b(), z1 = a.c(), w1 = a.d();
435  double x2 = b.a(), y2 = b.b(), z2 = b.c(), w2 = b.d();
436  double x3 = c.a(), y3 = c.b(), z3 = c.c(), w3 = c.d();
437  double x4 = d.a(), y4 = d.b(), z4 = d.c(), w4 = d.d();
438  double x = x1 - x2; if (x<0) x = -x; // assuming a != b ;-)
439  double y = y1 - y2; if (y<0) y = -y;
440  double z = z1 - z2; if (z<0) z = -z;
441  double n = (x>y && x>z) ? (x1*w3-x3*w1)*(x2*w4-x4*w2) :
442  (y>z) ? (y1*w3-y3*w1)*(y2*w4-y4*w2) :
443  (z1*w3-z3*w1)*(z2*w4-z4*w2);
444  double m = (x>y && x>z) ? (x1*w4-x4*w1)*(x2*w3-x3*w2) :
445  (y>z) ? (y1*w4-y4*w1)*(y2*w3-y3*w2) :
446  (z1*w4-z4*w1)*(z2*w3-z3*w2);
447  if (n == 0 && m == 0)
448  std::cerr << "cross ratio not defined: three of the given planes coincide\n";
449  return n/m;
450 }
451 
452 //: Conjugate point of three given collinear points.
453 // If cross ratio cr is given (default: -1), the generalized conjugate point
454 // returned is such that ((x1,x2;x3,answer)) = cr.
455 template <class T>
458  const vgl_homg_point_3d<T>& b,
459  const vgl_homg_point_3d<T>& c,
460  double cr)
461 // Default for cr is -1.
462 {
463  T x1 = a.x(), y1 = a.y(), z1 = a.z(), w1 = a.w();
464  T x2 = b.x(), y2 = b.y(), z2 = b.z(), w2 = b.w();
465  T x3 = c.x(), y3 = c.y(), z3 = c.z(), w3 = c.w();
466  T kx = x1*w3 - x3*w1, mx = x2*w3 - x3*w2, nx = T(kx*w2-cr*mx*w1);
467  T ky = y1*w3 - y3*w1, my = y2*w3 - y3*w2, ny = T(ky*w2-cr*my*w1);
468  T kz = z1*w3 - z3*w1, mz = z2*w3 - z3*w2, nz = T(kz*w2-cr*mz*w1);
469  return vgl_homg_point_3d<T>(T(x2*kx-cr*x1*mx)*ny*nz,T(y2*ky-cr*y1*my)*nx*nz,T(z2*kz-cr*z1*mz)*nx*ny,nx*ny*nz);
470 }
471 
472 template <class T>
473 double
475  const vgl_homg_plane_3d<T>& plane)
476 {
477  if ((plane.a()==0 && plane.b()== 0 && plane.c()== 0) || point.w()==0) {
478  std::cerr << "vgl_homg_operators_3d<T>::perp_dist_squared() -- plane or point at infinity\n";
479  return 1e38;
480  }
481 
482 #define dot(p,q) ((p).a()*(q).x()+(p).b()*(q).y()+(p).c()*(q).z()+(p).d()*(q).w())
483  double numerator = dot(plane,point) / point.w();
484 #undef dot
485  if (numerator == 0) return 0.0;
486  double denominator = plane.a()*plane.a() + plane.b()*plane.b() + plane.c()*plane.c();
487  return numerator * numerator / denominator;
488 }
489 
490 template <class T>
491 vnl_vector_fixed<T,4>
493 {
494  vnl_matrix<T> D(planes.size(), 4);
495 
496  typename std::vector<vgl_homg_plane_3d<T> >::const_iterator i = planes.begin();
497  for (unsigned j = 0; i != planes.end(); ++i,++j)
498  D.set_row(j, get_vector(*i).as_ref());
499 
500  vnl_svd<T> svd(D);
501  return svd.nullvector();
502 }
503 
504 //: Homographic transformation of a 3D point through a 4x4 projective transformation matrix
505 template <class T>
506 vgl_homg_point_3d<T> operator*(vnl_matrix_fixed<T,4,4> const& m,
507  vgl_homg_point_3d<T> const& p)
508 {
509  return vgl_homg_point_3d<T>(m(0,0)*p.x()+m(0,1)*p.y()+m(0,2)*p.z()+m(0,3)*p.w(),
510  m(1,0)*p.x()+m(1,1)*p.y()+m(1,2)*p.z()+m(1,3)*p.w(),
511  m(2,0)*p.x()+m(2,1)*p.y()+m(2,2)*p.z()+m(2,3)*p.w(),
512  m(3,0)*p.x()+m(3,1)*p.y()+m(3,2)*p.z()+m(3,3)*p.w());
513 }
514 
515 //: Homographic transformation of a 3D plane through a 4x4 projective transformation matrix
516 template <class T>
517 vgl_homg_plane_3d<T> operator*(vnl_matrix_fixed<T,4,4> const& m,
518  vgl_homg_plane_3d<T> const& p)
519 {
520  return vgl_homg_plane_3d<T>(m(0,0)*p.a()+m(0,1)*p.b()+m(0,2)*p.c()+m(0,3)*p.d(),
521  m(1,0)*p.a()+m(1,1)*p.b()+m(1,2)*p.c()+m(1,3)*p.d(),
522  m(2,0)*p.a()+m(2,1)*p.b()+m(2,2)*p.c()+m(2,3)*p.d(),
523  m(3,0)*p.a()+m(3,1)*p.b()+m(3,2)*p.c()+m(3,3)*p.d());
524 }
525 
526 //: Project a 3D point to 2D through a 3x4 projective transformation matrix
527 template <class T>
528 vgl_homg_point_2d<T> operator*(vnl_matrix_fixed<T,3,4> const& m,
529  vgl_homg_point_3d<T> const& p)
530 {
531  return vgl_homg_point_2d<T>(m(0,0)*p.x()+m(0,1)*p.y()+m(0,2)*p.z()+m(0,3)*p.w(),
532  m(1,0)*p.x()+m(1,1)*p.y()+m(1,2)*p.z()+m(1,3)*p.w(),
533  m(2,0)*p.x()+m(2,1)*p.y()+m(2,2)*p.z()+m(2,3)*p.w());
534 }
535 
536 //: Backproject a 2D line through a 4x3 projective transformation matrix
537 template <class T>
538 vgl_homg_plane_3d<T> operator*(vnl_matrix_fixed<T,4,3> const& m,
539  vgl_homg_line_2d<T> const& l)
540 {
541  return vgl_homg_plane_3d<T>(m(0,0)*l.a()+m(0,1)*l.b()+m(0,2)*l.c(),
542  m(1,0)*l.a()+m(1,1)*l.b()+m(1,2)*l.c(),
543  m(2,0)*l.a()+m(2,1)*l.b()+m(2,2)*l.c(),
544  m(3,0)*l.a()+m(3,1)*l.b()+m(3,2)*l.c());
545 }
546 
547 #undef VGL_HOMG_OPERATORS_3D_INSTANTIATE
548 #define VGL_HOMG_OPERATORS_3D_INSTANTIATE(T) \
549  template class vgl_homg_operators_3d<T >; \
550  template vgl_homg_point_3d<T > operator*(vnl_matrix_fixed<T,4,4> const&,\
551  vgl_homg_point_3d<T > const&); \
552  template vgl_homg_plane_3d<T > operator*(vnl_matrix_fixed<T,4,4> const&,\
553  vgl_homg_plane_3d<T > const&); \
554  template vgl_homg_point_2d<T > operator*(vnl_matrix_fixed<T,3,4> const&,\
555  vgl_homg_point_3d<T > const&); \
556  template vgl_homg_plane_3d<T > operator*(vnl_matrix_fixed<T,4,3> const&,\
557  vgl_homg_line_2d<T > const&)
558 
559 #endif // vgl_homg_operators_3d_hxx_
3D homogeneous operations.
Definition: vgl_algo_fwd.h:29
homogeneous plane in 3D projective space
point in projective 3D space
T dot_product(v const &a, v const &b)
dot product or inner product of two vectors.
static double perp_dist_squared(const vgl_homg_point_3d< Type > &point, const vgl_homg_plane_3d< Type > &plane)
Get the square of the perpendicular distance to a plane.
static vgl_homg_point_3d< Type > intersect_line_and_plane(const vgl_homg_line_3d &, const vgl_homg_plane_3d< Type > &)
Return the intersection point of the line and plane.
static vgl_homg_point_3d< Type > intersection(const vgl_homg_plane_3d< Type > &, const vgl_homg_plane_3d< Type > &, const vgl_homg_plane_3d< Type > &)
Compute best-fit intersection of planes in a point.
Represents a homogeneous 2D line.
Definition: vgl_fwd.h:14
static vgl_homg_point_3d< Type > planes_to_point(const std::vector< vgl_homg_plane_3d< Type > > &planes)
Intersect a set of 3D planes to find the least-square point of intersection.
Represents a homogeneous 3D plane.
Definition: vgl_fwd.h:22
static double cross_ratio(const vgl_homg_point_3d< Type > &p1, const vgl_homg_point_3d< Type > &p2, const vgl_homg_point_3d< Type > &p3, const vgl_homg_point_3d< Type > &p4)
Calculates the cross ratio of four collinear points p1, p2, p3 and p4.
static Type plane_plane_angle(const vgl_homg_plane_3d< Type > &plane1, const vgl_homg_plane_3d< Type > &plane2)
Dihedral angle (of intersection) of 2 planes.
Type b() const
Return y coefficient.
Type c() const
Return z coefficient.
Type d() const
Return homogeneous scaling coefficient.
line in projective 2D space
Represents a homogeneous 3D point.
Definition: vgl_fwd.h:9
static vnl_vector_fixed< Type, 4 > most_orthogonal_vector_svd(const std::vector< vgl_homg_plane_3d< Type > > &planes)
compute most orthogonal vector with SVD.
#define dot(p, q)
static vgl_homg_point_3d< Type > midpoint(const vgl_homg_point_3d< Type > &p1, const vgl_homg_point_3d< Type > &p2)
Return the midpoint of the line joining two homogeneous points.
static Type distance_squared(const vgl_homg_point_3d< Type > &point1, const vgl_homg_point_3d< Type > &point2)
Return the squared distance between the points.
point in projective 2D space
vgl_vector_3d< double > normal() const
3D homogeneous functions
static vgl_homg_point_3d< Type > conjugate(const vgl_homg_point_3d< Type > &a, const vgl_homg_point_3d< Type > &b, const vgl_homg_point_3d< Type > &c, double cr=-1.0)
Conjugate point of three given collinear points.
Type a() const
Return x coefficient.
static vgl_homg_line_3d planes_to_line(const vgl_homg_plane_3d< Type > &plane1, const vgl_homg_plane_3d< Type > &plane2)
Return the intersection line of the planes.
static double angle_between_oriented_lines(const vgl_homg_line_3d &line1, const vgl_homg_line_3d &line2)
Return the angle between the (oriented) lines (in radians).
void set(Type px, Type py, Type pz, Type pw=(Type) 1)
Set x,y,z,w.
bool ideal(Type tol=(Type) 0) const
Return true iff the point is at infinity (an ideal point).
#define l
vgl_homg_point_1d< T > operator *(vnl_matrix_fixed< T, 2, 2 > const &m, vgl_homg_point_1d< T > const &p)
Transform a point through a 2x2 projective transformation matrix.
static void unitize(vgl_homg_point_3d< Type > &a)
Normalize vgl_homg_point_3d<Type> to unit magnitude.
static vgl_homg_point_3d< Type > perp_projection(const vgl_homg_line_3d &line, const vgl_homg_point_3d< Type > &point)
Compute the perpendicular projection point of p onto l.
static vgl_homg_line_3d perp_line_through_point(const vgl_homg_line_3d &line, const vgl_homg_point_3d< Type > &point)
Return the line which is perpendicular to l and passes through p.
Represents a homogeneous 2D point.
Definition: vgl_fwd.h:8
vgl_homg_point_3d< Type > point_finite() const
Finite point (Could be an ideal point, if the whole line is at infinity.).
static Type distance(const vgl_homg_point_3d< Type > &point1, const vgl_homg_point_3d< Type > &point2)
Return the Euclidean distance between the points.
static vnl_vector_fixed< Type, 4 > get_vector(vgl_homg_point_3d< Type > const &p)
Get a vnl_vector_fixed representation of a homogeneous object.
vgl_homg_point_3d< Type > point_infinite() const
Infinite point: the intersection of the line with the plane at infinity.
Represents a homogeneous 3D line using two points.
Definition: vgl_fwd.h:15