1919
2020#include " matfun.h"
2121#include " uncertain.h"
22- #include " usegment.h"
23-
24- #include < QDebug>
25- #include < QStringLiteral>
2622
2723#include < cassert>
2824#include < cfloat>
2925
3026#include < Eigen/Core>
31- #include < Eigen/Eigenvalues>
3227
3328
34- namespace Uncertain {
29+ using Eigen::Matrix;
30+ using Eigen::Matrix2d;
31+ using Eigen::Vector2d;
3532
3633using Matfun::null;
3734using Matfun::sign;
3835
3936
37+ namespace Uncertain {
38+
4039
4140// ! Spherically normalize the entity
4241void BasicEntity2D::normalizeSpherical ()
@@ -45,12 +44,12 @@ void BasicEntity2D::normalizeSpherical()
4544
4645 const Matrix3d I = Matrix3d::Identity ();
4746 assert (m_val.norm () > 0.0 );
48- Matrix3d const Jac = (I - m_val*m_val.adjoint ()/m_val.squaredNorm ()) / m_val.norm ();
47+ const Matrix3d Jac = (I - m_val*m_val.adjoint ()/m_val.squaredNorm ()) / m_val.norm ();
4948 m_cov = Jac*m_cov*Jac.adjoint ();
50-
5149 m_val.normalize (); // x = x /norm(x)
5250}
5351
52+
5453// ! Check if uncertainty entity is identical with uncertain entity 'us'
5554bool BasicEntity2D::isIdenticalTo ( const BasicEntity2D & us,
5655 const double T) const
@@ -67,11 +66,11 @@ bool BasicEntity2D::isIdenticalTo( const BasicEntity2D & us,
6766 a.m_val *= sign ( a.v ()(idx) ); // a = a*sign( a(idx) );
6867 b.m_val *= sign ( b.v ()(idx) ); // b = b*sign( b(idx) );
6968
70- Matrix<double , 3 , 2 > const JJ = null (a.v ()); // (A.120)
71- Vector2d const d = JJ .adjoint ()*( a.v () -b.v () ); // (10.141)
72- Eigen::Matrix2d const Cov_dd = JJ .adjoint () * (a.Cov () + b.Cov ()) * JJ ;
69+ const Matrix<double , 3 , 2 > Jac = null (a.v ()); // (A.120)
70+ const Vector2d d = Jac .adjoint ()*( a.v () -b.v () ); // (10.141)
71+ const Matrix2d Sigma_dd = Jac .adjoint () * (a.Cov () + b.Cov ()) * Jac ;
7372
74- return d.dot (Cov_dd .ldlt ().solve (d) ) < T; // dof = 2
73+ return d.dot ( Sigma_dd .ldlt ().solve (d) ) < T; // dof = 2
7574}
7675
7776} // namespace Uncertain
0 commit comments