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());
}
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 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 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 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";
}
});
}
