00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <debug.h>
00024 #include <dwarfutil.h>
00025 #include <CovarianceEllipsoid.h>
00026
00027
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
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
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
00067
00068
00069
00070
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
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
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
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
00116 double h = m_Scale[ iBestFit ];
00117 m_Scale[ iBestFit ] = m_Scale[ j ];
00118 m_Scale[ j ] = h;
00119 }
00120 }
00121
00122
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
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
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
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
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 }