Trying to find bug in VAMatlab tracking implementation, no luck yet

parent ba20855d
......@@ -32,8 +32,6 @@ public:
CVAMatlabConnection();
~CVAMatlabConnection();
bool IsConnected() const;
};
#endif // INCLUDE_WATCHER_VA_MATLAB_CONNECTION
......@@ -715,7 +715,7 @@ void set_tracked_real_world_sound_receiver_head_rigid_body_index( int nlhs, mxAr
int iTrackedRigidBodyIndex = matlabGetIntegerScalar( prhs[ 1 ], "index" );
pConnection->pVAMatlabTracker->iTrackedRealWorldSoundReceiverRigidBodyIndex = iTrackedRigidBodyIndex;
pConnection->pVAMatlabTracker->iTrackedRealWorldSoundReceiverHeadRigidBodyIndex = iTrackedRigidBodyIndex;
}
REGISTER_PRIVATE_FUNCTION( set_tracked_real_world_sound_receiver_torso_rigid_body_index );
......@@ -776,11 +776,11 @@ void set_tracked_sound_source( int nlhs, mxArray *plhs[], int nrhs, const mxArra
int iSourceID = matlabGetIntegerScalar( prhs[ 1 ], "sourceID" );
pConnection->pVAMatlabTracker->iTrackedSourceID = iSourceID;
pConnection->pVAMatlabTracker->iTrackedSoundSourceID = iSourceID;
}
REGISTER_PRIVATE_FUNCTION( set_tracked_sound_source_rigid_body_index );
DECLARE_FUNCTION_REQUIRED_INARG( set_tracked_sound_source_rigid_body_index, index, "scalar number", "Tracked source rigid body index (default is 1)" );
DECLARE_FUNCTION_REQUIRED_INARG( set_tracked_sound_source_rigid_body_index, index, "scalar number", "Tracked sound source rigid body index (default is 1)" );
void set_tracked_sound_source_rigid_body_index( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
REQUIRE_INPUT_ARGS( 2 );
......@@ -790,7 +790,7 @@ void set_tracked_sound_source_rigid_body_index( int nlhs, mxArray *plhs[], int n
int iTrackedRigidBodyIndex = matlabGetIntegerScalar( prhs[ 1 ], "index" );
pConnection->pVAMatlabTracker->iTrackedSourceRigidBodyIndex = iTrackedRigidBodyIndex;
pConnection->pVAMatlabTracker->iTrackedSoundSourceRigidBodyIndex = iTrackedRigidBodyIndex;
}
REGISTER_PRIVATE_FUNCTION( set_tracked_sound_source_rigid_body_translation );
......@@ -805,22 +805,52 @@ void set_tracked_sound_source_rigid_body_translation( int nlhs, mxArray *plhs[],
VAVec3 v3Pos;
matlabGetRealVector3( prhs[ 1 ], "offset", v3Pos );
pConnection->pVAMatlabTracker->vTrackedSourceTranslation = VistaVector3D( v3Pos.comp );
pConnection->pVAMatlabTracker->vTrackedSoundSourceTranslation = VistaVector3D( v3Pos.comp );
}
REGISTER_PRIVATE_FUNCTION( set_tracked_sound_source_rigid_body_rotation );
DECLARE_FUNCTION_REQUIRED_INARG( set_tracked_sound_source_rigid_body_rotation, rotation, "real 1x4", "Tracked sound source rigid body rotation (quaternion values with w (real), i, j, k)" );
void set_tracked_sound_source_rigid_body_rotation( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
REGISTER_PRIVATE_FUNCTION(set_tracked_sound_source_rigid_body_rotation);
DECLARE_FUNCTION_REQUIRED_INARG(set_tracked_sound_source_rigid_body_rotation, rotation, "real 1x4", "Tracked sound source rigid body rotation (quaternion values with w (real), i, j, k)");
void set_tracked_sound_source_rigid_body_rotation(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
REQUIRE_INPUT_ARGS( 2 );
REQUIRE_INPUT_ARGS(2);
ConnectionHandle hHandle = GetConnectionHandle( prhs[ 0 ] );
CVAMatlabConnection* pConnection = g_vpConnections[ hHandle ];
ConnectionHandle hHandle = GetConnectionHandle(prhs[0]);
CVAMatlabConnection* pConnection = g_vpConnections[hHandle];
VAQuat qOrient;
matlabGetQuaternion( prhs[ 1 ], "rotation", qOrient );
matlabGetQuaternion(prhs[1], "rotation", qOrient);
pConnection->pVAMatlabTracker->qTrackedSoundSourceRotation = VistaQuaternion(qOrient.comp);
}
REGISTER_PRIVATE_FUNCTION(get_tracker_info);
DECLARE_FUNCTION_OUTARG(get_tracker_info, state, "struct", "Returns the current state of the tracker configuration");
void get_tracker_info(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
ConnectionHandle hHandle = GetConnectionHandle(prhs[0]);
CVAMatlabConnection* pConnection = g_vpConnections[hHandle];
CVAStruct oTrackerInfo;
oTrackerInfo["IsConnected"] = pConnection->pVAMatlabTracker->IsConnected();
// Sound receiver
oTrackerInfo["TrackedSoundReceiverID"] = pConnection->pVAMatlabTracker->iTrackedSoundReceiverID;
oTrackerInfo["TrackedSoundReceiverHeadRigidBodyIndex"] = pConnection->pVAMatlabTracker->iTrackedSoundReceiverHeadRigidBodyIndex;
oTrackerInfo["TrackedSoundReceiverTorsoRigidBodyIndex"] = pConnection->pVAMatlabTracker->iTrackedSoundReceiverTorsoRigidBodyIndex;
//oTrackerInfo["TrackedSoundReceiverTranslation"] = pConnection->pVAMatlabTracker->vTrackedSoundReceiverTranslation;
//oTrackerInfo["TrackedSoundReceiverRotation"] = pConnection->pVAMatlabTracker->qTrackedSoundReceiverRotation;
// Real world sound receiver
oTrackerInfo["TrackedRealWorldSoundReceiverID"] = pConnection->pVAMatlabTracker->iTrackedRealWorldSoundReceiverID;
oTrackerInfo["TrackedRealWorldSoundReceiverHeadRigidBodyIndex"] = pConnection->pVAMatlabTracker->iTrackedRealWorldSoundReceiverHeadRigidBodyIndex;
oTrackerInfo["TrackedRealWorldSoundReceiverTorsoRigidBodyIndex"] = pConnection->pVAMatlabTracker->iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex;
// Sound source
oTrackerInfo["TrackedSoundSourceID"] = pConnection->pVAMatlabTracker->iTrackedSoundSourceID;
oTrackerInfo["TrackedSoundSourceRigidBodyIndex"] = pConnection->pVAMatlabTracker->iTrackedSoundSourceRigidBodyIndex;
pConnection->pVAMatlabTracker->qTrackedSourceRotation = VistaQuaternion( qOrient.comp );
plhs[0] = matlabCreateStruct(oTrackerInfo);
}
......
......@@ -82,20 +82,20 @@ void TrackerDataHandler( sFrameOfMocapData* sFrame, void* pUserData )
// Real-world sound receiver
int iTrackedRealWorldSoundReceiverID = pVAMatlabTracker->iTrackedRealWorldSoundReceiverID;
int iTrackedRealWorldSoundReceiverRigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverRigidBodyIndex;
int iTrackedRealWorldSoundReceiverHATORigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex;
int iTrackedRealWorldSoundReceiverHeadRigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverHeadRigidBodyIndex;
int iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex;
if( iTrackedRealWorldSoundReceiverID != -1 )
{
// Real world receiver tracking
if( ( iTrackedRealWorldSoundReceiverRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedRealWorldSoundReceiverRigidBodyIndex > 0 ) )
if ((iTrackedRealWorldSoundReceiverHeadRigidBodyIndex <= sFrame->nRigidBodies) && (iTrackedRealWorldSoundReceiverHeadRigidBodyIndex > 0))
{
try
{
VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedRealWorldSoundReceiverTranslation;
VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedRealWorldSoundReceiverRotation;
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedRealWorldSoundReceiverRigidBodyIndex - 1 ] );
const sRigidBodyData& oBodyData(sFrame->RigidBodies[iTrackedRealWorldSoundReceiverHeadRigidBodyIndex - 1]);
VistaVector3D vPosPivotPoint( oBodyData.x, oBodyData.y, oBodyData.z );
VistaQuaternion qOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
......@@ -110,10 +110,10 @@ void TrackerDataHandler( sFrameOfMocapData* sFrame, void* pUserData )
// Real world receiver HATO orientation
if( ( iTrackedRealWorldSoundReceiverHATORigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedRealWorldSoundReceiverHATORigidBodyIndex > 0 ) )
if ((iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex <= sFrame->nRigidBodies) && (iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex > 0))
{
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedRealWorldSoundReceiverHATORigidBodyIndex - 1 ] );
const sRigidBodyData& oBodyData(sFrame->RigidBodies[iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex - 1]);
const VistaQuaternion qTorsoOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
const VistaQuaternion qTorsoOrient = qOrientRotation * qOrientRigidBody;
......@@ -130,15 +130,15 @@ void TrackerDataHandler( sFrameOfMocapData* sFrame, void* pUserData )
}
// Source
int iTrackedSourceID = pVAMatlabTracker->iTrackedSourceID;
int iTrackedSourceRigidBodyIndex = pVAMatlabTracker->iTrackedSourceRigidBodyIndex;
int iTrackedSourceID = pVAMatlabTracker->iTrackedSoundSourceID;
int iTrackedSourceRigidBodyIndex = pVAMatlabTracker->iTrackedSoundSourceRigidBodyIndex;
if( ( iTrackedSourceID != -1 ) && ( iTrackedSourceRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedSourceRigidBodyIndex > 0 ) )
{
try
{
VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedSourceTranslation;
VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedSourceRotation;
VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedSoundSourceTranslation;
VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedSoundSourceRotation;
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedSourceRigidBodyIndex - 1 ] );
VistaVector3D vPosPivotPoint( oBodyData.x, oBodyData.y, oBodyData.z );
......@@ -180,15 +180,15 @@ void CVAMatlabTracker::Reset()
qTrackedSoundReceiverRotation.SetToNeutralQuaternion();
iTrackedRealWorldSoundReceiverID = -1;
iTrackedRealWorldSoundReceiverRigidBodyIndex = 1;
iTrackedRealWorldSoundReceiverHeadRigidBodyIndex = 1;
iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex = 1;
vTrackedRealWorldSoundReceiverTranslation.SetToZeroVector();
qTrackedRealWorldSoundReceiverRotation.SetToNeutralQuaternion();
iTrackedSourceID = -1;
iTrackedSourceRigidBodyIndex = 1;
vTrackedSourceTranslation.SetToZeroVector();
qTrackedSourceRotation.SetToNeutralQuaternion();
iTrackedSoundSourceID = -1;
iTrackedSoundSourceRigidBodyIndex = 1;
vTrackedSoundSourceTranslation.SetToZeroVector();
qTrackedSoundSourceRotation.SetToNeutralQuaternion();
}
bool CVAMatlabTracker::Initialize( std::string sServerAdress, std::string sLocalAdress )
......
......@@ -44,15 +44,15 @@ public:
VistaQuaternion qTrackedSoundReceiverRotation; //!< Orientation rotation from default orientation (initial NatNet rigid body orientation)
int iTrackedRealWorldSoundReceiverID; //!< -1 if deactivated
int iTrackedRealWorldSoundReceiverRigidBodyIndex; //!< Starts with 1 (default)
int iTrackedRealWorldSoundReceiverHeadRigidBodyIndex; //!< Starts with 1 (default)
int iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex; //!< Starts with 1 (default)
VistaVector3D vTrackedRealWorldSoundReceiverTranslation; //!< Position offset from pivot point (default NatNet rigid body barycenter)
VistaQuaternion qTrackedRealWorldSoundReceiverRotation; //!< Orientation rotation from default orientation (initial NatNet rigid body orientation)
int iTrackedSourceID; //!< -1 if deactivated
int iTrackedSourceRigidBodyIndex; //!< Starts with 1 (default)
VistaVector3D vTrackedSourceTranslation; //!< Position offset from pivot point (default NatNet rigid body barycenter)
VistaQuaternion qTrackedSourceRotation; //!< Orientation rotation from default orientation (initial NatNet rigid body orientation)
int iTrackedSoundSourceID; //!< -1 if deactivated
int iTrackedSoundSourceRigidBodyIndex; //!< Starts with 1 (default)
VistaVector3D vTrackedSoundSourceTranslation; //!< Position offset from pivot point (default NatNet rigid body barycenter)
VistaQuaternion qTrackedSoundSourceRotation; //!< Orientation rotation from default orientation (initial NatNet rigid body orientation)
private:
NatNetClient* m_pTrackerClient;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment