div class="twikiTopBar">

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

ErrorPoseAbility.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  * ErrorPoseAbility.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: ErrorPoseAbility.cpp,v 1.3 2004/08/31 17:25:30 strassef Exp $
00020  */
00021 
00022 
00023 #include <debug.h>
00024 #include <dwarfutil.h>
00025 #include <ubitrack/EasyPoseData.h>
00026 #include <ErrorPoseAbility.h>
00027 #include <algorithm>
00028 #include <TimeUtil.h>
00029 #include <sstream>
00030 
00031 using TNT::matmult;
00032 using std::string;
00033 using std::auto_ptr;
00034 using namespace DWARF;
00035 
00036 typedef TNT::Array2D<double> Matrix;
00037 
00038 // If this is defined, the ellipse will be controlled by an additional
00039 // posedata ability. This gives smoother movements, but requires a working
00040 // UMA or manual setup of inference components
00041 //#define USE_POSEDATA_ABILITY
00042 
00043 #define ERROR_SCALE_TEST
00044 #define ERROR_ROT_TEST
00045 
00046 // TNT is crap. There is no matrix transpose.
00047 static Matrix transpose( const Matrix& M )
00048         {
00049         int n = M.dim1();
00050         int m = M.dim2();
00051 
00052         Matrix Mt( m, n );
00053 
00054         for ( int i = 0; i < n; i++ )
00055                 for ( int j = 0; j < m; j++ )
00056                         Mt[j][i] = M[i][j];
00057 
00058         return Mt;
00059         }
00060 
00061 const char* pszVRMLEllipse =
00062         "#VRML V2.0 utf8\n" \
00063         "Transform {\n" \
00064                 "scale 1.0 1.0 1.0\n" \
00065                 "children Shape {\n" \
00066                         "appearance Appearance { material Material { diffuseColor 0.9 0.9 0.1 }}\n" \
00067                         "geometry Sphere { radius 1.0 }\n" \
00068                 "}\n" \
00069         "} " ;
00070 
00071 const char* pszVRMLCube =
00072         "#VRML V2.0 utf8\n" \
00073         \
00074         "PROTO Arr [\n" \
00075                 "field SFColor color 0 0 0\n" \
00076         "] {\n" \
00077                 "Group {\n" \
00078                         "children [\n" \
00079                                 "Shape {\n" \
00080                                         "appearance DEF APP Appearance {\n" \
00081                                                 "material Material {\n" \
00082                                                         "diffuseColor IS color\n" \
00083                                                 "}\n" \
00084                                         "}\n" \
00085                                         "geometry Cylinder {\n" \
00086                                                 "radius 0.02\n" \
00087                                                 "height 1\n" \
00088                                         "}\n" \
00089                                 "}\n" \
00090                                 "Transform {\n" \
00091                                         "translation 0 0.5 0\n" \
00092                                         "children [\n" \
00093                                                 "Shape {\n" \
00094                                                         "appearance USE APP\n" \
00095                                                         "geometry Cone {\n" \
00096                                                                 "height 0.1\n" \
00097                                                                 "bottomRadius 0.04\n" \
00098                                                         "}\n" \
00099                                                 "}\n" \
00100                                         "]\n" \
00101                                 "}\n" \
00102                         "]\n" \
00103                 "}\n" \
00104         "}\n" \
00105         \
00106         "Transform {\n" \
00107         "scale 0.2 0.2 0.2\n" \
00108         "children [\n" \
00109                 "Group {\n" \
00110                         "children [\n" \
00111                                 "DEF ARROW Transform {\n" \
00112                                         "translation 0 0.5 0\n" \
00113                                         "children Arr {color 0 1 0}\n" \
00114                                 "}\n" \
00115                                 "Transform {\n" \
00116                                         "translation 0 0 0.5\n" \
00117                                         "rotation 1 0 0 1.6\n" \
00118                                         "children Arr {color 0 0 1}\n" \
00119                                 "}\n" \
00120                                 "Transform {\n" \
00121                                         "translation 0.5 0 0\n" \
00122                                         "rotation 0 0 1 -1.6\n" \
00123                                         "children Arr {color 1 0 0}\n" \
00124                                 "}\n" \
00125                         "]\n" \
00126                 "}\n" \
00127         "]\n" \
00128         "}\n";
00129 
00130 
00131 ErrorPoseAbility::ErrorPoseAbility( const char* pszSource, const char* pszTarget,
00132         DWARF::ActiveServiceDescription_ptr pServiceDesc )
00133         {
00134         // create name of target viewer object
00135         m_sTarget = pszTarget;
00136         string sAbilityName = string( pszSource ) + "2" + pszTarget;
00137         m_sAbilityTarget = "Error-" + sAbilityName;
00138 
00139 #ifdef USE_POSEDATA_ABILITY
00140         try
00141                 {
00142                 // create
00143                 DEBUGSTREAM( 10, "Creating new ability " << sAbilityName );
00144                 AbilityDescription_var pAbilDesc = pServiceDesc->newAbility( sAbilityName.c_str() );
00145                 pAbilDesc->setType( "PoseData" );
00146                 pAbilDesc->setAttribute( "UTSource", pszSource );
00147                 pAbilDesc->setAttribute( "UTTarget", m_sAbilityTarget.c_str() );
00148 
00149                 ConnectorDescription_var pConnDesc = pAbilDesc->newConnector( "PushSupplier" );
00150 
00151                 // create and register a PoseSender for this ability
00152                 m_pSender = auto_ptr<PoseSender>( new PoseSender( pAbilDesc ) );
00153                 pServiceDesc->registerAbility( sAbilityName.c_str(), m_pSender->_this() );
00154                 }
00155         catch( CORBA::Exception& )
00156                 {
00157                 DEBUGSTREAM( 5, "CORBA::Exception when creating ability" );
00158                 }
00159 #endif
00160         }
00161 
00162 
00163 void ErrorPoseAbility::SetViewer( ViewerControl_ptr pViewer )
00164         {
00165         // duplicate CORBA pointer and store result
00166         m_pViewer = ViewerControl::_duplicate( pViewer );
00167 
00168         // create a new "Cube" in the viewer
00169         DEBUGSTREAM( 10, "Creating new object " << m_sTarget << " in viewer" );
00170         if ( !m_pViewer->createConnectedObject( m_sTarget.c_str(), "ROOT", pszVRMLCube ) )
00171                 DEBUGSTREAM( 10, "createConnectedObject failed!" );
00172 
00173         // create position ellipse in the viewer
00174         DEBUGSTREAM( 10, "Creating new object " << m_sAbilityTarget << " in viewer" );
00175 #ifndef USE_POSEDATA_ABILITY
00176         if ( !m_pViewer->createObject( m_sAbilityTarget.c_str(), m_sTarget.c_str(), pszVRMLEllipse ) )
00177                 DEBUGSTREAM( 10, "createObject failed!" );
00178 #else
00179         if ( !m_pViewer->createConnectedObject( m_sAbilityTarget.c_str(), "ROOT", pszVRMLEllipse ) )
00180                 DEBUGSTREAM( 10, "createConnectedObject failed!" );
00181 #endif
00182 
00183         // create x-orientation ellipse in the viewer
00184         DEBUGSTREAM( 10, "Creating new object " << m_sAbilityTarget << "-RotX in viewer" );
00185         if ( !m_pViewer->createObject( ( m_sAbilityTarget + "-RotX" ).c_str(),
00186                 m_sTarget.c_str(), pszVRMLEllipse ) )
00187                 { DEBUGSTREAM( 10, "createObject failed!" ); }
00188         else
00189                 m_pViewer->setProperty( ( m_sAbilityTarget + "-RotX" ).c_str(),
00190                         "transform { translation 0.2 0 0 }" );
00191 
00192         // create y-orientation ellipse in the viewer
00193         DEBUGSTREAM( 10, "Creating new object " << m_sAbilityTarget << "-RotY in viewer" );
00194         if ( !m_pViewer->createObject( ( m_sAbilityTarget + "-RotY" ).c_str(),
00195                 m_sTarget.c_str(), pszVRMLEllipse ) )
00196                 { DEBUGSTREAM( 10, "createObject failed!" ); }
00197         else
00198                 m_pViewer->setProperty( ( m_sAbilityTarget + "-RotY" ).c_str(),
00199                         "transform { translation 0 0.2 0 }" );
00200 
00201         // create z-orientation ellipse in the viewer
00202         DEBUGSTREAM( 10, "Creating new object " << m_sAbilityTarget << "-RotZ in viewer" );
00203         if ( !m_pViewer->createObject( ( m_sAbilityTarget + "-RotZ" ).c_str(),
00204                 m_sTarget.c_str(), pszVRMLEllipse ) )
00205                 { DEBUGSTREAM( 10, "createObject failed!" ); }
00206         else
00207                 m_pViewer->setProperty( ( m_sAbilityTarget + "-RotZ" ).c_str(),
00208                         "transform { translation 0 0 0.2 }" );
00209         }
00210 
00211 
00212 TNT::Array2D<double> ErrorPoseAbility::CreateCovRot( const double* pVec,
00213         const TNT::Array2D<double>& RotCov )
00214         {
00215         Matrix V( 3, 3 );
00216         V[0][0] =  0.0;            V[0][1] =  2.0 * pVec[2];  V[0][2] = -2.0 * pVec[1];
00217         V[1][0] = -2.0 * pVec[2];  V[1][1] =  0.0;            V[1][2] =  2.0 * pVec[0];
00218         V[2][0] =  2.0 * pVec[1];  V[2][1] = -2.0 * pVec[0];  V[2][2] =  0.0;
00219 
00220         return matmult( matmult( V, RotCov ), transpose( V ) );
00221         }
00222 
00223 
00224 void ErrorPoseAbility::UpdateViewerRot( CovarianceEllipsoid& E, const char* pszObjName )
00225         {
00226         // get scaling & orientation
00227         const TNT::Array1D<double>& Scale = E.GetScaling();
00228 
00229         quaternion QRot;
00230         E.GetRotationQuaternion( QRot );
00231 
00232         // convert to VRML rotation
00233         double VRMLRot[ 4 ];
00234         Util::quaternionToVrml( QRot, VRMLRot );
00235 
00236         // format scale string for viewer
00237         std::ostringstream sScale;
00238         sScale << "transform {" << std::endl;
00239         sScale << "scaleFactor " << Scale[0] << " " << Scale[1] << " " << Scale[2] << std::endl;
00240         sScale << "rotation " << VRMLRot[ 0 ] << " " << VRMLRot[ 1 ] << " " << VRMLRot[ 2 ] << " " <<
00241                  VRMLRot[ 3 ] << std::endl;
00242         sScale << "}";
00243 
00244         // scale ellipsoid in viewer
00245         try
00246                 {
00247                 DEBUGSTREAM( 20, "Sending property of " << pszObjName << " to viewer: " << sScale.str() );
00248                 m_pViewer->setProperty( pszObjName, sScale.str().c_str() );
00249                 }
00250         catch( CORBA::Exception& )
00251                 {
00252                 DEBUGSTREAM( 10, "Caught a CORBA::Exception trying to call ViewerControl::setProperty!" );
00253                 }
00254         }
00255 
00256 
00257 void ErrorPoseAbility::TransformAndSendEvent( const PoseData& Pose )
00258         {
00259         // convert PoseData covariance to TNT matrix (no copying!)
00260         const Matrix C( 6, 6, const_cast<double*>( Pose.covariance[ 0 ] ) );
00261 
00262         // extract diagonal submatrices so the rotation/position eigenvalues won't get mixed up
00263         // TNT is crap -- submatrix would be really useful here, but it has bugs
00264         Matrix Cpos( 3, 3 );
00265         for ( int i = 0; i < 3; i++ )
00266                 for ( int j = 0; j < 3; j++ )
00267                         Cpos[i][j] = C[i][j];
00268 
00269         Matrix Crot( 3, 3 );
00270         for ( int i = 0; i < 3; i++ )
00271                 for ( int j = 0; j < 3; j++ )
00272                         Crot[i][j] = C[i+3][j+3];
00273 
00274         DEBUGSTREAM( 40, "Cpos = " << Cpos );
00275         DEBUGSTREAM( 40, "Crot = " << Crot );
00276 
00277 #ifdef ERROR_SCALE_TEST
00278         // scale x axis
00279         Crot[0][0] *= 9;
00280         Crot[2][2] /= 9;
00281 #endif
00282 
00283 #ifdef ERROR_ROT_TEST
00284         // rotate covariance around the quaternion part of the pose
00285         Matrix Rhom( 4, 4 );
00286         double TestQuat[ 4 ] =  { 0.4, 0.2, 0.4, 0.9 };
00287         DWARF::Util::quaternionToMatrix( Util::normalizeQuaternion( TestQuat ), Rhom[0] );
00288 //      DWARF::Util::quaternionToMatrix( Pose.orientation, Rhom[0] );
00289         Matrix R( 3, 3 );
00290         for ( int i = 0; i < 3; i++ )
00291                 for ( int j = 0; j < 3; j++ )
00292                         R[i][j] = Rhom[i][j];
00293         Crot = matmult( matmult( transpose( R ), Crot ), R );
00294 #endif
00295 
00296         // compute eigenvectors and -values
00297         m_PosEllipsoid.SetCovariance( Cpos );
00298         const TNT::Array1D<double>& Scale = m_PosEllipsoid.GetScaling();
00299 
00300         // format scale string for viewer
00301         std::ostringstream sScale;
00302         sScale << "transform {" << std::endl;
00303         sScale << "scaleFactor " << Scale[0] << " " << Scale[1] << " " << Scale[2] << std::endl;
00304 
00305         // compute orientation
00306         quaternion QCovRot;
00307         m_PosEllipsoid.GetRotationQuaternion( QCovRot );
00308 
00309 #ifndef USE_POSEDATA_ABILITY
00310         // multiply orientation with inverse of pose orientation to transform covariance ellipse
00311         // from source to target coordinate system
00312         quaternion QPoseRotInv;
00313         for ( int i = 0; i < 4; i++ )
00314                 QPoseRotInv[ i ] = Pose.orientation[ i ];
00315         Util::invertQuaternion( QPoseRotInv );
00316 
00317         quaternion QRot;
00318         Util::multiplyQuaternion( QRot, QPoseRotInv, QCovRot );
00319 
00320         // convert to VRML rotation
00321         double VRMLRot[ 4 ];
00322         Util::quaternionToVrml( QRot, VRMLRot );
00323 
00324         sScale << "rotation " << VRMLRot[ 0 ] << " " << VRMLRot[ 1 ] << " " << VRMLRot[ 2 ] << " " <<
00325                  VRMLRot[ 3 ] << std::endl;
00326 #endif
00327 
00328         sScale << "}";
00329 
00330         // scale ellipsoid in viewer
00331         try
00332                 {
00333                 DEBUGSTREAM( 20, "Sending to viewer: " << sScale.str() );
00334                 m_pViewer->setProperty( m_sAbilityTarget.c_str(), sScale.str().c_str() );
00335                 }
00336         catch( CORBA::Exception& )
00337                 {
00338                 DEBUGSTREAM( 10, "Caught a CORBA::Exception trying to call ViewerControl::setProperty!" );
00339                 }
00340 
00341 #ifdef USE_POSEDATA_ABILITY
00342         // create new pose for error
00343         EasyPoseData ErrorPose( Pose );
00344         ErrorPose.setOrientation( QCovRot );
00345         ErrorPose.setTarget( m_sAbilityTarget );
00346 
00347         // send the new pose to the viewer
00348         m_pSender->sendPoseData( ErrorPose );
00349 #endif
00350 
00351         // scale rotation ellipsoids
00352         const double fAxisX[ 3 ] = { 0.2, 0.0, 0.0 };
00353         Matrix CRotX = CreateCovRot( fAxisX, Crot );
00354         m_OriEllipsoidX.SetCovariance( CRotX );
00355         UpdateViewerRot( m_OriEllipsoidX, ( m_sAbilityTarget + "-RotX" ).c_str() );
00356 
00357         const double fAxisY[ 3 ] = { 0.0, 0.2, 0.0 };
00358         Matrix CRotY = CreateCovRot( fAxisY, Crot );
00359         m_OriEllipsoidY.SetCovariance( CRotY );
00360         UpdateViewerRot( m_OriEllipsoidY, ( m_sAbilityTarget + "-RotY" ).c_str() );
00361 
00362         const double fAxisZ[ 3 ] = { 0.0, 0.0, 0.2 };
00363         Matrix CRotZ = CreateCovRot( fAxisZ, Crot );
00364         m_OriEllipsoidZ.SetCovariance( CRotZ );
00365         UpdateViewerRot( m_OriEllipsoidZ, ( m_sAbilityTarget + "-RotZ" ).c_str() );
00366         }

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