Jafar
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines
DistanceCovImpl.hpp
00001 /* $Id$ */
00002 
00003 #include "Distance.hpp"
00004 #include <kernel/jafarMacro.hpp>
00005 
00006 namespace jafar {
00007   namespace geom {
00008     namespace details {
00009       template<typename _T_>
00010       inline double sum( const _T_& t)
00011       {
00012         double r = 0.0;
00013         for(std::size_t i = 0; i < t.size1(); ++i)
00014         {
00015           r += t(i,i);
00016         }
00017         return r;
00018       }
00019     }
00020     template<int dimension>
00021     inline void norm2Cov( const typename Atom<dimension>::HomogenousVecD& vec,
00022                           const typename Atom<dimension>::HomogenousSymMatrixD& vecCov,
00023                           double& norm, double& cov)
00024     {
00025       norm = ublas::norm_2( vec );
00026       if(fabs(norm) > 1e-6)
00027       {
00028         double inv_d = 1.0 / norm;
00029         typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00030         HomogenousVecD Jp = vec * inv_d;
00031         cov = ublas::inner_prod( HomogenousVecD( ublas::prod( Jp, vecCov ) ), Jp );
00032       } else {
00033         cov = details::sum( vecCov );
00034       }      
00035     }
00036     template<int dimension>
00037     inline void distanceCov(const typename Atom<dimension>::HomogenousVecD& pt1, const typename Atom<dimension>::HomogenousSymMatrixD& pt1Cov, const typename Atom<dimension>::HomogenousVecD& pt2, const typename Atom<dimension>::HomogenousSymMatrixD& pt2Cov, double& dist, double& cov )
00038     {
00039       norm2Cov<dimension>(pt1 - pt2, pt1Cov + pt2Cov, dist, cov);      
00040     }
00041     template<int dimension>
00042     inline void distanceCov(const Point<dimension>& pt1, const Point<dimension>& pt2, double& distance, double& cov )
00043     {
00044       JFR_ASSERT(pt1.reference() == pt2.reference(), "reference need to be equal");
00045       JFR_ASSERT(pt1.hasCov(), "First point need to have covariance");
00046       JFR_ASSERT(pt2.hasCov(), "Second point need to have covariance");
00047       distanceCov<dimension>( pt1.homogenousCoordinates(), pt1.homogenousCoordinatesCov(), pt2.homogenousCoordinates(), pt2.homogenousCoordinatesCov(), distance, cov );
00048     }
00049     template<int dimension>
00050     inline void distanceCov(const Point<dimension>& pt1,
00051                             const typename Atom<dimension>::HomogenousVecD& pt2,
00052                             const typename Atom<dimension>::HomogenousSymMatrixD& pt2Cov,
00053                             double& distance, double& cov )
00054     {
00055       distanceCov<dimension>(pt1.homogenousCoordinates(), pt1.homogenousCoordinatesCov(), pt2, pt2Cov, distance, cov);
00056     }
00060     template<int dimension>
00061     inline void distanceCov(const typename Atom<dimension>::HomogenousVecD& pt1v,
00062                             const typename Atom<dimension>::HomogenousSymMatrixD& pt1vCov,
00063                             const Line<dimension>& li2, 
00064                             double& distance, double& cov )
00065     {
00066       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00067       typedef typename Atom<dimension>::HomogenousSymMatrixD HomogenousSymMatrixD;
00068       JFR_ASSERT(li2.hasCov(), "Second line need to have covariance");
00069       HomogenousVecD proj;
00070       HomogenousSymMatrixD projCov;
00071       li2.projectCov( pt1v, pt1vCov, proj, projCov );
00072       distanceCov<dimension>( pt1v, pt1vCov, proj, projCov, distance, cov );
00073     }
00074     template<int dimension>
00075     inline void distanceCov(const Line<dimension>& li2, 
00076                             const typename Atom<dimension>::HomogenousVecD& pt1v,
00077                             const typename Atom<dimension>::HomogenousSymMatrixD& pt1vCov,
00078                             double& distance, double& cov )
00079     {
00080       distanceCov(pt1v, pt1vCov, li2, distance, cov); 
00081     }
00082     template<int dimension>
00083     inline void distanceCov(const Point<dimension>& pt1, const Line<dimension>& li2, double& distance, double& cov )
00084     {
00085       JFR_ASSERT(pt1.hasCov(), "First point need to have covariance");
00086       JFR_ASSERT(li2.hasCov(), "Second line need to have covariance");
00087       distanceCov(pt1.homogenousCoordinates(), pt1.homogenousCoordinatesCov(), li2, distance, cov );
00088     }
00089     template<int dimension>
00090     inline void distanceCov(const Line<dimension>& li1, const Point<dimension>& pt2, double& distance, double& cov )
00091     {
00092       distanceCov(pt2, li1, distance, cov );
00093     }
00094     template<int dimension>
00095     inline void distanceCov(const typename Atom<dimension>::HomogenousVecD& pt1,
00096                             const typename Atom<dimension>::HomogenousSymMatrixD& pt1Cov,
00097                             const HyperPlane<dimension>& hp2, double& distance, double& cov )
00098     {
00099       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00100       typedef typename Atom<dimension>::HomogenousSymMatrixD HomogenousSymMatrixD;
00101       typedef typename Atom<dimension>::HomogenousMatrix1D HomogenousMatrix1D;
00102       JFR_ASSERT(hp2.hasCov(), "Second hyper plane need to have covariance");
00103       HomogenousVecD proj;
00104       HomogenousSymMatrixD projCov;
00105       hp2.projectionVectorCov( pt1, pt1Cov, proj, projCov );      
00106       JFR_ASSERT( fabs( proj(dimension) - 1.0 ) < 1e-6 , "Homogenous coefficient need to be 0" );
00107       distance = ublas::norm_2(proj);
00108       if(fabs(distance) > 1e-6)
00109       {
00110         double inv_d = 1.0 / distance;
00111         HomogenousMatrix1D Jp;
00112         for(int i = 0; i < dimension; ++i)
00113         {
00114           Jp(0, i) = proj(i) * inv_d;
00115         }
00116         cov = (ublas::prod( HomogenousMatrix1D( ublas::prod( Jp, projCov) ), ublas::trans( Jp ) ))(0,0);
00117       } else {
00118         cov = details::sum(projCov);
00119       }
00120     }
00121     template<int dimension>
00122     inline void distanceCov(const HyperPlane<dimension>& hp,
00123                             const typename Atom<dimension>::HomogenousVecD& pt1,
00124                             const typename Atom<dimension>::HomogenousSymMatrixD& pt1Cov,
00125                             double& distance, double& cov )
00126     {
00127       distanceCov( pt1, pt1Cov, hp, distance, cov );
00128     }
00129     template<int dimension>
00130     inline void distanceCov(const Point<dimension>& pt1, const HyperPlane<dimension>& hp2, double& distance, double& cov )
00131     {
00132       JFR_ASSERT(pt1.hasCov(), "First point need to have covariance");
00133       JFR_ASSERT(hp2.hasCov(), "Second hyper plan need to have covariance");
00134       distanceCov(pt1.homogenousCoordinates(), pt1.homogenousCoordinatesCov(), hp2, distance, cov);
00135     }
00136     template<int dimension>
00137     inline void distanceCov(const HyperPlane<dimension>& hp, const Point<dimension>& pt, double& distance, double& cov )
00138     {
00139       JFR_ASSERT(hp.hasCov(), "First hyper plane need to have covariance");
00140       JFR_ASSERT(pt.hasCov(), "Second line need to have covariance");
00141       distanceCov(pt, hp, distance, cov);
00142     }
00143     template<int dimension>
00144     inline void distanceCov(const Segment<dimension>& seg,
00145                             const typename Atom<dimension>::HomogenousVecD& pt1,
00146                             const typename Atom<dimension>::HomogenousSymMatrixD& pt1Cov,
00147                             double& distance, double& cov)
00148     {
00149       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00150       HomogenousVecD firstPoint = seg.firstPoint();
00151       HomogenousVecD lastPoint = seg.lastPoint();
00152       HomogenousVecD dir = lastPoint - firstPoint;
00153 
00154       // Compute the projection of the point
00155       HomogenousVecD proj_p_coor;
00156       typename Atom<dimension>::HomogenousSymMatrixD projCov;
00157       seg.projectCov(pt1, pt1Cov, proj_p_coor, projCov);
00158       
00159       // Check the position of the projection to determine wether to use extremeties or support      
00160       double nd = ublas::norm_2(dir);
00161       double alpha = ublas::inner_prod( dir, pt1 - firstPoint ) / (nd * nd);
00162       if( alpha < 0.0 ) distanceCov<dimension>(firstPoint, seg.firstPointCov(), pt1, pt1Cov, distance, cov);
00163       else if( alpha > 1.0 ) distanceCov<dimension>(lastPoint, seg.lastPointCov(), pt1, pt1Cov, distance, cov);
00164       else distanceCov<dimension>( proj_p_coor, projCov, pt1, pt1Cov, distance, cov );      
00165     }
00166     template<int dimension>
00167     inline void distanceCov(const Segment<dimension>& seg, const Point<dimension>& pt, double& distance, double& cov)
00168     {
00169       distanceCov(seg, pt.homogenousCoordinates(), pt.homogenousCoordinatesCov(), distance, cov);  
00170     }
00171     template<int dimension>
00172     inline void distanceCov(const Point<dimension>& pt, const Segment<dimension>& seg, double& distance, double& cov)
00173     {
00174       distanceCov(seg, pt.homogenousCoordinates(), pt.homogenousCoordinatesCov(), distance, cov);
00175     }
00176     template<int dimension>
00177     inline void distanceCov(const Segment<dimension>& seg, const Line<dimension>& l, double& distance, double& cov)
00178     {
00179       JFR_ASSERT(seg.hasCov(), "First segment need to have covariance");
00180       JFR_ASSERT(l.hasCov(), "Second line need to have covariance");
00181       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00182       typedef typename Atom<dimension>::HomogenousSymMatrixD HomogenousSymMatrixD;
00183       HomogenousVecD v1, v2;
00184       HomogenousSymMatrixD M1, M2;
00185       seg.closestPointCov(l, v1, M1, v2, M2);
00186       HomogenousVecD a = seg.lastPoint() - seg.firstPoint();
00187       double t = ublas::inner_prod(v1 - seg.firstPoint(), a) / ublas::inner_prod(a, a);
00188       if(t < 0)
00189       {
00190         distanceCov(seg.firstPoint(), seg.firstPointCov(), l, distance, cov);
00191       } else if( t > 1.0 )
00192       {
00193         distanceCov(seg.lastPoint(), seg.lastPointCov(), l, distance, cov);
00194       } else {
00195         distanceCov<dimension>(v1, M1, v2, M2, distance, cov );
00196       }
00197     }
00198     template<int dimension>
00199     inline void distanceCov(const Line<dimension>& l, const Segment<dimension>& seg, double& distance, double& cov)
00200     {
00201       JFR_ASSERT(l.hasCov(), "First line need to have covariance");
00202       JFR_ASSERT(seg.hasCov(), "Second segment need to have covariance");
00203       distanceCov(seg, l, distance, cov);
00204     }
00205     template<int dimension>
00206     inline double norm2_cov( const typename Atom<dimension>::HomogenousVecD& v1,
00207                                   const typename Atom<dimension>::HomogenousSymMatrixD& v1Cov)
00208     {
00209       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00210       return 2 * (ublas::inner_prod( HomogenousVecD( ublas::prod( v1, v1Cov) ), v1));
00211     }
00212     template<int dimension>
00213     inline void distanceCov(const Segment<dimension>& seg1, const Segment<dimension>& seg2, double& distance, double& cov)
00214     {
00215       JFR_ASSERT(seg1.hasCov(), "First segment need to have covariance");
00216       JFR_ASSERT(seg2.hasCov(), "Second segment need to have covariance");
00217       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00218       typedef typename Atom<dimension>::HomogenousSymMatrixD HomogenousSymMatrixD;
00219       typedef typename Atom<dimension>::HomogenousMatrixD HomogenousMatrixD;
00220       // First, check for parallel lines
00221       HomogenousVecD s1ds2d = seg1.direction() - seg2.direction();
00222       if( (ublas::inner_prod(s1ds2d, s1ds2d) + 2.0 * sqrt(norm2_cov<dimension>(s1ds2d, seg1.directionCov() + seg2.directionCov()) )) < 0.05 )
00223       {
00224         distanceCov<dimension>( (const Line<dimension>&)seg1, seg2.firstPoint(), seg2.firstPointCov(), distance, cov);
00225       } else {
00226         HomogenousVecD v1, v2;
00227         HomogenousSymMatrixD M1, M2;
00228         seg1.closestPointCov(seg2, v1, M1, v2, M2);
00229         HomogenousVecD a1 = seg1.lastPoint() - seg1.firstPoint();
00230         double t1 = ublas::inner_prod(v1 - seg1.firstPoint(), a1) / ublas::inner_prod(a1, a1);
00231         HomogenousVecD a2 = seg2.lastPoint() - seg2.firstPoint();
00232         double t2 = ublas::inner_prod(v2 - seg2.firstPoint(), a2) / ublas::inner_prod(a2, a2);
00233         
00234         if(t1 <= 0.0)
00235         {
00236           if(t2 <= 0.0)
00237           {
00238             distanceCov<dimension>(seg1.firstPoint(), seg1.firstPointCov(), seg2.firstPoint(), seg2.firstPointCov(), distance, cov);
00239           } else if(t2 >= 1.0) {
00240             distanceCov<dimension>(seg1.firstPoint(), seg1.firstPointCov(), seg2.lastPoint(), seg2.lastPointCov(), distance, cov);
00241           } else {
00242             distanceCov<dimension>(seg2, seg1.firstPoint(), seg1.firstPointCov(), distance, cov);
00243           }
00244         } else if( t1 >= 1.0) {
00245           if(t2 <= 0.0)
00246           {
00247             distanceCov<dimension>(seg1.lastPoint(), seg1.lastPointCov(), seg2.firstPoint(), seg2.firstPointCov(), distance, cov);
00248           } else if(t2 >= 1.0) {
00249             distanceCov<dimension>(seg1.lastPoint(), seg1.lastPointCov(), seg2.lastPoint(), seg2.lastPointCov(), distance, cov);
00250           } else {
00251             distanceCov<dimension>(seg2, seg1.lastPoint(), seg1.lastPointCov(), distance, cov);
00252           }
00253         } else {
00254           if(t2 <= 0.0)
00255           {
00256             distanceCov<dimension>(seg1, seg2.firstPoint(), seg2.firstPointCov(), distance, cov);
00257           } else if(t2 >= 1.0) {
00258             distanceCov<dimension>(seg1, seg2.lastPoint(), seg2.lastPointCov(), distance, cov);
00259           } else {
00260             // Use id - outer_prod(seg1.direction() * seg.direction) + seg1.directionCov()
00261             HomogenousVecD v = v1- v2;
00262             HomogenousMatrixD JacProj = jblas::identity_mat(dimension + 1) - ublas::outer_prod(seg1.direction(), seg1.direction()); JacProj(dimension, dimension) -= 1.0;
00263             HomogenousSymMatrixD vCov = ublas::prod( HomogenousMatrixD( ublas::prod( JacProj, M1+M2)), ublas::trans(JacProj)) + seg1.directionCov();
00264             norm2Cov<dimension>(v, vCov, distance, cov );
00265             if( distance < 1e-6)
00266             { // HACK, when the distance is small, the covariance on the closest point become monstruous for no good reason
00267               cov = details::sum(seg1.directionCov() + seg2.directionCov());
00268             }
00269           }
00270         }
00271       }
00272     }
00273     template<int dimension>
00274     inline void distanceCov(const Segment<dimension>& seg, const HyperPlane<dimension>& hp, double& distance, double& _distanceCov)
00275     {
00276       JFR_ASSERT(seg.hasCov(), "First segment need to have covariance");
00277       JFR_ASSERT(hp.hasCov(), "Second hyper plane need to have covariance");
00278       JFR_ASSERT(seg.reference() == hp.reference(), "Unimplemented different references");
00279       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00280       HomogenousVecD intersectPoint;
00281       bool v = intersection<dimension>(seg, hp, intersectPoint);
00282       if(v)
00283       {
00284         HomogenousVecD firstPoint = seg.firstPoint();
00285         HomogenousVecD lastPoint = seg.lastPoint();
00286         HomogenousVecD dir = lastPoint - firstPoint;
00287         double nd = ublas::norm_2(dir);
00288         double alpha = ublas::inner_prod( dir, intersectPoint - firstPoint ) / (nd * nd);
00289         if( alpha < 0.0 ) distanceCov( firstPoint, seg.firstPointCov(), hp, distance, _distanceCov );
00290         else if( alpha > 1.0 ) distanceCov( lastPoint, seg.lastPointCov(), hp, distance, _distanceCov );
00291         else {
00292           distance = 0.0;
00293           _distanceCov = 0.0; // It would be more precise, I think to compute alphaCov and check how much it is far from 0.0 and 1.0
00294         }
00295       } else {
00296         distanceCov( seg.firstPoint(), seg.firstPointCov(), hp, distance, _distanceCov );
00297       }
00298     }
00299     template<int dimension>
00300     inline void distanceCov(const HyperPlane<dimension>& hp, const Segment<dimension>& seg, double& distance, double& _distanceCov)
00301     {
00302       JFR_ASSERT(hp.hasCov(), "First hyper plane need to have covariance");
00303       JFR_ASSERT(seg.hasCov(), "Second segment need to have covariance");
00304       distanceCov( seg, hp, distance, _distanceCov);
00305     }
00306     template<int dimension, typename _T2_>
00307     inline void distanceCovPolyLine(const PolyLine<dimension>& pl, const _T2_& t2, double& _distance, double& _distanceCov)
00308     {
00309       JFR_ASSERT(pl.hasCov(), "Poly line need to have covariance");
00310       JFR_ASSERT(t2.hasCov(), "Second object need to have covariance");
00311       std::vector< Segment<dimension> > segments = pl.segments();
00312       typename std::vector< Segment<dimension> >::iterator it = segments.begin();
00313       distanceCov( *it, t2, _distance, _distanceCov );
00314       ++it;
00315       for( ; it != segments.end(); ++it)
00316       {
00317         double d, dCov;
00318         distanceCov( *it, t2, d, dCov );
00319         if( d < _distance) {
00320           _distance = d;
00321           _distanceCov = dCov;
00322         }
00323       }
00324     }
00325     template<int dimension>
00326     inline void distanceCov(const Segment<dimension>& seg, const PolyLine<dimension>& pl, double& distance, double& distanceCov)
00327     {
00328       distanceCovPolyLine(pl, seg, distance, distanceCov);
00329     }
00330     template<int dimension>
00331     inline void distanceCov(const PolyLine<dimension>& pl, const Segment<dimension>& seg, double& distance, double& distanceCov)
00332     {
00333       distanceCovPolyLine(pl, seg, distance, distanceCov);
00334     }
00335     template<int dimension>
00336     inline void distanceCov(const Facet<dimension>& hp, const Segment<dimension>& seg, double& distance, double& distanceCov)
00337     {
00338       distanceCovPolyLine(hp.spacePolyLine(), seg, distance, distanceCov);
00339     }
00340     template<int dimension>
00341     inline void distanceCov(const Segment<dimension>& seg, const Facet<dimension>& hp, double& distance, double& distanceCov)
00342     {
00343       distanceCovPolyLine(hp.spacePolyLine(), seg, distance, distanceCov);
00344     }
00345     template<int dimension>
00346     inline void distanceCov(const Facet<dimension>& hp, const Line<dimension>& seg, double& distance, double& distanceCov)
00347     {
00348       distanceCovPolyLine(hp.spacePolyLine(), seg, distance, distanceCov);
00349     }
00350     template<int dimension>
00351     inline void distanceCov(const Line<dimension>& seg, const Facet<dimension>& hp, double& distance, double& distanceCov)
00352     {
00353       distanceCovPolyLine(hp.spacePolyLine(), seg, distance, distanceCov);
00354     }
00355     template<int dimension>
00356     inline void distanceCov(const Facet<dimension>& hp, const HyperPlane<dimension>& seg, double& distance, double& distanceCov)
00357     {
00358       distanceCovPolyLine(hp.spacePolyLine(), seg, distance, distanceCov);
00359     }
00360     template<int dimension>
00361     inline void distanceCov(const HyperPlane<dimension>& seg, const Facet<dimension>& hp, double& distance, double& distanceCov)
00362     {
00363       distanceCovPolyLine(hp.spacePolyLine(), seg, distance, distanceCov);
00364     }
00365     template<int dimension>
00366     inline void distanceCov(const Facet<dimension>& seg, const Facet<dimension>& hp, double& distance, double& distanceCov)
00367     {
00368       distanceCovPolyLine(hp.spacePolyLine(), seg.spacePolyLine(), distance, distanceCov);
00369     }
00370   }
00371 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines
Generated on Wed Oct 15 2014 00:37:19 for Jafar by doxygen 1.7.6.1
LAAS-CNRS