public override void receiveErrorPose(SimpleErrorPose newPose) { lock (thisLock) { m_pose = UbiMeasurementUtils.ubitrackToUnity(newPose); } }
// Use this for initialization public override void utInit(SimpleFacade simpleFacade) { base.utInit(simpleFacade); switch (ubitrackEvent) { case UbitrackEventType.Pull: { m_posePull = simpleFacade.getSimplePullSinkErrorPose(patternID); m_simplePose = new SimpleErrorPose(); if (m_posePull == null) { throw new Exception("SimpleApplicationPullSinkErrorPose not found for poseID:" + patternID); } break; } case UbitrackEventType.Push: { m_poseReceiver = new UnityErrorPoseReceiver(); if (!simpleFacade.setErrorPoseCallback(patternID, m_poseReceiver)) { throw new Exception("SimpleErrorPoseReceiver could not be set for poseID:" + patternID); } break; } default: break; } }
internal static Measurement <ErrorPose> ubitrackToUnity(SimpleErrorPose ubitrackPose) { Vector3 trans = new Vector3((float)ubitrackPose.tx, (float)ubitrackPose.ty, (float)ubitrackPose.tz); Quaternion rot = new Quaternion((float)ubitrackPose.rx, (float)ubitrackPose.ry, (float)ubitrackPose.rz, (float)ubitrackPose.rw); ErrorPose data = new ErrorPose(); trans = trans * UbiToUnityScaleFactor; coordsysemChange(trans, ref data.pos); coordsysemChange(rot, ref data.rot); ubitrackCovarianceToFloatArray(ubitrackPose, ref data.covariance); return(new Measurement <ErrorPose>(data, ubitrackPose.timestamp)); }
public static void ubitrackCovarianceToFloatArray(SimpleErrorPose ubitrackPose, ref float[,] covariance) { // todo needs rhs lhs transfrom covariance[0, 0] = (float)ubitrackPose.co11; covariance[0, 1] = (float)ubitrackPose.co12; covariance[0, 2] = (float)ubitrackPose.co13; covariance[0, 3] = (float)ubitrackPose.co14; covariance[0, 4] = (float)ubitrackPose.co15; covariance[0, 5] = (float)ubitrackPose.co16; covariance[1, 0] = (float)ubitrackPose.co21; covariance[1, 1] = (float)ubitrackPose.co22; covariance[1, 2] = (float)ubitrackPose.co23; covariance[1, 3] = (float)ubitrackPose.co24; covariance[1, 4] = (float)ubitrackPose.co25; covariance[1, 5] = (float)ubitrackPose.co26; covariance[2, 0] = (float)ubitrackPose.co31; covariance[2, 1] = (float)ubitrackPose.co32; covariance[2, 2] = (float)ubitrackPose.co33; covariance[2, 3] = (float)ubitrackPose.co34; covariance[2, 4] = (float)ubitrackPose.co35; covariance[2, 5] = (float)ubitrackPose.co36; covariance[3, 0] = (float)ubitrackPose.co41; covariance[3, 1] = (float)ubitrackPose.co42; covariance[3, 2] = (float)ubitrackPose.co43; covariance[3, 3] = (float)ubitrackPose.co44; covariance[3, 4] = (float)ubitrackPose.co45; covariance[3, 5] = (float)ubitrackPose.co46; covariance[4, 0] = (float)ubitrackPose.co51; covariance[4, 1] = (float)ubitrackPose.co52; covariance[4, 2] = (float)ubitrackPose.co53; covariance[4, 3] = (float)ubitrackPose.co54; covariance[4, 4] = (float)ubitrackPose.co55; covariance[4, 5] = (float)ubitrackPose.co56; covariance[5, 0] = (float)ubitrackPose.co61; covariance[5, 1] = (float)ubitrackPose.co62; covariance[5, 2] = (float)ubitrackPose.co63; covariance[5, 3] = (float)ubitrackPose.co64; covariance[5, 4] = (float)ubitrackPose.co65; covariance[5, 5] = (float)ubitrackPose.co66; }