core/vgl/algo/vgl_homg_operators_3d.txx
Go to the documentation of this file.
00001 // This is core/vgl/algo/vgl_homg_operators_3d.txx
00002 #ifndef vgl_homg_operators_3d_txx_
00003 #define vgl_homg_operators_3d_txx_
00004 //:
00005 // \file
00006 
00007 #include "vgl_homg_operators_3d.h"
00008 //
00009 #include <vcl_iostream.h>
00010 #include <vcl_vector.h>
00011 #include <vcl_cmath.h> // for vcl_sqrt(), vcl_acos()
00012 #include <vcl_cassert.h>
00013 
00014 #include <vnl/vnl_vector_fixed.h>
00015 #include <vnl/vnl_matrix_fixed.h>
00016 #include <vnl/vnl_matrix.h>
00017 #include <vnl/algo/vnl_svd.h>
00018 
00019 #include <vgl/vgl_homg_point_3d.h>
00020 #include <vgl/vgl_homg_plane_3d.h>
00021 #include <vgl/vgl_homg_line_2d.h>
00022 #include <vgl/vgl_homg_point_2d.h>
00023 
00024 //-----------------------------------------------------------------------------
00025 
00026 //: Return the angle between the (oriented) lines (in radians)
00027 //
00028 template <class Type>
00029 double vgl_homg_operators_3d<Type>::angle_between_oriented_lines(const vgl_homg_line_3d& l1,
00030                                                                  const vgl_homg_line_3d& l2)
00031 {
00032   vgl_homg_point_3d<Type> const& dir1 = l1.point_infinite();
00033   vgl_homg_point_3d<Type> const& dir2 = l2.point_infinite();
00034   double n = dir1.x()*dir1.x()+dir1.y()*dir1.y()+dir1.z()*dir1.z();
00035   n       *= dir2.x()*dir2.x()+dir2.y()*dir2.y()+dir2.z()*dir2.z();
00036   // dot product of unit direction vectors:
00037   n = (dir1.x()*dir2.x()+dir1.y()*dir2.y()+dir1.z()*dir2.z())/vcl_sqrt(n);
00038   return vcl_acos(n);
00039 }
00040 
00041 template <class Type>
00042 Type vgl_homg_operators_3d<Type>::distance_squared(const vgl_homg_point_3d<Type>& point1,
00043                                                    const vgl_homg_point_3d<Type>& point2)
00044 {
00045   Type mag = 0;
00046   Type d;
00047 
00048   d = point1.x()/point1.w() - point2.x()/point2.w();
00049   mag += d*d;
00050 
00051   d = point1.y()/point1.w() - point2.y()/point2.w();
00052   mag += d*d;
00053 
00054   d = point1.z()/point1.w() - point2.z()/point2.w();
00055   mag += d*d;
00056 
00057   return mag;
00058 }
00059 
00060 //-----------------------------------------------------------------------------
00061 
00062 //: Return the Euclidean distance between the points
00063 //
00064 template <class Type>
00065 Type vgl_homg_operators_3d<Type>::distance(const vgl_homg_point_3d<Type>&point1,
00066                                            const vgl_homg_point_3d<Type>&point2)
00067 {
00068   return vcl_sqrt(vgl_homg_operators_3d<Type>::distance_squared(point1,point2));
00069 }
00070 
00071 //-----------------------------------------------------------------------------
00072 
00073 //: Return the intersection point of the line and plane
00074 //
00075 template <class Type>
00076 vgl_homg_point_3d<Type> vgl_homg_operators_3d<Type>::intersect_line_and_plane(
00077                                   const vgl_homg_line_3d &line,
00078                                   const vgl_homg_plane_3d<Type>& plane)
00079 {
00080   // use P.(S + lambda D) = 0 to find lambda, and hence a point on the plane.
00081 
00082   const vnl_vector_fixed<Type,4> x1 = get_vector(line.point_finite());
00083   const vnl_vector_fixed<Type,4> x2 = get_vector(line.point_infinite());
00084   const vnl_vector_fixed<Type,4>  p = get_vector(plane);
00085 
00086   // FIXME: this works for double and smaller, but not complex. it might happen.
00087 
00088   double numerator = -dot_product(x1, p);  // find out if dot_product is ok
00089   double denominator = dot_product(x2, p);
00090 
00091   // Scale for conditioning
00092   double scale;
00093   if ( numerator + denominator != 0 )
00094     scale = 1.0/(numerator + denominator);
00095   else
00096     scale = 1.0/numerator;
00097   numerator *= scale;
00098   denominator *= scale;
00099 
00100   vnl_vector_fixed<Type,4> r = x1 * Type(denominator) + x2 * Type(numerator);
00101   return vgl_homg_point_3d<Type>(r[0], r[1], r[2], r[3]);
00102 }
00103 
00104 //-----------------------------------------------------------------------------
00105 //
00106 // Compute the intersection point of the lines, or the mid-point
00107 // of the common perpendicular if the lines are skew
00108 //
00109 #if 0 // linker error better than run-time error.
00110 template <class Type>
00111 vgl_homg_point_3d<Type>
00112 vgl_homg_operators_3d<Type>::lines_to_point(const vgl_homg_line_3d& ,
00113                                             const vgl_homg_line_3d& )
00114 {
00115   vcl_cerr << "Warning: vgl_homg_operators_3d<Type>::lines_to_point() not yet implemented\n";
00116   return vgl_homg_point_3d<Type>();
00117 }
00118 #endif
00119 
00120 //-----------------------------------------------------------------------------
00121 //
00122 // - Compute the best fit intersection point of the lines
00123 //
00124 #if 0 // linker error better than run-time error.
00125 template <class Type>
00126 vgl_homg_point_3d<Type>
00127 vgl_homg_operators_3d<Type>::lines_to_point(const vcl_vector<vgl_homg_line_3d >& )
00128 {
00129   vcl_cerr << "Warning: vgl_homg_operators_3d<Type>::lines_to_point() not yet implemented\n";
00130   return vgl_homg_point_3d<Type>();
00131 }
00132 #endif
00133 
00134 //-----------------------------------------------------------------------------
00135 
00136 //: Return the squared perpendicular distance between the line and point
00137 //
00138 template <class Type>
00139 double
00140 vgl_homg_operators_3d<Type>::perp_dist_squared(const vgl_homg_line_3d& l,
00141                                                const vgl_homg_point_3d<Type>& p)
00142 {
00143   vgl_homg_point_3d<Type> q = vgl_homg_operators_3d<Type>::perp_projection(l, p); // foot point
00144   return vgl_homg_operators_3d<Type>::distance_squared(p,q);
00145 }
00146 
00147 //-----------------------------------------------------------------------------
00148 
00149 //: Return the line which is perpendicular to l and passes through p.
00150 //
00151 template <class Type>
00152 typename vgl_homg_operators_3d<Type>::vgl_homg_line_3d
00153 vgl_homg_operators_3d<Type>::perp_line_through_point(const vgl_homg_line_3d& l,
00154                                                      const vgl_homg_point_3d<Type>& p)
00155 {
00156   if (p.ideal())
00157   {
00158     // this only works if p is not on l; and since the implementation below
00159     // only works when p is a finite point, use this one when p is at infinity.
00160     vgl_homg_point_3d<Type> perp_dirn = vgl_homg_operators_3d<Type>::perp_projection(l,p);
00161     if (get_vector(p)==get_vector(perp_dirn))
00162       vcl_cerr << "Warning: perp_line_through_point() makes no sense if the point is the infinity point of the line\n";
00163     return vgl_homg_line_3d(p, perp_dirn);
00164   }
00165   else // by Brendan McCane
00166   {
00167     // OK this is a better version because it works even if the point
00168     // is on the line. It does this simply by creating a direction
00169     // perpendicular to the current line and then creating a new
00170     // line with the passed in pt and the perpendicular direction.
00171     vgl_homg_point_3d<Type> dirn = l.point_infinite();
00172     vgl_homg_point_3d<Type> perp_dirn(Type(1)/dirn.x(), (-dirn.z()-1)/dirn.y(), Type(1), Type(0));
00173     // should put an assert in here making sure that the dot product
00174     // is zero (or close to), but I don't know how to do that with
00175     // templates eg complex<1e-8 probably doesn't make sense.
00176     return vgl_homg_line_3d(p, perp_dirn);
00177   }
00178 }
00179 
00180 
00181 //-----------------------------------------------------------------------------
00182 
00183 //: Compute the perpendicular projection point of p onto l.
00184 //
00185 template <class Type>
00186 vgl_homg_point_3d<Type>
00187 vgl_homg_operators_3d<Type>::perp_projection(const vgl_homg_line_3d& l,
00188                                              const vgl_homg_point_3d<Type>& p)
00189 {
00190   vgl_homg_point_3d<Type> const& q = l.point_finite();
00191   Type a[3]  = { q.x()/q.w(), q.y()/q.w(), q.z()/q.w() };
00192   Type b[3]  = { p.x()/p.w()-a[0], p.y()/p.w()-a[1], p.z()/p.w()-a[2] };
00193 
00194   vgl_homg_point_3d<Type> const& i = l.point_infinite();
00195   Type dp = i.x()*i.x()+i.y()*i.y()+i.z()*i.z();
00196   dp = (b[0]*i.x() + b[1]*i.y() + b[2]*i.z()) / dp;
00197 
00198   return vgl_homg_point_3d<Type>(a[0]+dp*i.x(), a[1]+dp*i.y(), a[2]+dp*i.z());
00199 }
00200 
00201 
00202 //-----------------------------------------------------------------------------
00203 
00204 //: Dihedral angle (of intersection) of 2 planes
00205 //
00206 template <class Type>
00207 Type vgl_homg_operators_3d<Type>::plane_plane_angle(const vgl_homg_plane_3d<Type>& plane1,
00208                                                     const vgl_homg_plane_3d<Type>& plane2)
00209 {
00210   double cosang = dot_product(plane1.normal(), plane2.normal());
00211 
00212   return (Type)vcl_acos(cosang);
00213 }
00214 
00215 //-----------------------------------------------------------------------------
00216 
00217 //: Return the intersection line of the planes
00218 //
00219 template <class Type>
00220 typename vgl_homg_operators_3d<Type>::vgl_homg_line_3d
00221 vgl_homg_operators_3d<Type>::planes_to_line(const vgl_homg_plane_3d<Type>& plane1,
00222                                             const vgl_homg_plane_3d<Type>& plane2)
00223 {
00224   // TODO need equivalent of get_vector
00225   vnl_matrix_fixed<Type,2,4> M;
00226   M.set_row(0, get_vector(plane1));
00227   M.set_row(1, get_vector(plane2));
00228   vnl_svd<Type> svd(M.as_ref());
00229   vnl_matrix<Type> ns = svd.nullspace(2);
00230   vnl_vector_fixed<Type,4> r = ns.get_column(0);
00231   vgl_homg_point_3d<Type> p1(r[0], r[1], r[2], r[3]);
00232   r = ns.get_column(1);
00233   vgl_homg_point_3d<Type> p2(r[0], r[1], r[2], r[3]);
00234   return vgl_homg_line_3d(p1, p2);
00235 }
00236 
00237 
00238 //-----------------------------------------------------------------------------
00239 //
00240 // - Compute the best-fit intersection line of the planes
00241 //
00242 #if 0 // linker error better than run-time error.
00243 template <class Type>
00244 vgl_homg_operators_3d<Type>::vgl_homg_line_3d
00245 vgl_homg_operators_3d<Type>::planes_to_line(const vcl_vector<vgl_homg_plane_3d<Type> >&)
00246 {
00247   vcl_cerr << "Warning: vgl_homg_operators_3d<Type>::planes_to_line() not yet implemented\n";
00248   return vgl_homg_line_3d<Type>();
00249 }
00250 #endif
00251 
00252 
00253 //-----------------------------------------------------------------------------
00254 //
00255 // - Return the line through the points
00256 //
00257 #if 0 // linker error better than run-time error.
00258 template <class Type>
00259 vgl_homg_operators_3d<Type>::vgl_homg_line_3d
00260 vgl_homg_operators_3d<Type>::points_to_line(const vgl_homg_point_3d<Type>&,
00261                                             const vgl_homg_point_3d<Type>&)
00262 {
00263   vcl_cerr << "Warning: vgl_homg_operators_3d<Type>::points_to_line() not yet implemented\n";
00264   return vgl_homg_line_3d<Type>();
00265 }
00266 #endif
00267 
00268 //-----------------------------------------------------------------------------
00269 //
00270 // - Compute the best-fit line through the points
00271 //
00272 #if 0 // linker error better than run-time error.
00273 template <class Type>
00274 vgl_homg_operators_3d<Type>::vgl_homg_line_3d
00275 vgl_homg_operators_3d<Type>::points_to_line(const vcl_vector<vgl_homg_point_3d<Type> >&)
00276 {
00277   vcl_cerr << "Warning: vgl_homg_operators_3d<Type>::points_to_line() not yet implemented\n";
00278   return vgl_homg_line_3d<Type>();
00279 }
00280 #endif
00281 
00282 //-----------------------------------------------------------------------------
00283 //
00284 // - Return the plane through the points
00285 //
00286 #if 0 // linker error better than run-time error.
00287 template <class Type>
00288 vgl_homg_plane_3d<Type>
00289 vgl_homg_operators_3d<Type>::points_to_plane(const vgl_homg_point_3d<Type>&,
00290                                              const vgl_homg_point_3d<Type>&,
00291                                              const vgl_homg_point_3d<Type>&)
00292 {
00293   vcl_cerr << "Warning: vgl_homg_operators_3d<Type>::points_to_plane() not yet implemented\n";
00294   return vgl_homg_plane_3d<Type>();
00295 }
00296 #endif
00297 
00298 
00299 //-----------------------------------------------------------------------------
00300 //
00301 // - Compute the best-fit plane through the points
00302 //
00303 #if 0 // linker error better than run-time error.
00304 template <class Type>
00305 vgl_homg_plane_3d<Type>
00306 vgl_homg_operators_3d<Type>::points_to_plane(const vcl_vector<vgl_homg_point_3d<Type> >&)
00307 {
00308   vcl_cerr << "Warning: vgl_homg_operators_3d<Type>::points_to_plane() not yet implemented\n";
00309   return vgl_homg_plane_3d<Type>();
00310 }
00311 #endif
00312 
00313 //-----------------------------------------------------------------------------
00314 
00315 //: Compute best-fit intersection of planes in a point.
00316 //
00317 template <class Type>
00318 vgl_homg_point_3d<Type>
00319 vgl_homg_operators_3d<Type>::intersection(const vgl_homg_plane_3d<Type>& p1,
00320                                           const vgl_homg_plane_3d<Type>& p2,
00321                                           const vgl_homg_plane_3d<Type>& p3)
00322 {
00323   return vgl_homg_point_3d<Type>(p1, p2, p3);
00324 }
00325 
00326 template <class Type>
00327 vgl_homg_point_3d<Type>
00328 vgl_homg_operators_3d<Type>::intersection(const vcl_vector<vgl_homg_plane_3d<Type> >& planes)
00329 {
00330   int n = planes.size();
00331   assert(n >= 3);
00332   vnl_matrix<Type> A(n, 4);
00333 
00334   for (int i=0; i<n; ++i) {
00335     A(i,0) = planes[i].nx();
00336     A(i,1) = planes[i].ny();
00337     A(i,2) = planes[i].nz();
00338     A(i,3) = planes[i].d();
00339   }
00340 
00341   vnl_svd<Type> svd(A);
00342   return vgl_homg_point_3d<Type>(svd.nullvector().begin());
00343 }
00344 
00345 
00346 template <class Type>
00347 vnl_vector_fixed<Type,4> vgl_homg_operators_3d<Type>::get_vector(vgl_homg_point_3d<Type> const& p)
00348 {
00349   return vnl_vector_fixed<Type,4>(p.x(),p.y(),p.z(),p.w());
00350 }
00351 
00352 template <class Type>
00353 vnl_vector_fixed<Type,4> vgl_homg_operators_3d<Type>::get_vector(vgl_homg_plane_3d<Type> const& p)
00354 {
00355   return vnl_vector_fixed<Type,4>(p.nx(),p.ny(),p.nz(),p.d());
00356 }
00357 
00358 template <class Type>
00359 void vgl_homg_operators_3d<Type>::unitize(vgl_homg_point_3d<Type>& a)
00360 {
00361   double norm = a.x()*a.x() + a.y()*a.y() + a.z()*a.z() + a.w()*a.w();
00362 
00363   if (norm == 0.0) {
00364     vcl_cerr << "vgl_homg_operators_3d<Type>::unitize() -- Zero length vector\n";
00365     return;
00366   }
00367   norm = 1.0/vcl_sqrt(norm);
00368   a.set(Type(a.x()*norm), Type(a.y()*norm), Type(a.z()*norm), Type(a.w()*norm));
00369 }
00370 
00371 //: Return the midpoint of the line joining two homogeneous points
00372 //  When one of the points is at infinity, that point is returned.
00373 //  When both points are at infinity, the invalid point (0,0,0,0) is returned.
00374 template <class Type>
00375 vgl_homg_point_3d<Type>
00376 vgl_homg_operators_3d<Type>::midpoint(const vgl_homg_point_3d<Type>& p1,
00377                                       const vgl_homg_point_3d<Type>& p2)
00378 {
00379   Type x = p1.x() * p2.w() + p2.x() * p1.w();
00380   Type y = p1.y() * p2.w() + p2.y() * p1.w();
00381   Type z = p1.z() * p2.w() + p2.z() * p1.w();
00382   Type w = p1.w() * p2.w() + p2.w() * p1.w();
00383 
00384   return vgl_homg_point_3d<Type>(x,y,z,w);
00385 }
00386 
00387 //: Intersect a set of 3D planes to find the least-square point of intersection.
00388 // This finds the point $\bf x$ that minimizes $\|\tt L \bf x\|$, where $\tt L$
00389 // is the matrix whose rows are the planes. The implementation uses vnl_svd
00390 // to accumulate and compute the nullspace of $\tt L^\top \tt L$.
00391 template <class Type>
00392 vgl_homg_point_3d<Type>
00393 vgl_homg_operators_3d<Type>::planes_to_point(const vcl_vector<vgl_homg_plane_3d<Type> >& planes)
00394 {
00395   assert(planes.size() >= 3);
00396 
00397   vnl_vector_fixed<Type,4> mov = most_orthogonal_vector_svd(planes);
00398   return vgl_homg_point_3d<Type>(mov[0], mov[1], mov[2], mov[3]);
00399 }
00400 
00401 template <class Type>
00402 double vgl_homg_operators_3d<Type>::cross_ratio(const vgl_homg_point_3d<Type>& a,
00403                                                 const vgl_homg_point_3d<Type>& b,
00404                                                 const vgl_homg_point_3d<Type>& c,
00405                                                 const vgl_homg_point_3d<Type>& d)
00406 {
00407   double x1 = a.x(), y1 = a.y(), z1 = a.z(), w1 = a.w();
00408   double x2 = b.x(), y2 = b.y(), z2 = b.z(), w2 = b.w();
00409   double x3 = c.x(), y3 = c.y(), z3 = c.z(), w3 = c.w();
00410   double x4 = d.x(), y4 = d.y(), z4 = d.z(), w4 = d.w();
00411   double x = x1 - x2; if (x<0) x = -x; // assuming a != b ;-)
00412   double y = y1 - y2; if (y<0) y = -y;
00413   double z = z1 - z2; if (z<0) z = -z;
00414   double n = (x>y && x>z) ? (x1*w3-x3*w1)*(x2*w4-x4*w2) :
00415              (y>z)        ? (y1*w3-y3*w1)*(y2*w4-y4*w2) :
00416                             (z1*w3-z3*w1)*(z2*w4-z4*w2);
00417   double m = (x>y && x>z) ? (x1*w4-x4*w1)*(x2*w3-x3*w2) :
00418              (y>z)        ? (y1*w4-y4*w1)*(y2*w3-y3*w2) :
00419                             (z1*w4-z4*w1)*(z2*w3-z3*w2);
00420   if (n == 0 && m == 0)
00421     vcl_cerr << "cross ratio not defined: three of the given points coincide\n";
00422   return n/m;
00423 }
00424 
00425 template <class Type>
00426 double vgl_homg_operators_3d<Type>::cross_ratio(const vgl_homg_plane_3d<Type>& a,
00427                                                 const vgl_homg_plane_3d<Type>& b,
00428                                                 const vgl_homg_plane_3d<Type>& c,
00429                                                 const vgl_homg_plane_3d<Type>& d)
00430 {
00431   double x1 = a.a(), y1 = a.b(), z1 = a.c(), w1 = a.d();
00432   double x2 = b.a(), y2 = b.b(), z2 = b.c(), w2 = b.d();
00433   double x3 = c.a(), y3 = c.b(), z3 = c.c(), w3 = c.d();
00434   double x4 = d.a(), y4 = d.b(), z4 = d.c(), w4 = d.d();
00435   double x = x1 - x2; if (x<0) x = -x; // assuming a != b ;-)
00436   double y = y1 - y2; if (y<0) y = -y;
00437   double z = z1 - z2; if (z<0) z = -z;
00438   double n = (x>y && x>z) ? (x1*w3-x3*w1)*(x2*w4-x4*w2) :
00439              (y>z)        ? (y1*w3-y3*w1)*(y2*w4-y4*w2) :
00440                             (z1*w3-z3*w1)*(z2*w4-z4*w2);
00441   double m = (x>y && x>z) ? (x1*w4-x4*w1)*(x2*w3-x3*w2) :
00442              (y>z)        ? (y1*w4-y4*w1)*(y2*w3-y3*w2) :
00443                             (z1*w4-z4*w1)*(z2*w3-z3*w2);
00444   if (n == 0 && m == 0)
00445     vcl_cerr << "cross ratio not defined: three of the given planes coincide\n";
00446   return n/m;
00447 }
00448 
00449 //: Conjugate point of three given collinear points.
00450 // If cross ratio cr is given (default: -1), the generalized conjugate point
00451 // returned is such that ((x1,x2;x3,answer)) = cr.
00452 template <class T>
00453 vgl_homg_point_3d<T>
00454 vgl_homg_operators_3d<T>::conjugate(const vgl_homg_point_3d<T>& a,
00455                                     const vgl_homg_point_3d<T>& b,
00456                                     const vgl_homg_point_3d<T>& c,
00457                                     double cr)
00458 // Default for cr is -1.
00459 {
00460   T x1 = a.x(), y1 = a.y(), z1 = a.z(), w1 = a.w();
00461   T x2 = b.x(), y2 = b.y(), z2 = b.z(), w2 = b.w();
00462   T x3 = c.x(), y3 = c.y(), z3 = c.z(), w3 = c.w();
00463   T kx = x1*w3 - x3*w1, mx = x2*w3 - x3*w2, nx = T(kx*w2-cr*mx*w1);
00464   T ky = y1*w3 - y3*w1, my = y2*w3 - y3*w2, ny = T(ky*w2-cr*my*w1);
00465   T kz = z1*w3 - z3*w1, mz = z2*w3 - z3*w2, nz = T(kz*w2-cr*mz*w1);
00466   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);
00467 }
00468 
00469 template <class T>
00470 double
00471 vgl_homg_operators_3d<T>::perp_dist_squared(const vgl_homg_point_3d<T>& point,
00472                                             const vgl_homg_plane_3d<T>& plane)
00473 {
00474   if ((plane.a()==0 && plane.b()== 0 && plane.c()== 0) || point.w()==0) {
00475     vcl_cerr << "vgl_homg_operators_3d<T>::perp_dist_squared() -- plane or point at infinity\n";
00476     return 1e38;
00477   }
00478 
00479 #define dot(p,q) p.a()*q.x()+p.b()*q.y()+p.c()*q.z()+p.d()*q.w()
00480   double numerator = dot(plane,point) / point.w();
00481 #undef dot
00482   if (numerator == 0) return 0.0;
00483   double denominator = plane.a()*plane.a() + plane.b()*plane.b() + plane.c()*plane.c();
00484   return numerator * numerator / denominator;
00485 }
00486 
00487 template <class T>
00488 vnl_vector_fixed<T,4>
00489 vgl_homg_operators_3d<T>::most_orthogonal_vector_svd(const vcl_vector<vgl_homg_plane_3d<T> >& planes)
00490 {
00491   vnl_matrix<T> D(planes.size(), 4);
00492 
00493   typename vcl_vector<vgl_homg_plane_3d<T> >::const_iterator i = planes.begin();
00494   for (unsigned j = 0; i != planes.end(); ++i,++j)
00495     D.set_row(j, get_vector(*i).as_ref());
00496 
00497   vnl_svd<T> svd(D);
00498   return svd.nullvector();
00499 }
00500 
00501 //: Homographic transformation of a 3D point through a 4x4 projective transformation matrix
00502 template <class T>
00503 vgl_homg_point_3d<T> operator*(vnl_matrix_fixed<T,4,4> const& m,
00504                                vgl_homg_point_3d<T> const& p)
00505 {
00506   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(),
00507                               m(1,0)*p.x()+m(1,1)*p.y()+m(1,2)*p.z()+m(1,3)*p.w(),
00508                               m(2,0)*p.x()+m(2,1)*p.y()+m(2,2)*p.z()+m(2,3)*p.w(),
00509                               m(3,0)*p.x()+m(3,1)*p.y()+m(3,2)*p.z()+m(3,3)*p.w());
00510 }
00511 
00512 //: Homographic transformation of a 3D plane through a 4x4 projective transformation matrix
00513 template <class T>
00514 vgl_homg_plane_3d<T> operator*(vnl_matrix_fixed<T,4,4> const& m,
00515                                vgl_homg_plane_3d<T> const& p)
00516 {
00517   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(),
00518                               m(1,0)*p.a()+m(1,1)*p.b()+m(1,2)*p.c()+m(1,3)*p.d(),
00519                               m(2,0)*p.a()+m(2,1)*p.b()+m(2,2)*p.c()+m(2,3)*p.d(),
00520                               m(3,0)*p.a()+m(3,1)*p.b()+m(3,2)*p.c()+m(3,3)*p.d());
00521 }
00522 
00523 //: Project a 3D point to 2D through a 3x4 projective transformation matrix
00524 template <class T>
00525 vgl_homg_point_2d<T> operator*(vnl_matrix_fixed<T,3,4> const& m,
00526                                vgl_homg_point_3d<T> const& p)
00527 {
00528   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(),
00529                               m(1,0)*p.x()+m(1,1)*p.y()+m(1,2)*p.z()+m(1,3)*p.w(),
00530                               m(2,0)*p.x()+m(2,1)*p.y()+m(2,2)*p.z()+m(2,3)*p.w());
00531 }
00532 
00533 //: Backproject a 2D line through a 4x3 projective transformation matrix
00534 template <class T>
00535 vgl_homg_plane_3d<T> operator*(vnl_matrix_fixed<T,4,3> const& m,
00536                                vgl_homg_line_2d<T> const& l)
00537 {
00538   return vgl_homg_plane_3d<T>(m(0,0)*l.a()+m(0,1)*l.b()+m(0,2)*l.c(),
00539                               m(1,0)*l.a()+m(1,1)*l.b()+m(1,2)*l.c(),
00540                               m(2,0)*l.a()+m(2,1)*l.b()+m(2,2)*l.c(),
00541                               m(3,0)*l.a()+m(3,1)*l.b()+m(3,2)*l.c());
00542 }
00543 
00544 #undef VGL_HOMG_OPERATORS_3D_INSTANTIATE
00545 #define VGL_HOMG_OPERATORS_3D_INSTANTIATE(T) \
00546   template class vgl_homg_operators_3d<T >; \
00547   template vgl_homg_point_3d<T > operator*(vnl_matrix_fixed<T,4,4> const&,\
00548                                            vgl_homg_point_3d<T > const&); \
00549   template vgl_homg_plane_3d<T > operator*(vnl_matrix_fixed<T,4,4> const&,\
00550                                            vgl_homg_plane_3d<T > const&); \
00551   template vgl_homg_point_2d<T > operator*(vnl_matrix_fixed<T,3,4> const&,\
00552                                            vgl_homg_point_3d<T > const&); \
00553   template vgl_homg_plane_3d<T > operator*(vnl_matrix_fixed<T,4,3> const&,\
00554                                            vgl_homg_line_2d<T > const&)
00555 
00556 #endif // vgl_homg_operators_3d_txx_