Java tutorial
/* * Copyright 2014 Google Inc. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ package com.projecttango.experiments.javapointcloud; import android.content.Context; import com.google.atap.tangoservice.Tango; import com.google.atap.tangoservice.Tango.OnTangoUpdateListener; import com.google.atap.tangoservice.TangoConfig; import com.google.atap.tangoservice.TangoCoordinateFramePair; import com.google.atap.tangoservice.TangoErrorException; import com.google.atap.tangoservice.TangoEvent; import com.google.atap.tangoservice.TangoInvalidException; import com.google.atap.tangoservice.TangoOutOfDateException; import com.google.atap.tangoservice.TangoPoseData; import com.google.atap.tangoservice.TangoXyzIjData; import android.app.Activity; import android.content.Intent; import android.content.pm.PackageInfo; import android.content.pm.PackageManager.NameNotFoundException; import android.opengl.GLSurfaceView; import android.os.Bundle; import android.os.Environment; import android.util.Log; import android.view.MotionEvent; import android.view.View; import android.view.View.OnClickListener; import android.os.ParcelFileDescriptor; import android.widget.Button; import android.widget.TextView; import android.widget.Toast; import java.io.File; import java.io.FileWriter; import java.io.BufferedWriter; import java.io.PrintWriter; import java.io.FileInputStream; import java.io.FileOutputStream; import java.io.FileDescriptor; import java.io.IOException; import java.nio.ByteBuffer; import java.nio.ByteOrder; import java.nio.FloatBuffer; import java.text.DecimalFormat; import java.util.ArrayList; import org.apache.commons.io.IOUtils; /** * Main Activity class for the Point Cloud Sample. Handles the connection to the * {@link Tango} service and propagation of Tango XyzIj data to OpenGL and * Layout views. OpenGL rendering logic is delegated to the {@link PCrenderer} * class. */ public class PointCloudActivity extends Activity implements OnClickListener { private static final String TAG = PointCloudActivity.class.getSimpleName(); private static final int SECS_TO_MILLISECS = 1000; private Tango mTango; private TangoConfig mConfig; private PCRenderer mRenderer; private GLSurfaceView mGLView; private TextView mDeltaTextView; private TextView mPoseCountTextView; private TextView mPoseTextView; private TextView mQuatTextView; private TextView mPoseStatusTextView; private TextView mTangoEventTextView; private TextView mPointCountTextView; private TextView mTangoServiceVersionTextView; private TextView mApplicationVersionTextView; private TextView mAverageZTextView; private TextView mFrequencyTextView; private Button mFirstPersonButton; private Button mThirdPersonButton; private Button mTopDownButton; private File outFile; private int count; private int mPreviousPoseStatus; private float mDeltaTime; private float mPosePreviousTimeStamp; private float mXyIjPreviousTimeStamp; private float mCurrentTimeStamp; private String mServiceVersion; private boolean mIsTangoServiceConnected; @Override protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); setContentView(R.layout.activity_jpoint_cloud); setTitle(R.string.app_name); outFile = new File(Environment.getExternalStoragePublicDirectory(Environment.DIRECTORY_DOWNLOADS), "shoopdawoop.txt"); if (!outFile.exists()) try { outFile.createNewFile(); } catch (IOException e1) { throw new AssertionError(); // TODO Auto-generated catch block } mPoseTextView = (TextView) findViewById(R.id.pose); mQuatTextView = (TextView) findViewById(R.id.quat); mPoseCountTextView = (TextView) findViewById(R.id.posecount); mDeltaTextView = (TextView) findViewById(R.id.deltatime); mTangoEventTextView = (TextView) findViewById(R.id.tangoevent); mPoseStatusTextView = (TextView) findViewById(R.id.status); mPointCountTextView = (TextView) findViewById(R.id.pointCount); mTangoServiceVersionTextView = (TextView) findViewById(R.id.version); mApplicationVersionTextView = (TextView) findViewById(R.id.appversion); mAverageZTextView = (TextView) findViewById(R.id.averageZ); mFrequencyTextView = (TextView) findViewById(R.id.frameDelta); mFirstPersonButton = (Button) findViewById(R.id.first_person_button); mFirstPersonButton.setOnClickListener(this); mThirdPersonButton = (Button) findViewById(R.id.third_person_button); mThirdPersonButton.setOnClickListener(this); mTopDownButton = (Button) findViewById(R.id.top_down_button); mTopDownButton.setOnClickListener(this); mTango = new Tango(this); Log.d(TAG, "Tango constructed on activity context"); mConfig = new TangoConfig(); mConfig = mTango.getConfig(TangoConfig.CONFIG_TYPE_CURRENT); mConfig.putBoolean(TangoConfig.KEY_BOOLEAN_DEPTH, true); int maxDepthPoints = mConfig.getInt("max_point_cloud_elements"); mRenderer = new PCRenderer(maxDepthPoints); mGLView = (GLSurfaceView) findViewById(R.id.gl_surface_view); mGLView.setEGLContextClientVersion(2); mGLView.setRenderer(mRenderer); mGLView.setRenderMode(GLSurfaceView.RENDERMODE_WHEN_DIRTY); PackageInfo packageInfo; try { packageInfo = this.getPackageManager().getPackageInfo(this.getPackageName(), 0); mApplicationVersionTextView.setText(packageInfo.versionName); } catch (NameNotFoundException e) { e.printStackTrace(); } // Display the version of Tango Service mServiceVersion = mConfig.getString("tango_service_library_version"); mTangoServiceVersionTextView.setText(mServiceVersion); mIsTangoServiceConnected = false; } @Override protected void onPause() { super.onPause(); try { mTango.disconnect(); mIsTangoServiceConnected = false; } catch (TangoErrorException e) { Toast.makeText(getApplicationContext(), R.string.TangoError, Toast.LENGTH_SHORT).show(); } } @Override protected void onResume() { super.onResume(); if (!mIsTangoServiceConnected) { startActivityForResult(Tango.getRequestPermissionIntent(Tango.PERMISSIONTYPE_MOTION_TRACKING), Tango.TANGO_INTENT_ACTIVITYCODE); } Log.d(TAG, "onResumed"); } @Override protected void onActivityResult(int requestCode, int resultCode, Intent data) { // Check which request we're responding to if (requestCode == Tango.TANGO_INTENT_ACTIVITYCODE) { Log.d(TAG, "Triggered"); // Make sure the request was successful if (resultCode == RESULT_CANCELED) { Toast.makeText(this, R.string.motiontrackingpermission, Toast.LENGTH_LONG).show(); finish(); return; } try { setTangoListeners(); Log.d(TAG, "setTangoListeners"); } catch (TangoErrorException e) { Toast.makeText(this, R.string.TangoError, Toast.LENGTH_SHORT).show(); } catch (SecurityException e) { Toast.makeText(getApplicationContext(), R.string.motiontrackingpermission, Toast.LENGTH_SHORT) .show(); } try { mTango.connect(mConfig); Log.d(TAG, "Tango connected"); mIsTangoServiceConnected = true; } catch (TangoOutOfDateException e) { Toast.makeText(getApplicationContext(), R.string.TangoOutOfDateException, Toast.LENGTH_SHORT) .show(); } catch (TangoErrorException e) { Toast.makeText(getApplicationContext(), R.string.TangoError, Toast.LENGTH_SHORT).show(); } setUpExtrinsics(); Log.d(TAG, "Extrinsics set up"); } } @Override protected void onDestroy() { super.onDestroy(); } @Override public void onClick(View v) { switch (v.getId()) { case R.id.first_person_button: mRenderer.setFirstPersonView(); break; case R.id.third_person_button: mRenderer.setThirdPersonView(); break; case R.id.top_down_button: mRenderer.setTopDownView(); break; default: Log.w(TAG, "Unrecognized button click."); return; } } @Override public boolean onTouchEvent(MotionEvent event) { return mRenderer.onTouchEvent(event); } private void setUpExtrinsics() { // Set device to imu matrix in Model Matrix Calculator. TangoPoseData device2IMUPose = new TangoPoseData(); TangoCoordinateFramePair framePair = new TangoCoordinateFramePair(); framePair.baseFrame = TangoPoseData.COORDINATE_FRAME_IMU; framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_DEVICE; try { device2IMUPose = mTango.getPoseAtTime(0.0, framePair); Log.d(TAG, "Got init pose"); } catch (TangoErrorException e) { Toast.makeText(getApplicationContext(), R.string.TangoError, Toast.LENGTH_SHORT).show(); } mRenderer.getModelMatCalculator().SetDevice2IMUMatrix(device2IMUPose.getTranslationAsFloats(), device2IMUPose.getRotationAsFloats()); // Set color camera to imu matrix in Model Matrix Calculator. TangoPoseData color2IMUPose = new TangoPoseData(); framePair.baseFrame = TangoPoseData.COORDINATE_FRAME_IMU; framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR; try { color2IMUPose = mTango.getPoseAtTime(0.0, framePair); } catch (TangoErrorException e) { Toast.makeText(getApplicationContext(), R.string.TangoError, Toast.LENGTH_SHORT).show(); } mRenderer.getModelMatCalculator().SetColorCamera2IMUMatrix(color2IMUPose.getTranslationAsFloats(), color2IMUPose.getRotationAsFloats()); } private void setTangoListeners() { // Configure the Tango coordinate frame pair final ArrayList<TangoCoordinateFramePair> framePairs = new ArrayList<TangoCoordinateFramePair>(); framePairs.add(new TangoCoordinateFramePair(TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, TangoPoseData.COORDINATE_FRAME_DEVICE)); // Listen for new Tango data mTango.connectListener(framePairs, new OnTangoUpdateListener() { @Override public void onPoseAvailable(final TangoPoseData pose) { mDeltaTime = (float) (pose.timestamp - mPosePreviousTimeStamp) * SECS_TO_MILLISECS; mPosePreviousTimeStamp = (float) pose.timestamp; if (mPreviousPoseStatus != pose.statusCode) { count = 0; } count++; mPreviousPoseStatus = pose.statusCode; mRenderer.getModelMatCalculator().updateModelMatrix(pose.getTranslationAsFloats(), pose.getRotationAsFloats()); mRenderer.updateViewMatrix(); mGLView.requestRender(); // Translation and Quaternion strings defined here. Can this be accessed later on xyzji availible? final DecimalFormat threeDec = new DecimalFormat("0.000"); final String translationString = "[" + threeDec.format(pose.translation[0]) + ", " + threeDec.format(pose.translation[1]) + ", " + threeDec.format(pose.translation[2]) + "] "; final String quaternionString = "[" + threeDec.format(pose.rotation[0]) + ", " + threeDec.format(pose.rotation[1]) + ", " + threeDec.format(pose.rotation[2]) + ", " + threeDec.format(pose.rotation[3]) + "] "; // Update the UI with TangoPose information runOnUiThread(new Runnable() { @Override public void run() { // Display pose data on screen in TextViews mPoseTextView.setText(translationString); mQuatTextView.setText(quaternionString); mPoseCountTextView.setText(Integer.toString(count)); mDeltaTextView.setText(threeDec.format(mDeltaTime)); if (pose.statusCode == TangoPoseData.POSE_VALID) { mPoseStatusTextView.setText(R.string.pose_valid); } else if (pose.statusCode == TangoPoseData.POSE_INVALID) { mPoseStatusTextView.setText(R.string.pose_invalid); } else if (pose.statusCode == TangoPoseData.POSE_INITIALIZING) { mPoseStatusTextView.setText(R.string.pose_initializing); } else if (pose.statusCode == TangoPoseData.POSE_UNKNOWN) { mPoseStatusTextView.setText(R.string.pose_unknown); } } }); // NEW - Printing data to an output Log Log.d(TAG, "OUTPUT onPoseAvailable:" + " TStamp: " + mPosePreviousTimeStamp + " dTime: " + mDeltaTime + " T: " + translationString + " R: " + quaternionString + " Status: " + mPoseStatusTextView.getText().toString()); } @Override public void onXyzIjAvailable(final TangoXyzIjData xyzIj) { Log.v(TAG, "SOME SHIT IS GOIN ON"); mCurrentTimeStamp = (float) xyzIj.timestamp; final float frameDelta = (mCurrentTimeStamp - mXyIjPreviousTimeStamp) * SECS_TO_MILLISECS; mXyIjPreviousTimeStamp = mCurrentTimeStamp; byte[] buffer = new byte[xyzIj.xyzCount * 3 * 4]; FileInputStream fileStream = new FileInputStream(xyzIj.xyzParcelFileDescriptor.getFileDescriptor()); try { fileStream.read(buffer, xyzIj.xyzParcelFileDescriptorOffset, buffer.length); fileStream.close(); } catch (IOException e) { e.printStackTrace(); } try { TangoPoseData pointCloudPose = mTango.getPoseAtTime(mCurrentTimeStamp, framePairs.get(0)); // NEW - Printing data to an output Log float[] translations = pointCloudPose.getTranslationAsFloats(); float[] rotations = pointCloudPose.getRotationAsFloats(); FloatBuffer mPointCloudFloatBuffer; mPointCloudFloatBuffer = ByteBuffer.wrap(buffer).order(ByteOrder.nativeOrder()).asFloatBuffer(); PrintWriter p = new PrintWriter(new BufferedWriter(new FileWriter(outFile, true))); Log.d(TAG, "OUTPUT onxyzjiNEW:" + mPointCloudFloatBuffer.toString()); p.println(mPointCloudFloatBuffer.get(xyzIj.xyzCount) + ", " + mPointCloudFloatBuffer.get(xyzIj.xyzCount + 1) + ", " + mPointCloudFloatBuffer.get(xyzIj.xyzCount + 2)); p.close(); mRenderer.getPointCloud().UpdatePoints(buffer, xyzIj.xyzCount); mRenderer.getModelMatCalculator().updatePointCloudModelMatrix( pointCloudPose.getTranslationAsFloats(), pointCloudPose.getRotationAsFloats()); mRenderer.getPointCloud() .setModelMatrix(mRenderer.getModelMatCalculator().getPointCloudModelMatrixCopy()); } catch (IOException e) { e.printStackTrace(); } catch (TangoErrorException e) { Toast.makeText(getApplicationContext(), R.string.TangoError, Toast.LENGTH_SHORT).show(); } catch (IndexOutOfBoundsException e) { Toast.makeText(getApplicationContext(), "SHIT IS FUCKED", Toast.LENGTH_SHORT).show(); } catch (TangoInvalidException e) { Toast.makeText(getApplicationContext(), R.string.TangoError, Toast.LENGTH_SHORT).show(); } // Must run UI changes on the UI thread. Running in the Tango // service thread // will result in an error. runOnUiThread(new Runnable() { DecimalFormat threeDec = new DecimalFormat("0.000"); @Override public void run() { // Display number of points in the point cloud mPointCountTextView.setText(Integer.toString(xyzIj.xyzCount)); mFrequencyTextView.setText("" + threeDec.format(frameDelta)); mAverageZTextView.setText("" + threeDec.format(mRenderer.getPointCloud().getAverageZ())); } }); } @Override public void onTangoEvent(final TangoEvent event) { runOnUiThread(new Runnable() { @Override public void run() { mTangoEventTextView.setText(event.eventKey + ": " + event.eventValue); } }); Log.v(TAG, "onTangoEvent" + event.eventKey + ": " + event.eventValue); } }); } }