div class="twikiTopBar">

Chair for Computer Aided Medical Procedures & Augmented Reality
Lehrstuhl für Informatikanwendungen in der Medizin & Augmented Reality

CovarianceEllipsoid.cpp

00001 /* ========================================================================
00002  * Copyright (C) 2000-2004  Technische Universitaet Muenchen
00003  * License: GPL v2 (or later)
00004  *
00005  * For further information please contact the DWARF team at
00006  * <dwarf-gnu@augmentedreality.de>
00007  *
00008  * CovarianceEllipsoid.cpp
00009  * Daniel Pustka <pustka@in.tum.de> April 2004
00010  * Distributed Wearable Augmented Reality Framework - www.augmentedreality.de
00011  * For documentation and instructions, see the DWARF documentation website at
00012  * http://wwwbruegge.in.tum.de/projects/dwarf/doc/
00013  *
00014  * For further questions about DWARF, please contact us at
00015  * <dwarf@augmentedreality.de>
00016  * For questions about this particular service, contact
00017  * Daniel Pustka <pustka@in.tum.de>
00018  *
00019  * $Id: CovarianceEllipsoid.cpp,v 1.1 2004/05/09 11:36:52 pustka Exp $
00020  */
00021 
00022 
00023 #include <debug.h>
00024 #include <dwarfutil.h>
00025 #include <CovarianceEllipsoid.h>
00026 
00027 // TNT is crap -- this time it really sucks -- jama_eig.h includes: "using namespace TNT"
00028 namespace TNT_sucks
00029         {
00030         #include <jama_eig.h>
00031         }
00032 namespace JAMA
00033         { using TNT_sucks::JAMA::Eigenvalue; }
00034 
00035 using TNT::matmult;
00036 using namespace DWARF;
00037 
00038 typedef TNT::Array2D<double> Matrix;
00039 
00040 
00041 CovarianceEllipsoid::CovarianceEllipsoid()
00042         : m_Eigenvectors( 3, 3 )
00043         {
00044         // initialize m_Eigenvectors to identity matrix
00045         m_Eigenvectors = 0.0;
00046         for ( int i = 0; i < 3; i++ )
00047                 m_Eigenvectors[i][i] = 1.0;
00048         }
00049 
00050 
00051 
00052 void CovarianceEllipsoid::SetCovariance( const TNT::Array2D<double>& C )
00053         {
00054         DEBUGSTREAM( 30, "Covariance matrix: " << C );
00055 
00056         // compute eigenvalues and eigenvectors of covariance matrix
00057         JAMA::Eigenvalue<double> EigenDecomposition( C );
00058 
00059         EigenDecomposition.getRealEigenvalues( m_Scale );
00060         DEBUGSTREAM( 40, "Eigenvalues: " << m_Scale );
00061 
00062         Matrix Eigenvectors;
00063         EigenDecomposition.getV( Eigenvectors );
00064         DEBUGSTREAM( 40, "Eigenvectors: " << Eigenvectors );
00065 
00066         // sort and if necessary invert eigenvectors to match order of last decomposition.
00067         // This avoids jumps between the time of the setProperty CORBA method invocation
00068         // and the arrival of the PoseData event at the viewer
00069 
00070         // do a min-sort kind of thing
00071         for ( int j = 0; j < 2; j++ )
00072                 {
00073                 int iBestFit = 0;
00074                 double fBestFitDistance = 1000;
00075                 double fInvert = 1.0;
00076                 double fDist;
00077 
00078                 for ( int i = j; i < 3; i++ )
00079                         {
00080                         // compute distance to vector
00081                         fDist = 0;
00082                         for ( int k = 0; k < 3; k++ )
00083                                 fDist += ( Eigenvectors[k][i] - m_Eigenvectors[k][j] ) * ( Eigenvectors[k][i] - m_Eigenvectors[k][j] );
00084 
00085                         if ( fDist < fBestFitDistance )
00086                                 {
00087                                 fBestFitDistance = fDist;
00088                                 iBestFit = i;
00089                                 fInvert = 1.0;
00090                                 }
00091 
00092                         // compute distance to inverted vector
00093                         fDist = 0;
00094                         for ( int k = 0; k < 3; k++ )
00095                                 fDist += ( Eigenvectors[k][i] + m_Eigenvectors[k][j] ) * ( Eigenvectors[k][i] + m_Eigenvectors[k][j] );
00096 
00097                         if ( fDist < fBestFitDistance )
00098                                 {
00099                                 fBestFitDistance = fDist;
00100                                 iBestFit = i;
00101                                 fInvert = -1.0;
00102                                 }
00103                         }
00104 
00105                 // swap vectors
00106                 if ( iBestFit != j || fInvert < 0.0 )
00107                         {
00108                         for ( int k = 0; k < 3; k++ )
00109                                 {
00110                                 double h = Eigenvectors[ k ][ iBestFit ] * fInvert;
00111                                 Eigenvectors[ k ][ iBestFit ] = Eigenvectors[ k ][ j ];
00112                                 Eigenvectors[ k ][ j ] = h;
00113                                 }
00114 
00115                         // swap eigenvalues
00116                         double h = m_Scale[ iBestFit ];
00117                         m_Scale[ iBestFit ] = m_Scale[ j ];
00118                         m_Scale[ j ] = h;
00119                         }
00120                 }
00121 
00122         // to assure right-handed coordinate system, compute cross-product of first two eigenvectors
00123         Eigenvectors[ 0 ][ 2 ] = Eigenvectors[ 1 ][ 0 ] * Eigenvectors[ 2 ][ 1 ] - Eigenvectors[ 2 ][ 0 ] * Eigenvectors[ 1 ][ 1 ];
00124         Eigenvectors[ 1 ][ 2 ] = Eigenvectors[ 2 ][ 0 ] * Eigenvectors[ 0 ][ 1 ] - Eigenvectors[ 0 ][ 0 ] * Eigenvectors[ 2 ][ 1 ];
00125         Eigenvectors[ 2 ][ 2 ] = Eigenvectors[ 0 ][ 0 ] * Eigenvectors[ 1 ][ 1 ] - Eigenvectors[ 1 ][ 0 ] * Eigenvectors[ 0 ][ 1 ];
00126 
00127         m_Eigenvectors = Eigenvectors;
00128 
00129         // compute sigmas
00130         double fCovVisFact = 3;
00131         for ( int i = 0; i < 3; i++ )
00132                 m_Scale[ i ] = sqrt( m_Scale[ i ] ) * fCovVisFact;
00133         }
00134 
00135 
00136 void CovarianceEllipsoid::GetRotationQuaternion( double* Q ) const
00137         {
00138         // normalize and copy eigenvectors to 4*4 matrix for matrixToQuaternion
00139         Matrix HomRot( 4, 4 );
00140         HomRot = 0.0;
00141         HomRot[3][3] = 1.0;
00142         for ( int j = 0; j < 3; j++ )
00143                 {
00144                 // compute length of column vector
00145                 double fLen = 0.0;
00146                 for ( int i = 0; i < 3; i++ )
00147                         fLen += m_Eigenvectors[i][j] * m_Eigenvectors[i][j];
00148                 fLen = sqrt( fLen );
00149 
00150                 // copy and normalize -- damn Q_UNITCHECK!
00151                 for ( int i = 0; i < 3; i++ )
00152                         HomRot[i][j] = m_Eigenvectors[i][j] / fLen;
00153                 }
00154 
00155         DEBUGSTREAM( 30, "Rotation matrix: " << HomRot );
00156         Util::matrixToQuaternion( HomRot[0], Q );
00157         DEBUGSTREAM( 20, "Rotation quaternion: (" << Q[ 0 ] << ", " << Q[ 1 ] << ", " <<
00158                 Q[ 2 ] << ", " << Q[ 3 ] << ")" );
00159         }

CovarianceEllipsoid.cpp Source File | generated on Sun Apr 29 02:00:57 2007 by Doxygen 1.4.1 for DWARF