00001
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
00155 HomogenousVecD proj_p_coor;
00156 typename Atom<dimension>::HomogenousSymMatrixD projCov;
00157 seg.projectCov(pt1, pt1Cov, proj_p_coor, projCov);
00158
00159
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
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
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 {
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;
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 }