Part google-ar#3

- analyze 3D point clouds

part0_work/src/main/java/com/google/ar/core/codelab/rawdepth/DepthData.java

@@ -5,11 +5,15 @@
 import com.google.ar.core.Anchor;
 import com.google.ar.core.CameraIntrinsics;
 import com.google.ar.core.Frame;
+import com.google.ar.core.Plane;
+import com.google.ar.core.Pose;
+import com.google.ar.core.TrackingState;
 import com.google.ar.core.exceptions.NotYetAvailableException;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
 import java.nio.FloatBuffer;
 import java.nio.ShortBuffer;
+import java.util.Collection;
 
 /**
  * Convert depth data from ARCore depth images to 3D pointclouds. Points are added by calling the
@@ -113,6 +117,11 @@ private static FloatBuffer convertRawDepthImagesTo3dPointBuffer(
                     + x * confidenceImagePlane.getPixelStride());
         final float confidenceNormalized = ((float) (confidencePixelValue & 0xff)) / 255.0f;
 
+        if (confidenceNormalized < 0.3 || depthMeters > 1.5) {
+          // Ignore "low-confidence" pixels or depth that is too far away.
+          continue;
+        }
+
         // Unproject the depth into a 3D point in camera coordinates.
         pointCamera[0] = depthMeters * (x - cx) / fx;
         pointCamera[1] = depthMeters * (cy - y) / fy;
@@ -131,4 +140,47 @@ private static FloatBuffer convertRawDepthImagesTo3dPointBuffer(
     points.rewind();
     return points;
   }
+
+  public static void filterUsingPlanes(FloatBuffer points, Collection<Plane> allPlanes) {
+    float[] planeNormal = new float[3];
+
+    // Allocate the output buffer.
+    int numPoints = points.remaining() / DepthData.FLOATS_PER_POINT;
+
+    // Check each plane against each point.
+    for (Plane plane : allPlanes) {
+      if (plane.getTrackingState() != TrackingState.TRACKING || plane.getSubsumedBy() != null) {
+        continue;
+      }
+
+      // Compute the normal vector of the plane.
+      Pose planePose = plane.getCenterPose();
+      planePose.getTransformedAxis(1, 1.0f, planeNormal, 0);
+
+      // Filter points that are too close to the plane.
+      for (int index = 0; index < numPoints; ++index) {
+        // Retrieves the next point.
+        final float x = points.get(FLOATS_PER_POINT * index);
+        final float y = points.get(FLOATS_PER_POINT * index + 1);
+        final float z = points.get(FLOATS_PER_POINT * index + 2);
+
+        // Transform point to be in world coordinates, to match plane info.
+        float distance = (x - planePose.tx()) * planeNormal[0]
+            + (y - planePose.ty()) * planeNormal[1]
+            + (z - planePose.tz()) * planeNormal[2];
+        // Controls the size of objects detected.
+        // Smaller values mean smaller objects will be kept.
+        // Larger values will only allow detection of larger objects, but also helps reduce noise.
+        if (Math.abs(distance) > 0.03) {
+          continue;  // Keep this point, since it's far enough away from the plane.
+        }
+
+        // Invalidate points that are too close to planar surfaces.
+        points.put(FLOATS_PER_POINT * index, 0);
+        points.put(FLOATS_PER_POINT * index + 1, 0);
+        points.put(FLOATS_PER_POINT * index + 2, 0);
+        points.put(FLOATS_PER_POINT * index + 3, 0);
+      }
+    }
+  }
 }

part0_work/src/main/java/com/google/ar/core/codelab/rawdepth/RawDepthCodelabActivity.java

@@ -35,12 +35,15 @@
 import com.google.ar.core.Session;
 import com.google.ar.core.Trackable;
 import com.google.ar.core.TrackingState;
+import com.google.ar.core.codelab.common.helpers.AABB;
 import com.google.ar.core.codelab.common.helpers.CameraPermissionHelper;
 import com.google.ar.core.codelab.common.helpers.DisplayRotationHelper;
 import com.google.ar.core.codelab.common.helpers.FullScreenHelper;
+import com.google.ar.core.codelab.common.helpers.PointClusteringHelper;
 import com.google.ar.core.codelab.common.helpers.SnackbarHelper;
 import com.google.ar.core.codelab.common.helpers.TrackingStateHelper;
 import com.google.ar.core.codelab.common.rendering.BackgroundRenderer;
+import com.google.ar.core.codelab.common.rendering.BoxRenderer;
 import com.google.ar.core.codelab.common.rendering.DepthRenderer;
 import com.google.ar.core.exceptions.CameraNotAvailableException;
 import com.google.ar.core.exceptions.UnavailableApkTooOldException;
@@ -51,6 +54,7 @@
 import java.io.IOException;
 import java.nio.FloatBuffer;
 
+import java.util.List;
 import javax.microedition.khronos.egl.EGLConfig;
 import javax.microedition.khronos.opengles.GL10;
 
@@ -72,6 +76,7 @@ public class RawDepthCodelabActivity extends AppCompatActivity implements GLSurf
 
   private final BackgroundRenderer backgroundRenderer = new BackgroundRenderer();
   private final DepthRenderer depthRenderer = new DepthRenderer();
+  private final BoxRenderer boxRenderer = new BoxRenderer();
 
   @Override
   protected void onCreate(Bundle savedInstanceState) {
@@ -208,6 +213,7 @@ public void onSurfaceCreated(GL10 gl, EGLConfig config) {
       // Create the texture and pass it to ARCore session to be filled during update().
       backgroundRenderer.createOnGlThread(/*context=*/ this);
       depthRenderer.createOnGlThread(/*context=*/ this);
+      boxRenderer.createOnGlThread(/*context=*/this);
     } catch (IOException e) {
       Log.e(TAG, "Failed to read an asset file", e);
     }
@@ -260,9 +266,20 @@ public void onDrawFrame(GL10 gl) {
         return;
       }
 
+      // Filter the depth data.
+      DepthData.filterUsingPlanes(points, session.getAllTrackables(Plane.class));
+
       // Visualize depth points.
       depthRenderer.update(points);
       depthRenderer.draw(camera);
+
+      // Draw boxes around clusters of points.
+      PointClusteringHelper clusteringHelper = new PointClusteringHelper(points);
+      List<AABB> clusters = clusteringHelper.findClusters();
+      for (AABB aabb : clusters) {
+        boxRenderer.draw(aabb, camera);
+      }
+
     } catch (Throwable t) {
       // Avoid crashing the application due to unhandled exceptions.
       Log.e(TAG, "Exception on the OpenGL thread", t);