Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- * To change this license header, choose License Headers in Project Properties.
- * To change this template file, choose Tools | Templates
- * and open the template in the editor.
- */
- package de.imld.basic3d_android;
- import android.annotation.SuppressLint;
- import android.util.Log;
- import com.google.atap.tangoservice.Tango.OnTangoUpdateListener;
- import com.google.atap.tangoservice.TangoEvent;
- import com.google.atap.tangoservice.TangoPoseData;
- import com.google.atap.tangoservice.TangoXyzIjData;
- import com.jme3.math.FastMath;
- import com.jme3.math.Matrix4f;
- import com.jme3.math.Quaternion;
- import com.jme3.math.Vector3f;
- import com.projecttango.tangosupport.TangoPointCloudManager;
- import de.imld.tracking.PointCloudListener;
- import de.imld.tracking.PointCloudUpdate;
- import de.imld.tracking.TrackingListener;
- import de.imld.tracking.TrackingUpdate;
- import java.nio.FloatBuffer;
- import java.util.Date;
- import java.util.List;
- /**
- * Defines what happens, when Tango Updates come in
- * @author Georg Eckert
- */
- public class TangoUpdateListener implements OnTangoUpdateListener {
- // ############################################################## ATTRIBUTES
- TrackingUpdate trackUpdate;
- PointCloudUpdate pointCloudUpdate;
- private final List<TrackingListener> _listeners;
- private final List<PointCloudListener> _pclisteners;
- private final TangoProvider tangoProvider;
- private TangoPointCloudManager pcm;
- // Time
- private double _previousTimeStamp;
- // Update every 33ms, According to
- // https://developers.google.com/project-tango/ux/ux-best-practices#maximizing_performance
- private static final double UPDATE_INTERVAL_MS = 33.0;
- private static final int SECS_TO_MILLISECS = 1000;
- private double _timeToNextUpdate = UPDATE_INTERVAL_MS;
- private double mXyIjPreviousTimeStamp;
- private double mXyzIjTimeToNextUpdate = UPDATE_INTERVAL_MS;
- // Tango to JME Conversion
- private final Matrix4f transformation;
- private final Vector3f rotationAxis;
- private final Quaternion camRotation;
- private final Quaternion rotationCorr1, rotationCorr2;
- // ############################################################# CONSTRUCTOR
- public TangoUpdateListener(
- TrackingUpdate updateContainer,
- List<TrackingListener> listeners,
- TangoProvider tangoProvider,
- List<PointCloudListener> pclisteners,
- PointCloudUpdate pcu) {
- this.pcm = tangoProvider.getPointCloudManager();
- this._listeners = listeners;
- this._pclisteners = pclisteners;
- this.trackUpdate = updateContainer;
- this.pointCloudUpdate = pcu;
- this.tangoProvider = tangoProvider;
- // Tango 2 JME
- this.transformation = new Matrix4f();
- this.rotationAxis = new Vector3f();
- this.camRotation = new Quaternion();
- this.rotationCorr1 = new Quaternion();
- this.rotationCorr2 = new Quaternion();
- }
- /**
- * When a new TangoPoseData gets available
- * @param pose
- */
- @SuppressLint("DefaultLocale")
- @Override
- public void onPoseAvailable(TangoPoseData pose)
- {
- // ........................................................ TIME
- // Determine delta time
- final double deltaTime =
- (pose.timestamp - _previousTimeStamp) * SECS_TO_MILLISECS;
- _previousTimeStamp = pose.timestamp;
- _timeToNextUpdate -= deltaTime;
- // .............................................. TRANSFORMATION
- Matrix4f tango2opengl = new Matrix4f(
- 1, 0, 0, 0,
- 0, 0, -1, 0,
- 0, 1, 0, 0,
- 0, 0, 0, 1);
- // ---------------------------------------------------- rotation
- // Rotation Axis
- rotationAxis.x = pose.getRotationAsFloats()[0];
- rotationAxis.y = pose.getRotationAsFloats()[1];
- rotationAxis.z = pose.getRotationAsFloats()[2];
- // System.out.println("INFO: Rotation Axis " + rotationAxis);
- // Create rotation quaternion for camera
- camRotation.set(
- -rotationAxis.x, // X
- rotationAxis.y, // Y
- -rotationAxis.z, // Z
- pose.getRotationAsFloats()[3] // W, rotation degree
- );
- // Rotate 90° around x-axis
- rotationCorr1.fromAngleAxis(
- FastMath.HALF_PI, Vector3f.UNIT_X
- );
- // Rotate 180° around vertical (y) axis
- /*rotationCorr2.fromAngleAxis(
- FastMath.PI, Vector3f.UNIT_Y
- );*/
- // Combine rotations
- transformation.setRotationQuaternion(
- rotationCorr2.mult(rotationCorr1.mult(camRotation)));
- // ------------------------------------------------- translation
- /**
- * Setting the translation values:
- * X ... has to be inverted, jME-x-achsis seems to be -X
- * Y ... Y-Value given by Tango is Z-Value in jME
- * Z ... Z-Value given by Tango is Y-Value in jME
- *
- */
- transformation.setTranslation(
- (float) -pose.translation[0], // X points right
- (float) pose.translation[2]+4, // Y points up
- (float) pose.translation[1]); // Z points to viewer
- trackUpdate = new TrackingUpdate(
- new Date(), transformation, "tango", 0);
- // .............................................. TRANSFORMATION
- // Throttle updates based on UPDATE_INTERVAL_MS.
- if (_timeToNextUpdate < 0.0)
- {
- _timeToNextUpdate = UPDATE_INTERVAL_MS;
- // inform listeners
- for (TrackingListener listener : _listeners)
- {
- listener.acceptMessage(tangoProvider, trackUpdate);
- }
- }
- }
- @Override
- public void onXyzIjAvailable(TangoXyzIjData xyzIj)
- {
- System.out.println("POINT CLOUD AVAILABLE");
- Log.e("CLOUD", "CLOUD AVAILABLE");
- /*
- if (mTangoUx != null) {
- mTangoUx.updateXyzCount(xyzIj.xyzCount);
- }*/
- /*pcm.updateXyzIj(xyzIj);
- final double currentTimeStamp = xyzIj.timestamp;
- final double pointCloudFrameDelta = (currentTimeStamp - mXyIjPreviousTimeStamp)
- * SECS_TO_MILLISECS;
- mXyIjPreviousTimeStamp = currentTimeStamp;
- final double averageDepth = getAveragedDepth(xyzIj.xyz);
- mXyzIjTimeToNextUpdate -= pointCloudFrameDelta;
- pointCloudUpdate = new PointCloudUpdate(new Date(),
- pcm.getLatestXyzIj().xyz.array(),
- pcm.getLatestXyzIj().xyzCount,
- pcm.getLatestXyzIj().ijRows,
- pcm.getLatestXyzIj().ijCols,
- 0
- );
- System.out.println("POINT CLOUD READY");
- Log.i("POINT_CLOUD", pcm.getLatestXyzIj().xyz.toString());
- // Print Point Cloud data
- if(pointCloudUpdate.xyzCount == pointCloudUpdate.ijCols*pointCloudUpdate.ijRows) {
- Log.i("POINT CLOUD: ", "Array Size correct");
- for(int i=0; i<pointCloudUpdate.ijRows; i++) {
- System.out.println("[");
- for(int j=0; j<pointCloudUpdate.ijCols; j++) {
- System.out.print(pointCloudUpdate.xyz[i*pointCloudUpdate.ijCols+j]);
- }
- System.out.print("]");
- }
- }
- if (mXyzIjTimeToNextUpdate < 0.0) {
- mXyzIjTimeToNextUpdate = UPDATE_INTERVAL_MS;
- // inform listeners
- for (PointCloudListener listener : _pclisteners)
- {
- listener.acceptMessage(tangoProvider, pointCloudUpdate);
- }
- }*/
- }
- @Override
- public void onTangoEvent(TangoEvent arg0)
- {
- System.out.println("EVENT AVAILABLE");
- System.out.println(arg0.eventKey + arg0.eventValue);
- }
- @Override
- public void onFrameAvailable(int arg0)
- {
- System.out.println("FRAME AVAILABLE");
- }
- /**
- * Calculates the average depth from a point cloud buffer.
- *
- * @param pointCloudBuffer
- * @return Average depth.
- *
- * @author Google: Rajawali, Google Tango Examples
- */
- private float getAveragedDepth(FloatBuffer pointCloudBuffer) {
- int pointCount = pointCloudBuffer.capacity() / 3;
- float totalZ = 0;
- float averageZ = 0;
- for (int i = 0; i < pointCloudBuffer.capacity() - 3; i = i + 3) {
- totalZ = totalZ + pointCloudBuffer.get(i + 2);
- }
- if (pointCount != 0) {
- averageZ = totalZ / pointCount;
- }
- return averageZ;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement