Advertisement
Guest User

OnTangoUpdateListener Depth Callback never called

a guest
May 3rd, 2016
166
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 9.92 KB | None | 0 0
  1. /*
  2.  * To change this license header, choose License Headers in Project Properties.
  3.  * To change this template file, choose Tools | Templates
  4.  * and open the template in the editor.
  5.  */
  6. package de.imld.basic3d_android;
  7.  
  8. import android.annotation.SuppressLint;
  9. import android.util.Log;
  10. import com.google.atap.tangoservice.Tango.OnTangoUpdateListener;
  11. import com.google.atap.tangoservice.TangoEvent;
  12. import com.google.atap.tangoservice.TangoPoseData;
  13. import com.google.atap.tangoservice.TangoXyzIjData;
  14. import com.jme3.math.FastMath;
  15. import com.jme3.math.Matrix4f;
  16. import com.jme3.math.Quaternion;
  17. import com.jme3.math.Vector3f;
  18. import com.projecttango.tangosupport.TangoPointCloudManager;
  19. import de.imld.tracking.PointCloudListener;
  20. import de.imld.tracking.PointCloudUpdate;
  21. import de.imld.tracking.TrackingListener;
  22. import de.imld.tracking.TrackingUpdate;
  23. import java.nio.FloatBuffer;
  24. import java.util.Date;
  25. import java.util.List;
  26.  
  27. /**
  28.  * Defines what happens, when Tango Updates come in
  29.  * @author Georg Eckert
  30.  */
  31. public class TangoUpdateListener implements OnTangoUpdateListener {
  32.    
  33.     // ############################################################## ATTRIBUTES
  34.     TrackingUpdate trackUpdate;
  35.     PointCloudUpdate pointCloudUpdate;
  36.     private final List<TrackingListener> _listeners;
  37.     private final List<PointCloudListener> _pclisteners;
  38.     private final TangoProvider tangoProvider;
  39.     private TangoPointCloudManager pcm;
  40.    
  41.     // Time
  42.     private double _previousTimeStamp;
  43.     // Update every 33ms, According to
  44.     // https://developers.google.com/project-tango/ux/ux-best-practices#maximizing_performance
  45.     private static final double UPDATE_INTERVAL_MS = 33.0;
  46.     private static final int SECS_TO_MILLISECS = 1000;
  47.     private double _timeToNextUpdate = UPDATE_INTERVAL_MS;
  48.    
  49.      private double mXyIjPreviousTimeStamp;
  50.      private double mXyzIjTimeToNextUpdate = UPDATE_INTERVAL_MS;
  51.    
  52.     // Tango to JME Conversion
  53.     private final Matrix4f transformation;
  54.     private final Vector3f rotationAxis;
  55.     private final Quaternion camRotation;
  56.     private final Quaternion rotationCorr1, rotationCorr2;
  57.    
  58.     // ############################################################# CONSTRUCTOR
  59.     public TangoUpdateListener(
  60.         TrackingUpdate updateContainer,
  61.         List<TrackingListener> listeners,
  62.         TangoProvider tangoProvider,
  63.         List<PointCloudListener> pclisteners,
  64.         PointCloudUpdate pcu) {
  65.        
  66.         this.pcm = tangoProvider.getPointCloudManager();
  67.        
  68.         this._listeners = listeners;
  69.         this._pclisteners = pclisteners;
  70.         this.trackUpdate = updateContainer;
  71.         this.pointCloudUpdate = pcu;
  72.         this.tangoProvider = tangoProvider;
  73.        
  74.         // Tango 2 JME
  75.         this.transformation = new Matrix4f();
  76.         this.rotationAxis = new Vector3f();
  77.         this.camRotation = new Quaternion();
  78.         this.rotationCorr1 = new Quaternion();
  79.         this.rotationCorr2 = new Quaternion();
  80.     }
  81.  
  82.     /**
  83.              * When a new TangoPoseData gets available
  84.              * @param pose
  85.              */
  86.             @SuppressLint("DefaultLocale")
  87.             @Override
  88.             public void onPoseAvailable(TangoPoseData pose)
  89.             {
  90.  
  91.                 // ........................................................ TIME
  92.                 // Determine delta time
  93.                 final double deltaTime =
  94.                     (pose.timestamp - _previousTimeStamp) * SECS_TO_MILLISECS;
  95.                 _previousTimeStamp = pose.timestamp;
  96.                 _timeToNextUpdate -= deltaTime;
  97.  
  98.                 // .............................................. TRANSFORMATION
  99.                 Matrix4f tango2opengl = new Matrix4f(
  100.                     1, 0, 0, 0,
  101.                     0, 0, -1, 0,
  102.                     0, 1, 0, 0,
  103.                     0, 0, 0, 1);
  104.                
  105.                 // ---------------------------------------------------- rotation
  106.                 // Rotation Axis
  107.                 rotationAxis.x = pose.getRotationAsFloats()[0];
  108.                 rotationAxis.y = pose.getRotationAsFloats()[1];
  109.                 rotationAxis.z = pose.getRotationAsFloats()[2];
  110.                 // System.out.println("INFO: Rotation Axis " + rotationAxis);
  111.              
  112.                 // Create rotation quaternion for camera
  113.                 camRotation.set(
  114.                     -rotationAxis.x,                // X
  115.                     rotationAxis.y,                 // Y
  116.                     -rotationAxis.z,                // Z
  117.                     pose.getRotationAsFloats()[3]   // W, rotation degree
  118.                 );
  119.                    
  120.                 // Rotate 90° around x-axis
  121.                 rotationCorr1.fromAngleAxis(
  122.                     FastMath.HALF_PI, Vector3f.UNIT_X
  123.                 );
  124.                 // Rotate 180° around vertical (y) axis
  125.                 /*rotationCorr2.fromAngleAxis(
  126.                     FastMath.PI, Vector3f.UNIT_Y
  127.                 );*/
  128.                
  129.                 // Combine rotations
  130.                 transformation.setRotationQuaternion(
  131.                     rotationCorr2.mult(rotationCorr1.mult(camRotation)));
  132.                
  133.                 // ------------------------------------------------- translation
  134.                 /**
  135.                  * Setting the translation values:
  136.                  *  X ... has to be inverted, jME-x-achsis seems to be -X
  137.                  *  Y ... Y-Value given by Tango is Z-Value in jME
  138.                  *  Z ... Z-Value given by Tango is Y-Value in jME
  139.                  *
  140.                  */
  141.                 transformation.setTranslation(
  142.                     (float) -pose.translation[0],       // X points right
  143.                     (float) pose.translation[2]+4,        // Y points up
  144.                     (float) pose.translation[1]);    // Z points to viewer
  145.                
  146.                 trackUpdate = new TrackingUpdate(
  147.                     new Date(), transformation, "tango", 0);
  148.                 // .............................................. TRANSFORMATION
  149.  
  150.                 // Throttle updates based on UPDATE_INTERVAL_MS.
  151.                 if (_timeToNextUpdate < 0.0)
  152.                 {
  153.                     _timeToNextUpdate = UPDATE_INTERVAL_MS;
  154.  
  155.                     // inform listeners
  156.                     for (TrackingListener listener : _listeners)
  157.                     {
  158.                         listener.acceptMessage(tangoProvider, trackUpdate);
  159.                     }
  160.                 }
  161.             }
  162.  
  163.             @Override
  164.             public void onXyzIjAvailable(TangoXyzIjData xyzIj)
  165.             {
  166.                
  167.                 System.out.println("POINT CLOUD AVAILABLE");
  168.                 Log.e("CLOUD", "CLOUD AVAILABLE");
  169.                 /*
  170.                 if (mTangoUx != null) {
  171.                     mTangoUx.updateXyzCount(xyzIj.xyzCount);
  172.                 }*/
  173.                 /*pcm.updateXyzIj(xyzIj);
  174.  
  175.                 final double currentTimeStamp = xyzIj.timestamp;
  176.                 final double pointCloudFrameDelta = (currentTimeStamp - mXyIjPreviousTimeStamp)
  177.                         * SECS_TO_MILLISECS;
  178.                 mXyIjPreviousTimeStamp = currentTimeStamp;
  179.                 final double averageDepth = getAveragedDepth(xyzIj.xyz);
  180.  
  181.                 mXyzIjTimeToNextUpdate -= pointCloudFrameDelta;
  182.                
  183.                 pointCloudUpdate = new PointCloudUpdate(new Date(),
  184.                     pcm.getLatestXyzIj().xyz.array(),
  185.                     pcm.getLatestXyzIj().xyzCount,
  186.                     pcm.getLatestXyzIj().ijRows,
  187.                     pcm.getLatestXyzIj().ijCols,
  188.                     0
  189.                 );
  190.                
  191.                 System.out.println("POINT CLOUD READY");
  192.                
  193.                 Log.i("POINT_CLOUD", pcm.getLatestXyzIj().xyz.toString());
  194.                 // Print Point Cloud data
  195.                 if(pointCloudUpdate.xyzCount == pointCloudUpdate.ijCols*pointCloudUpdate.ijRows) {
  196.                     Log.i("POINT CLOUD: ", "Array Size correct");
  197.                     for(int i=0; i<pointCloudUpdate.ijRows; i++) {
  198.                         System.out.println("[");
  199.                         for(int j=0; j<pointCloudUpdate.ijCols; j++) {
  200.                             System.out.print(pointCloudUpdate.xyz[i*pointCloudUpdate.ijCols+j]);
  201.                         }
  202.                         System.out.print("]");
  203.                     }
  204.                 }
  205.                    
  206.  
  207.                 if (mXyzIjTimeToNextUpdate < 0.0) {
  208.                     mXyzIjTimeToNextUpdate = UPDATE_INTERVAL_MS;
  209.                    
  210.                     // inform listeners
  211.                     for (PointCloudListener listener : _pclisteners)
  212.                     {
  213.                         listener.acceptMessage(tangoProvider, pointCloudUpdate);
  214.                     }
  215.  
  216.                 }*/
  217.             }
  218.  
  219.             @Override
  220.             public void onTangoEvent(TangoEvent arg0)
  221.             {
  222.                 System.out.println("EVENT AVAILABLE");
  223.                 System.out.println(arg0.eventKey + arg0.eventValue);
  224.             }
  225.  
  226.             @Override
  227.             public void onFrameAvailable(int arg0)
  228.             {
  229.                 System.out.println("FRAME AVAILABLE");
  230.  
  231.             }
  232.            
  233.     /**
  234.      * Calculates the average depth from a point cloud buffer.
  235.      *
  236.      * @param pointCloudBuffer
  237.      * @return Average depth.
  238.      *
  239.      * @author Google: Rajawali, Google Tango Examples
  240.      */
  241.     private float getAveragedDepth(FloatBuffer pointCloudBuffer) {
  242.         int pointCount = pointCloudBuffer.capacity() / 3;
  243.         float totalZ = 0;
  244.         float averageZ = 0;
  245.         for (int i = 0; i < pointCloudBuffer.capacity() - 3; i = i + 3) {
  246.             totalZ = totalZ + pointCloudBuffer.get(i + 2);
  247.         }
  248.         if (pointCount != 0) {
  249.             averageZ = totalZ / pointCount;
  250.         }
  251.         return averageZ;
  252.     }
  253.    
  254. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement