ecl_statistics: covariance_ellipsoid.cpp Source File

Go to the documentation of this file.
00001 
00008 /*****************************************************************************
00009 ** Includes
00010 *****************************************************************************/
00011 
00012 #include <ecl/linear_algebra.hpp>
00013 #include "../../include/ecl/statistics/covariance_ellipsoid.hpp"
00014 
00015 /*****************************************************************************
00016 ** Namespaces
00017 *****************************************************************************/
00018 
00019 namespace ecl {
00020 
00021 /*****************************************************************************
00022 ** Using
00023 *****************************************************************************/
00024 
00025 using ecl::linear_algebra::Matrix2f;
00026 using ecl::linear_algebra::Vector2f;
00027 using ecl::linear_algebra::Matrix3f;
00028 using ecl::linear_algebra::Vector3f;
00029 
00030 using ecl::linear_algebra::Matrix2d;
00031 using ecl::linear_algebra::Vector2d;
00032 using ecl::linear_algebra::Matrix3d;
00033 using ecl::linear_algebra::Vector3d;
00034 
00035 /*****************************************************************************
00036 ** Implementation [CovarianceEllipsoid2f]
00037 *****************************************************************************/
00038 CovarianceEllipsoid<float,2>::CovarianceEllipsoid() :
00039         ellipse_lengths(Vector2f::Ones()),
00040         ellipse_axes(Matrix2f::Identity())
00041 {}
00042 
00043 CovarianceEllipsoid<float,2>::CovarianceEllipsoid(const ecl::linear_algebra::Matrix2f &M) {
00044         compute(M);
00045 }
00046 void CovarianceEllipsoid<float,2>::compute(const ecl::linear_algebra::Matrix2f &M) {
00047 
00048         // CHECK: symmetricity of M
00049 
00050         /*********************
00051         ** Eigenvalues
00052         **********************/
00053         float a = M(0,0);
00054         float b = M(0,1);
00055         float c = M(1,0);
00056         float d = M(1,1);
00057 
00058         float tmp = sqrtf((a+d)*(a+d)/4 - a*d + b*c);
00059     ellipse_lengths << sqrtf((a+d)/2 + tmp), sqrtf((a+d)/2 - tmp);
00060 
00061     // CHECK: all ellipse_lengths are >= 0 (positive semi-definite check)
00062     // CHECK: ordered in decreasing order of magnitude
00063 
00064     /*********************
00065         ** Eigenvectors
00066         **********************/
00067     if( c != 0 ) {
00068         ellipse_axes(0,0) = ellipse_lengths(0)*ellipse_lengths(0)-d;
00069         ellipse_axes(1,0) = c;
00070         ellipse_axes(0,1) = ellipse_lengths(1)*ellipse_lengths(1)-d;
00071         ellipse_axes(1,1) = c;
00072     } else if( b != 0 ) {
00073         ellipse_axes(0,0) = b;
00074         ellipse_axes(1,0) = ellipse_lengths(0)*ellipse_lengths(0)-a;
00075         ellipse_axes(0,1) = b;
00076         ellipse_axes(1,1) = ellipse_lengths(1)*ellipse_lengths(1)-a;
00077     }
00078     else {
00079         if ( a > d ) {
00080                         ellipse_axes << 1, 0,
00081                                                     0, 1;
00082         } else {
00083                 ellipse_axes << 0, 1,
00084                                                     1, 0;
00085         }
00086     }
00087     /*********************
00088         ** Normalise Evectors
00089         **********************/
00090     ellipse_axes.block<2,1>(0,0).normalize();
00091     ellipse_axes.block<2,1>(0,1).normalize();
00092 }
00093 
00094 double CovarianceEllipsoid<float,2>::rotation() {
00095         return atan2f(ellipse_axes(1,0), ellipse_axes(0,0));
00096 }
00097 
00098 Vector2f CovarianceEllipsoid<float,2>::intercepts() {
00099 
00100         Vector2f intercept_magnitudes;
00101         float t;
00102         float angle = rotation();
00103 
00104         /*********************
00105         ** Parametric Form
00106         **********************/
00107         //  here t is a parameter and theta is the rotation angle
00108         //
00109         //      x = lambda1 cos(t)cos(theta) - lambda2 sin(t)sin(theta)
00110         //  y = lambda1 cos(t)sin(theta) + lambda2 sin(t)cos(theta)
00111         //
00112         // To find the intersection with the x axis, set y = 0, then
00113         //      t = arctan[ - (lambda1/lambda2) tan(theta) ]
00114         // To find the intersection with the y axis, set x = 0, then
00115         //      t = arctan[ (lambda1/lambda2) cot(theta) ]
00116 
00117         /*********************
00118         ** X Axis
00119         **********************/
00120         t = atan2f( -(ellipse_lengths(0)/ellipse_lengths(1))*tanf(angle), 1.0 );
00121         intercept_magnitudes(0) = fabsf(ellipse_lengths(0)*cosf(t)*cosf(angle) - ellipse_lengths(1)*sinf(t)*sinf(angle));
00122         /*********************
00123         ** Y Axis
00124         **********************/
00125         t = atan2f( (ellipse_lengths(0)/ellipse_lengths(1)), tanf(angle) ); // Could this get dodgy with tan going to infinity?
00126         intercept_magnitudes(1) = fabsf(ellipse_lengths(0)*cosf(t)*sinf(angle) + ellipse_lengths(1)*sinf(t)*sinf(angle));
00127 
00128         return intercept_magnitudes;
00129 }
00130 
00131 /*****************************************************************************
00132 ** Implementation [CovarianceEllipsoid2d]
00133 *****************************************************************************/
00134 CovarianceEllipsoid<double,2>::CovarianceEllipsoid() :
00135         ellipse_lengths(Vector2d::Ones()),
00136         ellipse_axes(Matrix2d::Identity())
00137 {}
00138 
00139 CovarianceEllipsoid<double,2>::CovarianceEllipsoid(const ecl::linear_algebra::Matrix2d &M) {
00140         compute(M);
00141 }
00142 void CovarianceEllipsoid<double,2>::compute(const ecl::linear_algebra::Matrix2d &M) {
00143 
00144         // CHECK: symmetricity of M
00145 
00146         /*********************
00147         ** Eigenvalues
00148         **********************/
00149         double a = M(0,0);
00150         double b = M(0,1);
00151         double c = M(1,0);
00152         double d = M(1,1);
00153 
00154     double tmp = sqrt((a+d)*(a+d)/4 - a*d + b*c);
00155     ellipse_lengths << sqrt((a+d)/2 + tmp), sqrt((a+d)/2 - tmp);
00156 
00157     // CHECK: all ellipse_lengths are >= 0 (positive semi-definite check)
00158     // CHECK: ordered in decreasing order of magnitude
00159 
00160     /*********************
00161         ** Eigenvectors
00162         **********************/
00163     if( c != 0 ) {
00164         ellipse_axes(0,0) = ellipse_lengths(0)*ellipse_lengths(0)-d;
00165         ellipse_axes(1,0) = c;
00166         ellipse_axes(0,1) = ellipse_lengths(1)*ellipse_lengths(1)-d;
00167         ellipse_axes(1,1) = c;
00168     } else if( b != 0 ) {
00169         ellipse_axes(0,0) = b;
00170         ellipse_axes(1,0) = ellipse_lengths(0)*ellipse_lengths(0)-a;
00171         ellipse_axes(0,1) = b;
00172         ellipse_axes(1,1) = ellipse_lengths(1)*ellipse_lengths(1)-a;
00173     }
00174     else {
00175         if ( a > d ) {
00176                         ellipse_axes << 1, 0,
00177                                                     0, 1;
00178         } else {
00179                 ellipse_axes << 0, 1,
00180                                                     1, 0;
00181         }
00182     }
00183     /*********************
00184         ** Normalise Evectors
00185         **********************/
00186     ellipse_axes.block(0,0,2,1).normalize();
00187     ellipse_axes.block(0,1,2,1).normalize();
00188 }
00189 
00190 double CovarianceEllipsoid<double,2>::rotation() {
00191         return atan2(ellipse_axes(1,0), ellipse_axes(0,0));
00192 }
00193 
00194 Vector2d CovarianceEllipsoid<double,2>::intercepts() {
00195 
00196         Vector2d intercept_magnitudes;
00197         double t;
00198         double angle = rotation();
00199 
00200         /*********************
00201         ** Parametric Form
00202         **********************/
00203         //  here t is a parameter and theta is the rotation angle
00204         //
00205         //      x = lambda1 cos(t)cos(theta) - lambda2 sin(t)sin(theta)
00206         //  y = lambda1 cos(t)sin(theta) + lambda2 sin(t)cos(theta)
00207         //
00208         // To find the intersection with the x axis, set y = 0, then
00209         //      t = arctan[ - (lambda1/lambda2) tan(theta) ]
00210         // To find the intersection with the y axis, set x = 0, then
00211         //      t = arctan[ (lambda1/lambda2) cot(theta) ]
00212 
00213         /*********************
00214         ** X Axis
00215         **********************/
00216         t = atan2( -(ellipse_lengths(0)/ellipse_lengths(1))*tan(angle), 1.0 );
00217         intercept_magnitudes(0) = fabs(ellipse_lengths(0)*cos(t)*cos(angle) - ellipse_lengths(1)*sin(t)*sin(angle));
00218         /*********************
00219         ** Y Axis
00220         **********************/
00221         t = atan2( (ellipse_lengths(0)/ellipse_lengths(1)), tan(angle) ); // Could this get dodgy with tan going to infinity?
00222         intercept_magnitudes(1) = fabs(ellipse_lengths(0)*cos(t)*sin(angle) + ellipse_lengths(1)*sin(t)*sin(angle));
00223 
00224         return intercept_magnitudes;
00225 }
00226 
00227 
00228 
00229 /*****************************************************************************
00230 ** Implementation [CovarianceEllipsoid3f]
00231 *****************************************************************************/
00232 CovarianceEllipsoid<float,3>::CovarianceEllipsoid() :
00233         ellipse_lengths(Vector3f::Ones()),
00234         ellipse_axes(Matrix3f::Identity())
00235 {}
00236 
00237 CovarianceEllipsoid<float,3>::CovarianceEllipsoid(const ecl::linear_algebra::Matrix3f &M, const bool sort) {
00238         compute(M, sort);
00239 }
00240 void CovarianceEllipsoid<float,3>::compute(const ecl::linear_algebra::Matrix3f &M, const bool sort) {
00241 
00242         Eigen::EigenSolver<Matrix3f> esolver(M);
00243 
00244         ellipse_lengths[0] = sqrtf(esolver.pseudoEigenvalueMatrix()(0,0));
00245         ellipse_lengths[1] = sqrtf(esolver.pseudoEigenvalueMatrix()(1,1));
00246         ellipse_lengths[2] = sqrtf(esolver.pseudoEigenvalueMatrix()(2,2));
00247         ellipse_axes = esolver.pseudoEigenvectors ();
00248 
00249         if ( sort ) {
00250                 // Note that sorting of eigenvalues may end up with left-hand coordinate system.
00251                 // So here we correctly sort it so that it does end up being righ-handed and normalised.
00252                 ecl::linear_algebra::Vector3f c0 = ellipse_axes.block<3,1>(0,0);  c0.normalize();
00253                 ecl::linear_algebra::Vector3f c1 = ellipse_axes.block<3,1>(0,1);  c1.normalize();
00254                 ecl::linear_algebra::Vector3f c2 = ellipse_axes.block<3,1>(0,2);  c2.normalize();
00255                 ecl::linear_algebra::Vector3f cc = c0.cross(c1);
00256                 if (cc.dot(c2) < 0) {
00257                         ellipse_axes << c1, c0, c2;
00258                   double e = ellipse_lengths[0];  ellipse_lengths[0] = ellipse_lengths[1];  ellipse_lengths[1] = e;
00259                 } else {
00260                         ellipse_axes << c0, c1, c2;
00261                 }
00262         }
00263 }
00264 
00265 CovarianceEllipsoid<double,3>::CovarianceEllipsoid() :
00266         ellipse_lengths(Vector3d::Ones()),
00267         ellipse_axes(Matrix3d::Identity())
00268 {}
00269 
00270 CovarianceEllipsoid<double,3>::CovarianceEllipsoid(const ecl::linear_algebra::Matrix3d &M, const bool sort) {
00271         compute(M,sort);
00272 }
00273 void CovarianceEllipsoid<double,3>::compute(const ecl::linear_algebra::Matrix3d &M, const bool sort) {
00274         Eigen::EigenSolver<Matrix3d> esolver(M);
00275 
00276         ellipse_lengths[0] = sqrt(esolver.pseudoEigenvalueMatrix()(0,0));
00277         ellipse_lengths[1] = sqrt(esolver.pseudoEigenvalueMatrix()(1,1));
00278         ellipse_lengths[2] = sqrt(esolver.pseudoEigenvalueMatrix()(2,2));
00279         ellipse_axes = esolver.pseudoEigenvectors ();
00280 
00281         if ( sort ) {
00282                 // Note that sorting of eigenvalues may end up with left-hand coordinate system.
00283                 // So here we correctly sort it so that it does end up being righ-handed and normalised.
00284                 ecl::linear_algebra::Vector3d c0 = ellipse_axes.block<3,1>(0,0);  c0.normalize();
00285                 ecl::linear_algebra::Vector3d c1 = ellipse_axes.block<3,1>(0,1);  c1.normalize();
00286                 ecl::linear_algebra::Vector3d c2 = ellipse_axes.block<3,1>(0,2);  c2.normalize();
00287                 ecl::linear_algebra::Vector3d cc = c0.cross(c1);
00288                 if (cc.dot(c2) < 0) {
00289                         ellipse_axes << c1, c0, c2;
00290                   double e = ellipse_lengths[0];  ellipse_lengths[0] = ellipse_lengths[1];  ellipse_lengths[1] = e;
00291                 } else {
00292                         ellipse_axes << c0, c1, c2;
00293                 }
00294         }
00295 }
00296 
00297 
00298 } // namespace ecl
00299