Jafar: modules/geom/include/geom/AngleCovImpl.hpp Source File

00001 /* $ Id: $ */
00002 
00003 #include "Angle.hpp"
00004 
00005 namespace jafar {
00006   namespace geom {
00007     template<int dimension>
00008     inline void angleCov( const typename Atom<dimension>::HomogenousVecD& v1,
00009                           const typename Atom<dimension>::HomogenousSymMatrixD& v1Cov,
00010                           const typename Atom<dimension>::HomogenousVecD& v2,
00011                           const typename Atom<dimension>::HomogenousSymMatrixD& v2Cov,
00012                           double& angle, double &cov)
00013     {
00014       JFR_ASSERT( fabs(ublas::norm_2(v1) - 1.0) < 1e-6, "Vector norm should be equal to 1");
00015       JFR_ASSERT( fabs(ublas::norm_2(v2) - 1.0) < 1e-6, "Vector norm should be equal to 1");
00016       double ip = ublas::inner_prod( v1, v2 );
00017       if( ip < -1.0) ip = -1.0;
00018       else if( ip > 1.0 ) ip = 1.0;
00019       
00020       angle = impl::safeacos( ip );
00021       
00022       typedef typename Atom<dimension>::HomogenousMatrix1D HomogenousMatrix1D;
00023       typedef typename Atom<dimension>::HomogenousMatrixD1 HomogenousMatrixD1;
00024       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00025 //       JFR_DEBUG( v1 << " " << v2 << " " << ip );
00026       double d = 1 - ip * ip;
00027       double inv_ip2 = (d > 0.0) ? 1 / sqrt( d ) : 1e-6;
00028       HomogenousMatrix1D Jp1;
00029       HomogenousMatrix1D Jp2;
00030       for(int i = 0; i < dimension + 1; ++i)
00031       {
00032         Jp1(0,i) = -v2(i) * inv_ip2;
00033         Jp2(0,i) = -v1(i) * inv_ip2;
00034       }
00035 //       JFR_DEBUG(Jp1 << " " << inv_ip2 << " " << Jp2 << " " << v1Cov << " " << v2Cov );
00036       cov = (ublas::prod( HomogenousMatrixD1( ublas::prod(Jp1, v1Cov) ), ublas::trans( Jp1 ) )
00037           + ublas::prod( HomogenousMatrixD1( ublas::prod(Jp2, v2Cov) ), ublas::trans( Jp2 ) ) )(0,0);
00038 //       JFR_DEBUG(cov);
00039       JFR_ASSERT(std::isfinite(cov), "Not finite angle cov");
00040     }
00041     template<int dimension>
00042     inline void angleCov(const Line<dimension>& line1, const Line<dimension>& line2,
00043                          double& angle, double &cov)
00044     {
00045       angleCov<dimension>( line1.direction(), line1.directionCov(), line2.direction(), line2.directionCov(),
00046                 angle, cov );
00047     }
00048     template<int dimension>
00049     inline void angleCov(const Line<dimension>& line1, const HyperPlane<dimension>& hp2,
00050                          double& angle, double &cov)
00051     {
00052       angleCov<dimension>( line1.direction(), line1.directionCov(), hp2.normal(), hp2.normalCov(), angle, cov );
00053     }
00054     template<int dimension>
00055     inline void angleCov(const HyperPlane<dimension>& hp2, const Line<dimension>& line1, 
00056                          double& angle, double &cov)
00057     {
00058       angleCov( line1, hp2, angle, cov );
00059     }
00060     template<int dimension>
00061     inline void angleCov(const Segment<dimension>& line1, const Segment<dimension>& line2,
00062                          double& angle, double &cov)
00063     {
00064       angleCov<dimension>( line1.direction(), line1.directionCov(), line2.direction(), line2.directionCov(),
00065                 angle, cov );
00066     }
00067     template<int dimension>
00068     inline void angleCov(const Segment<dimension>& line1, const HyperPlane<dimension>& hp2,
00069                          double& angle, double &cov)
00070     {
00071       angleCov<dimension>( line1.direction(), line1.directionCov(), hp2.normal(), hp2.normalCov(), angle, cov );
00072     }
00073     template<int dimension>
00074     inline void angleCov(const HyperPlane<dimension>& hp2, const Segment<dimension>& line1, 
00075                          double& angle, double &cov)
00076     {
00077       angleCov( line1, hp2, angle, cov );
00078     }
00079     template<int dimension>
00080     inline void angleCov(const HyperPlane<dimension>& hp1, const HyperPlane<dimension>& hp2,
00081                          double& angle, double &cov)
00082     {
00083       angleCov<dimension>( hp1.normal(), hp1.normalCov(), hp2.normal(), hp2.normalCov(), angle, cov );
00084     }
00085   }
00086 }