2 #ifndef vgl_homg_operators_3d_hxx_ 3 #define vgl_homg_operators_3d_hxx_ 13 # include <vcl_msvc_warnings.h> 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> 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();
40 n = (dir1.
x()*dir2.
x()+dir1.
y()*dir2.
y()+dir1.
z()*dir2.
z())/std::sqrt(n);
51 d = point1.
x()/point1.
w() - point2.
x()/point2.
w();
54 d = point1.
y()/point1.
w() - point2.
y()/point2.
w();
57 d = point1.
z()/point1.
w() - point2.
z()/point2.
w();
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);
96 if ( numerator + denominator != 0 )
97 scale = 1.0/(numerator + denominator);
99 scale = 1.0/numerator;
101 denominator *= scale;
103 vnl_vector_fixed<Type,4> r = x1 * Type(denominator) + x2 * Type(numerator);
112 #if 0 // linker error better than run-time error. 113 template <
class Type>
116 const vgl_homg_line_3d& )
118 std::cerr <<
"Warning: vgl_homg_operators_3d<Type>::lines_to_point() not yet implemented\n";
127 #if 0 // linker error better than run-time error. 128 template <
class Type>
132 std::cerr <<
"Warning: vgl_homg_operators_3d<Type>::lines_to_point() not yet implemented\n";
141 template <
class Type>
154 template <
class Type>
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";
188 template <
class Type>
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] };
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;
209 template <
class Type>
215 return (Type)std::acos(cosang);
222 template <
class Type>
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);
235 r = ns.get_column(1);
245 #if 0 // linker error better than run-time error. 246 template <
class Type>
250 std::cerr <<
"Warning: vgl_homg_operators_3d<Type>::planes_to_line() not yet implemented\n";
251 return vgl_homg_line_3d<Type>();
260 #if 0 // linker error better than run-time error. 261 template <
class Type>
266 std::cerr <<
"Warning: vgl_homg_operators_3d<Type>::points_to_line() not yet implemented\n";
267 return vgl_homg_line_3d<Type>();
275 #if 0 // linker error better than run-time error. 276 template <
class Type>
280 std::cerr <<
"Warning: vgl_homg_operators_3d<Type>::points_to_line() not yet implemented\n";
281 return vgl_homg_line_3d<Type>();
289 #if 0 // linker error better than run-time error. 290 template <
class Type>
296 std::cerr <<
"Warning: vgl_homg_operators_3d<Type>::points_to_plane() not yet implemented\n";
306 #if 0 // linker error better than run-time error. 307 template <
class Type>
311 std::cerr <<
"Warning: vgl_homg_operators_3d<Type>::points_to_plane() not yet implemented\n";
320 template <
class Type>
329 template <
class Type>
333 int n = planes.size();
335 vnl_matrix<Type> A(n, 4);
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();
344 vnl_svd<Type> svd(A);
349 template <
class Type>
352 return vnl_vector_fixed<Type,4>(p.
x(),p.
y(),p.
z(),p.
w());
355 template <
class Type>
358 return vnl_vector_fixed<Type,4>(p.
nx(),p.
ny(),p.
nz(),p.
d());
361 template <
class Type>
364 double norm = a.
x()*a.
x() + a.
y()*a.
y() + a.
z()*a.
z() + a.
w()*a.
w();
367 std::cerr <<
"vgl_homg_operators_3d<Type>::unitize() -- Zero length vector\n";
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));
377 template <
class Type>
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();
394 template <
class Type>
398 assert(planes.size() >= 3);
400 vnl_vector_fixed<Type,4> mov = most_orthogonal_vector_svd(planes);
404 template <
class Type>
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;
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";
428 template <
class Type>
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;
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";
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);
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";
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();
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;
491 vnl_vector_fixed<T,4>
494 vnl_matrix<T> D(planes.size(), 4);
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());
501 return svd.nullvector();
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());
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());
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());
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());
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&) 559 #endif // vgl_homg_operators_3d_hxx_ 3D homogeneous operations.
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.
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.
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.
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.
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
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).
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.
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.