From 1ace7350daa658b287710edd2c35620e7a641e9f Mon Sep 17 00:00:00 2001 From: "dingyi.chen" Date: Wed, 11 May 2022 19:08:51 -0700 Subject: [PATCH] Part #3 - analyze 3D point clouds --- .../ar/core/codelab/rawdepth/DepthData.java | 52 +++++++++++++++++++ .../rawdepth/RawDepthCodelabActivity.java | 17 ++++++ 2 files changed, 69 insertions(+) diff --git a/part0_work/src/main/java/com/google/ar/core/codelab/rawdepth/DepthData.java b/part0_work/src/main/java/com/google/ar/core/codelab/rawdepth/DepthData.java index 65dfb96..aa05a2d 100644 --- a/part0_work/src/main/java/com/google/ar/core/codelab/rawdepth/DepthData.java +++ b/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 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); + } + } + } } diff --git a/part0_work/src/main/java/com/google/ar/core/codelab/rawdepth/RawDepthCodelabActivity.java b/part0_work/src/main/java/com/google/ar/core/codelab/rawdepth/RawDepthCodelabActivity.java index fc639c1..ebce96e 100755 --- a/part0_work/src/main/java/com/google/ar/core/codelab/rawdepth/RawDepthCodelabActivity.java +++ b/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 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);