public void OnPoseAvailable(TangoPoseData pose) { // Update the text views with Pose info. updateTextViewWith(pose); bool updateRenderer = false; if (mIsRelocalized) { if (pose.BaseFrame == TangoPoseData.CoordinateFrameAreaDescription && pose.TargetFrame == TangoPoseData.CoordinateFrameDevice) { updateRenderer = true; } } else { if (pose.BaseFrame == TangoPoseData.CoordinateFrameStartOfService && pose.TargetFrame == TangoPoseData.CoordinateFrameDevice) { updateRenderer = true; } } // Update the trajectory, model matrix, and view matrix, then // render the scene again if (updateRenderer && (mRenderer.Trajectory != null)) { float[] translation = pose.GetTranslationAsFloats(); mRenderer.Trajectory.updateTrajectory(translation); mRenderer.ModelMatCalculator.updateModelMatrix(translation, pose.GetRotationAsFloats()); mRenderer.updateViewMatrix(); mGLView.RequestRender(); } }
public void OnPoseAvailable(TangoPoseData pose) { RunOnUiThread(() => { // Log whenever Motion Tracking enters a n invalid state if (!mIsAutoRecovery && (pose.StatusCode == TangoPoseData.PoseInvalid)) { Log.Wtf(this.GetType().Name, "Invalid State"); } mDeltaTime = (float)(pose.Timestamp - mPreviousTimeStamp) * SECS_TO_MILLISECS; mPreviousTimeStamp = (float)pose.Timestamp; Log.Info(_TAG, "Delta Time is: " + mDeltaTime); count++; // Update the OpenGL renderable objects with the new Tango Pose // data if (mRenderer.Trajectory != null) { float[] translation = pose.GetTranslationAsFloats(); mRenderer.Trajectory.updateTrajectory(translation); mRenderer.ModelMatCalculator.updateModelMatrix(translation, pose.GetRotationAsFloats()); mRenderer.updateViewMatrix(); mGLView.RequestRender(); } string translationString = "[" + threeDec.format(pose.Translation[0]) + ", " + threeDec.format(pose.Translation[1]) + ", " + threeDec.format(pose.Translation[2]) + "] "; string quaternionString = "[" + threeDec.format(pose.Rotation[0]) + ", " + threeDec.format(pose.Rotation[1]) + ", " + threeDec.format(pose.Rotation[2]) + ", " + threeDec.format(pose.Rotation[3]) + "] "; // Display pose data On screen in TextViews mPoseTextView.Text = translationString; mQuatTextView.Text = quaternionString; mPoseCountTextView.Text = count.ToString(); mDeltaTextView.Text = threeDec.format(mDeltaTime); if (pose.StatusCode == TangoPoseData.PoseValid) { mPoseStatusTextView.Text = "Valid"; } else if (pose.StatusCode == TangoPoseData.PoseInvalid) { mPoseStatusTextView.Text = "Invalid"; } else if (pose.StatusCode == TangoPoseData.PoseInitializing) { mPoseStatusTextView.Text = "Initializing"; } else if (pose.StatusCode == TangoPoseData.PoseUnknown) { mPoseStatusTextView.Text = "Unknown"; } }); }
void Tango.IOnTangoUpdateListener.OnPoseAvailable(TangoPoseData pose) { ParentActivity.RunOnUiThread(() => { MotionTracking parent = (mParentActivity as MotionTracking); // Log whenever Motion Tracking enters a n invalid state if (!parent.mIsAutoRecovery && (pose.StatusCode == TangoPoseData.PoseInvalid)) { Log.Wtf(mParentActivity.GetType().Name, "Invalid State"); } parent.mDeltaTime = (float)(pose.Timestamp - parent.mPreviousTimeStamp) * SECS_TO_MILLISECS; parent.mPreviousTimeStamp = (float)pose.Timestamp; Log.Info(_TAG, "Delta Time is: " + parent.mDeltaTime); parent.count++; // Update the OpenGL renderable objects with the new Tango Pose // data float[] translation = pose.GetTranslationAsFloats(); parent.mRenderer.Trajectory.updateTrajectory(translation); parent.mRenderer.ModelMatCalculator.updateModelMatrix(translation, pose.GetRotationAsFloats()); parent.mRenderer.updateViewMatrix(); parent.mGLView.RequestRender(); DecimalFormat threeDec = new DecimalFormat("0.000"); string translationString = "[" + threeDec.format(pose.Translation[0]) + ", " + threeDec.format(pose.Translation[1]) + ", " + threeDec.format(pose.Translation[2]) + "] "; string quaternionString = "[" + threeDec.format(pose.Rotation[0]) + ", " + threeDec.format(pose.Rotation[1]) + ", " + threeDec.format(pose.Rotation[2]) + ", " + threeDec.format(pose.Rotation[3]) + "] "; // Display pose data On screen in TextViews parent.mPoseTextView.Text = translationString; parent.mQuatTextView.Text = quaternionString; parent.mPoseCountTextView.Text = parent.count.ToString(); parent.mDeltaTextView.Text = threeDec.format(parent.mDeltaTime); if (pose.StatusCode == TangoPoseData.PoseValid) { parent.mPoseStatusTextView.Text = "Valid"; } else if (pose.StatusCode == TangoPoseData.PoseInvalid) { parent.mPoseStatusTextView.Text = "Invalid"; } else if (pose.StatusCode == TangoPoseData.PoseInitializing) { parent.mPoseStatusTextView.Text = "Initializing"; } else if (pose.StatusCode == TangoPoseData.PoseUnknown) { parent.mPoseStatusTextView.Text = "Unknown"; } }); }
public void OnPoseAvailable(TangoPoseData p0) { var baseFrameType = (TangoCoordinateFrameType)p0.BaseFrame; var targetFrameType = (TangoCoordinateFrameType)p0.TargetFrame; // // Log.Debug("Tag", baseFrameType.ToString()); // Log.Debug("Tag", targetFrameType.ToString()); var translation = p0.GetTranslationAsFloats(); var orientation = p0.GetRotationAsFloats(); var position = new Vector3(translation[0], translation[1], translation[2]); var rotation = new Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]); // Log.Debug(Tag, $"{position.X}, {position.Y},{position.Z}"); // Log.Debug(Tag, $"{rotation.X}, {rotation.Y},{rotation.Z},{rotation.W}"); }
public void OnXyzIjAvailable(TangoXyzIjData xyzIj) { mCurrentTimeStamp = (float)xyzIj.Timestamp; float frameDelta = (mCurrentTimeStamp - mXyIjPreviousTimeStamp) * SECS_TO_MILLI; mXyIjPreviousTimeStamp = mCurrentTimeStamp; byte[] buffer = new byte[xyzIj.XyzCount * 3 * 4]; FileInputStream fileStream = new FileInputStream(xyzIj.XyzParcelFileDescriptor.FileDescriptor); try { fileStream.Read(buffer, xyzIj.XyzParcelFileDescriptorOffset, buffer.Length); fileStream.Close(); } catch (IOException e) { System.Diagnostics.Debug.WriteLine(e.ToString()); System.Diagnostics.Debug.WriteLine(e.StackTrace); } try { TangoPoseData pointCloudPose = mTango.GetPoseAtTime(mCurrentTimeStamp, framePairs[0]); // mRenderer.PointCloud.UpdatePoints(buffer, xyzIj.XyzCount); mRenderer.PointCloud.UpdatePoints((Java.Nio.FloatBuffer)buffer); mRenderer.ModelMatCalculator.updatePointCloudModelMatrix(pointCloudPose.GetTranslationAsFloats(), pointCloudPose.GetRotationAsFloats()); mRenderer.PointCloud.ModelMatrix = mRenderer.ModelMatCalculator.PointCloudModelMatrixCopy; } catch (TangoErrorException) { Toast.MakeText(Android.App.Application.Context, Resource.String.TangoError, Android.Widget.ToastLength.Short).Show(); } catch (TangoInvalidException) { Toast.MakeText(Android.App.Application.Context, Resource.String.TangoError, Android.Widget.ToastLength.Short).Show(); } // Must run UI changes On the UI thread. Running in the Tango // service thread // will result in an error. RunOnUiThread(() => { mPointCountTextView.Text = Convert.ToString(xyzIj.XyzCount); mFrequencyTextView.Text = "" + threeDec.format(frameDelta); mAverageZTextView.Text = "" + threeDec.format(mRenderer.PointCloud.AverageZ); }); }
public void OnPoseAvailable(TangoPoseData pose) { mDeltaTime = (float)(pose.Timestamp - mPosePreviousTimeStamp) * SECS_TO_MILLI; mPosePreviousTimeStamp = (float)pose.Timestamp; count++; mRenderer.ModelMatCalculator.updateModelMatrix(pose.GetTranslationAsFloats(), pose.GetRotationAsFloats()); mRenderer.updateViewMatrix(); mGLView.RequestRender(); // Update the UI with TangoPose information RunOnUiThread(() => { string translationString = "[" + threeDec.format(pose.Translation[0]) + ", " + threeDec.format(pose.Translation[1]) + ", " + threeDec.format(pose.Translation[2]) + "] "; string quaternionString = "[" + threeDec.format(pose.Rotation[0]) + ", " + threeDec.format(pose.Rotation[1]) + ", " + threeDec.format(pose.Rotation[2]) + ", " + threeDec.format(pose.Rotation[3]) + "] "; // Display pose data On screen in TextViews mPoseTextView.Text = translationString; mQuatTextView.Text = quaternionString; mPoseCountTextView.Text = Convert.ToString(count); mDeltaTextView.Text = threeDec.format(mDeltaTime); if (pose.StatusCode == TangoPoseData.PoseValid) { mPoseStatusTextView.Text = "Valid"; } else if (pose.StatusCode == TangoPoseData.PoseInvalid) { mPoseStatusTextView.Text = "Invalid"; } else if (pose.StatusCode == TangoPoseData.PoseInitializing) { mPoseStatusTextView.Text = "Initializing"; } else if (pose.StatusCode == TangoPoseData.PoseUnknown) { mPoseStatusTextView.Text = "Unknown"; } }); }
private void SetUpExtrinsics() { // Set device to imu matrix in Model Matrix Calculator. TangoPoseData device2IMUPose = new TangoPoseData(); TangoCoordinateFramePair framePair = new TangoCoordinateFramePair(); framePair.BaseFrame = TangoPoseData.CoordinateFrameImu; framePair.TargetFrame = TangoPoseData.CoordinateFrameDevice; try { device2IMUPose = mTango.GetPoseAtTime(0.0, framePair); } catch (TangoErrorException) { Toast.MakeText(ApplicationContext, Resource.String.TangoError, Android.Widget.ToastLength.Short).Show(); } mRenderer.ModelMatCalculator.SetDevice2IMUMatrix(device2IMUPose.GetTranslationAsFloats(), device2IMUPose.GetRotationAsFloats()); // Set color camera to imu matrix in Model Matrix Calculator. TangoPoseData color2IMUPose = new TangoPoseData(); framePair.BaseFrame = TangoPoseData.CoordinateFrameImu; framePair.TargetFrame = TangoPoseData.CoordinateFrameCameraColor; try { color2IMUPose = mTango.GetPoseAtTime(0.0, framePair); } catch (TangoErrorException) { Toast.MakeText(ApplicationContext, Resource.String.TangoError, Android.Widget.ToastLength.Short).Show(); } mRenderer.ModelMatCalculator.SetColorCamera2IMUMatrix(color2IMUPose.GetTranslationAsFloats(), color2IMUPose.GetRotationAsFloats()); }