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 <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
00039
00040
00041
00042
00043 #define ERROR_SCALE_TEST
00044 #define ERROR_ROT_TEST
00045
00046
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 }