drawdown-arcore-depth 0.1.4

package/android/src/main/java/org/drawdown/arcoredepth/DepthObservationSampler.java

@@ -0,0 +1,509 @@
+package org.drawdown.arcoredepth;
+
+import android.media.Image;
+
+import com.google.ar.core.Camera;
+import com.google.ar.core.CameraIntrinsics;
+import com.google.ar.core.Coordinates2d;
+import com.google.ar.core.Frame;
+import com.google.ar.core.TrackingState;
+import com.google.ar.core.exceptions.NotYetAvailableException;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/**
+ * DBH sampler anchored by ARCore Raw Depth.
+ *
+ * Raw Depth is deliberately used for the front-surface range because it is the
+ * ARCore API intended for measurement/shape work and supplies a per-pixel
+ * confidence image. The smoothed Depth image is used only to fill the trunk
+ * surface enough to estimate left/right boundaries, and only after Raw Depth
+ * has established a nearby, high-confidence foreground range.
+ */
+final class DepthObservationSampler {
+    private static final int RAW_CONFIDENCE_THRESHOLD = 128;
+    private static final int RAW_ANCHOR_SEARCH_RADIUS = 10;
+    private static final int RAW_PATCH_RADIUS = 5;
+    private static final int MIN_RAW_PATCH_SAMPLES = 3;
+    private static final int ROW_RADIUS = 2;
+    private static final int MAX_GAP_PIXELS = 1;
+    private static final int[] BOUNDARY_ROW_OFFSETS = new int[] { -4, -2, 0, 2, 4 };
+    private static final int MIN_CONSENSUS_ROWS = 3;
+    private static final int MAX_BOUNDARY_SPREAD_PX = 4;
+    private static final double MAX_TRUNK_WIDTH_FRACTION = 0.68;
+    private static final int EDGE_MARGIN_PX = 2;
+    private static final double MIN_TARGET_DEPTH_MM = 500.0;
+    private static final double MAX_TARGET_DEPTH_MM = 5000.0;
+
+    DepthObservation sample(Frame frame, Camera camera, int viewWidth, int viewHeight) {
+        if (camera.getTrackingState() != TrackingState.TRACKING) {
+            return null;
+        }
+
+        try (
+                Image rawDepth = frame.acquireRawDepthImage16Bits();
+                Image rawConfidence = frame.acquireRawDepthConfidenceImage();
+                Image smoothDepth = frame.acquireDepthImage16Bits()) {
+
+            CameraIntrinsics intrinsics = camera.getImageIntrinsics();
+            float[] focal = intrinsics.getFocalLength();
+            int[] imageDimensions = intrinsics.getImageDimensions();
+            if (focal == null || focal.length < 2 || imageDimensions == null || imageDimensions.length < 2) {
+                return null;
+            }
+
+            // ARCore documents raw depth, confidence and smoothed depth as the same size.
+            if (rawDepth.getWidth() != rawConfidence.getWidth()
+                    || rawDepth.getHeight() != rawConfidence.getHeight()
+                    || rawDepth.getWidth() != smoothDepth.getWidth()
+                    || rawDepth.getHeight() != smoothDepth.getHeight()) {
+                return null;
+            }
+
+            float[] viewCenter = new float[] { viewWidth / 2f, viewHeight / 2f };
+            float[] imageCenter = new float[2];
+            frame.transformCoordinates2d(
+                    Coordinates2d.VIEW,
+                    viewCenter,
+                    Coordinates2d.IMAGE_PIXELS,
+                    imageCenter);
+
+            int[] mappedCenter = imageToDepth(frame, rawDepth, imageCenter[0], imageCenter[1]);
+            if (mappedCenter == null) {
+                return null;
+            }
+
+            RawAnchor anchor = findRawAnchor(rawDepth, rawConfidence, mappedCenter[0], mappedCenter[1]);
+            if (anchor == null) {
+                return null;
+            }
+
+            double rawToleranceMm = clamp(anchor.depthMm * 0.10, 100.0, 250.0);
+            List<Integer> rawPatch = collectConfidentRawPatch(
+                    rawDepth,
+                    rawConfidence,
+                    anchor.x,
+                    anchor.y,
+                    RAW_PATCH_RADIUS,
+                    rawToleranceMm,
+                    anchor.depthMm);
+            if (rawPatch.size() < MIN_RAW_PATCH_SAMPLES) {
+                return null;
+            }
+
+            double centerDepthMm = median(rawPatch);
+            double centerMadMm = mad(rawPatch, centerDepthMm);
+            if (centerDepthMm < MIN_TARGET_DEPTH_MM || centerDepthMm > MAX_TARGET_DEPTH_MM) {
+                return null;
+            }
+
+            // Locate the foreground surface in the smoothed depth image, but only
+            // near the high-confidence raw-depth anchor. This prevents the 20-30 m
+            // outdoor background from becoming the trunk range.
+            int centerX = findNearestSmoothPixelAtDepth(
+                    smoothDepth,
+                    anchor.x,
+                    anchor.y,
+                    centerDepthMm,
+                    clamp(centerDepthMm * 0.10, 100.0, 250.0));
+            if (centerX < 0) {
+                return null;
+            }
+
+            double toleranceMm = clamp(centerDepthMm * 0.08, 80.0, 220.0);
+            Boundary boundary = findConsensusBoundary(
+                    smoothDepth,
+                    centerX,
+                    anchor.y,
+                    centerDepthMm,
+                    toleranceMm);
+            if (boundary == null || boundary.rightX - boundary.leftX < 3) {
+                return null;
+            }
+
+            int depthWidth = smoothDepth.getWidth();
+            int boundaryWidth = boundary.rightX - boundary.leftX;
+            if (boundary.leftX <= EDGE_MARGIN_PX
+                    || boundary.rightX >= depthWidth - 1 - EDGE_MARGIN_PX
+                    || boundaryWidth > depthWidth * MAX_TRUNK_WIDTH_FRACTION) {
+                return null;
+            }
+
+            float[] leftImage = depthToImage(frame, smoothDepth, boundary.leftX, anchor.y);
+            float[] rightImage = depthToImage(frame, smoothDepth, boundary.rightX, anchor.y);
+            if (leftImage == null || rightImage == null) {
+                return null;
+            }
+
+            double dx = rightImage[0] - leftImage[0];
+            double dy = rightImage[1] - leftImage[1];
+            double imageWidthPx = Math.hypot(dx, dy);
+            if (!Double.isFinite(imageWidthPx) || imageWidthPx < 8.0) {
+                return null;
+            }
+
+            double normalizedAngularWidth = Math.sqrt(
+                    Math.pow(dx / focal[0], 2.0)
+                            + Math.pow(dy / focal[1], 2.0));
+            if (!Double.isFinite(normalizedAngularWidth)
+                    || normalizedAngularWidth <= 0.0
+                    || normalizedAngularWidth > 0.95) {
+                return null;
+            }
+
+            double effectiveFocalPx = imageWidthPx / normalizedAngularWidth;
+            double confidenceMedian = medianConfidence(rawConfidence, anchor.x, anchor.y, RAW_PATCH_RADIUS);
+            double trackingConfidence = clamp(confidenceMedian / 255.0, 0.0, 1.0);
+
+            String boundaryConfidence;
+            if (boundary.consensusRows >= 4
+                    && boundary.maxSpreadPx <= 2
+                    && boundary.leftJumpMm >= 120.0
+                    && boundary.rightJumpMm >= 120.0
+                    && boundaryWidth >= 6
+                    && centerMadMm <= 50.0
+                    && trackingConfidence >= 0.65) {
+                boundaryConfidence = "high";
+            } else if (boundary.consensusRows >= MIN_CONSENSUS_ROWS
+                    && boundary.maxSpreadPx <= MAX_BOUNDARY_SPREAD_PX
+                    && boundaryWidth >= 4
+                    && centerMadMm <= 90.0
+                    && trackingConfidence >= 0.50) {
+                boundaryConfidence = "medium";
+            } else {
+                boundaryConfidence = "low";
+            }
+
+            return new DepthObservation(
+                    centerDepthMm / 1000.0,
+                    effectiveFocalPx,
+                    imageWidthPx,
+                    "arcore_raw_depth",
+                    trackingConfidence,
+                    boundaryConfidence,
+                    centerMadMm / 1000.0,
+                    System.currentTimeMillis());
+        } catch (NotYetAvailableException e) {
+            return null;
+        } catch (Exception e) {
+            return null;
+        }
+    }
+
+    private static RawAnchor findRawAnchor(Image rawDepth, Image confidence, int centerX, int centerY) {
+        RawAnchor best = null;
+        double bestScore = Double.POSITIVE_INFINITY;
+
+        for (int y = Math.max(0, centerY - RAW_ANCHOR_SEARCH_RADIUS);
+             y <= Math.min(rawDepth.getHeight() - 1, centerY + RAW_ANCHOR_SEARCH_RADIUS);
+             y++) {
+            for (int x = Math.max(0, centerX - RAW_ANCHOR_SEARCH_RADIUS);
+                 x <= Math.min(rawDepth.getWidth() - 1, centerX + RAW_ANCHOR_SEARCH_RADIUS);
+                 x++) {
+                int conf = getConfidence(confidence, x, y);
+                if (conf < RAW_CONFIDENCE_THRESHOLD) continue;
+                int depthMm = getDepthMm(rawDepth, x, y);
+                if (depthMm < MIN_TARGET_DEPTH_MM || depthMm > MAX_TARGET_DEPTH_MM) continue;
+
+                double spatial = Math.hypot(x - centerX, y - centerY);
+                // Prefer the pixel nearest the crosshair, breaking near-ties in
+                // favour of higher raw-depth confidence.
+                double score = spatial - (conf / 255.0) * 0.35;
+                if (score < bestScore) {
+                    bestScore = score;
+                    best = new RawAnchor(x, y, depthMm, conf);
+                }
+            }
+        }
+        return best;
+    }
+
+    private static List<Integer> collectConfidentRawPatch(
+            Image rawDepth,
+            Image confidence,
+            int centerX,
+            int centerY,
+            int radius,
+            double toleranceMm,
+            double expectedDepthMm) {
+        List<Integer> values = new ArrayList<>();
+        for (int y = Math.max(0, centerY - radius); y <= Math.min(rawDepth.getHeight() - 1, centerY + radius); y++) {
+            for (int x = Math.max(0, centerX - radius); x <= Math.min(rawDepth.getWidth() - 1, centerX + radius); x++) {
+                if (getConfidence(confidence, x, y) < RAW_CONFIDENCE_THRESHOLD) continue;
+                int value = getDepthMm(rawDepth, x, y);
+                if (value > 0 && Math.abs(value - expectedDepthMm) <= toleranceMm) {
+                    values.add(value);
+                }
+            }
+        }
+        return values;
+    }
+
+    private static int findNearestSmoothPixelAtDepth(
+            Image smoothDepth,
+            int rawX,
+            int rawY,
+            double targetDepthMm,
+            double toleranceMm) {
+        int bestX = -1;
+        double bestDistance = Double.POSITIVE_INFINITY;
+        for (int offset = 0; offset <= 6; offset++) {
+            int[] xs = offset == 0 ? new int[] { rawX } : new int[] { rawX - offset, rawX + offset };
+            for (int x : xs) {
+                if (x < 0 || x >= smoothDepth.getWidth()) continue;
+                double value = rowMedian(smoothDepth, x, rawY, ROW_RADIUS);
+                if (value <= 0 || Math.abs(value - targetDepthMm) > toleranceMm) continue;
+                double distance = Math.abs(x - rawX);
+                if (distance < bestDistance) {
+                    bestDistance = distance;
+                    bestX = x;
+                }
+            }
+            if (bestX >= 0) return bestX;
+        }
+        return -1;
+    }
+
+    private static int[] imageToDepth(Frame frame, Image depthImage, float imageX, float imageY) {
+        float[] cpu = new float[] { imageX, imageY };
+        float[] texture = new float[2];
+        frame.transformCoordinates2d(
+                Coordinates2d.IMAGE_PIXELS,
+                cpu,
+                Coordinates2d.TEXTURE_NORMALIZED,
+                texture);
+        if (!isTextureCoordinateValid(texture)) return null;
+        int x = clampInt((int) Math.floor(texture[0] * depthImage.getWidth()), 0, depthImage.getWidth() - 1);
+        int y = clampInt((int) Math.floor(texture[1] * depthImage.getHeight()), 0, depthImage.getHeight() - 1);
+        return new int[] { x, y };
+    }
+
+    private static float[] depthToImage(Frame frame, Image depthImage, int depthX, int depthY) {
+        float[] texture = new float[] {
+                depthX / (float) depthImage.getWidth(),
+                depthY / (float) depthImage.getHeight()
+        };
+        float[] cpu = new float[2];
+        frame.transformCoordinates2d(
+                Coordinates2d.TEXTURE_NORMALIZED,
+                texture,
+                Coordinates2d.IMAGE_PIXELS,
+                cpu);
+        if (!Float.isFinite(cpu[0]) || !Float.isFinite(cpu[1])) return null;
+        return cpu;
+    }
+
+    private static boolean isTextureCoordinateValid(float[] texture) {
+        return texture != null && texture.length >= 2
+                && Float.isFinite(texture[0]) && Float.isFinite(texture[1])
+                && texture[0] >= 0f && texture[0] <= 1f
+                && texture[1] >= 0f && texture[1] <= 1f;
+    }
+
+    private static Boundary findConsensusBoundary(
+            Image depthImage,
+            int centerX,
+            int centerY,
+            double centerDepthMm,
+            double toleranceMm) {
+        List<Boundary> candidates = new ArrayList<>();
+        for (int offset : BOUNDARY_ROW_OFFSETS) {
+            int y = centerY + offset;
+            if (y < ROW_RADIUS || y >= depthImage.getHeight() - ROW_RADIUS) continue;
+            double rowCenterDepth = rowMedian(depthImage, centerX, y, ROW_RADIUS);
+            if (rowCenterDepth <= 0 || Math.abs(rowCenterDepth - centerDepthMm) > toleranceMm) continue;
+            double rowTolerance = clamp(rowCenterDepth * 0.08, 80.0, 220.0);
+            Boundary candidate = findDepthBoundary(depthImage, centerX, y, rowCenterDepth, rowTolerance);
+            if (candidate == null || candidate.rightX - candidate.leftX < 3) continue;
+            if (candidate.leftX <= EDGE_MARGIN_PX
+                    || candidate.rightX >= depthImage.getWidth() - 1 - EDGE_MARGIN_PX
+                    || candidate.rightX - candidate.leftX > depthImage.getWidth() * MAX_TRUNK_WIDTH_FRACTION) continue;
+            candidates.add(candidate);
+        }
+        if (candidates.size() < MIN_CONSENSUS_ROWS) return null;
+
+        List<Integer> lefts = new ArrayList<>();
+        List<Integer> rights = new ArrayList<>();
+        List<Integer> widths = new ArrayList<>();
+        List<Double> leftJumps = new ArrayList<>();
+        List<Double> rightJumps = new ArrayList<>();
+        for (Boundary candidate : candidates) {
+            lefts.add(candidate.leftX);
+            rights.add(candidate.rightX);
+            widths.add(candidate.rightX - candidate.leftX);
+            leftJumps.add(candidate.leftJumpMm);
+            rightJumps.add(candidate.rightJumpMm);
+        }
+        int left = (int) Math.round(medianInts(lefts));
+        int right = (int) Math.round(medianInts(rights));
+        int medianWidth = (int) Math.round(medianInts(widths));
+        int spread = Math.max(maxAbsoluteDeviation(lefts, left), maxAbsoluteDeviation(rights, right));
+        if (left >= right || medianWidth < 3 || spread > MAX_BOUNDARY_SPREAD_PX) return null;
+        return new Boundary(left, right, medianDoubles(leftJumps), medianDoubles(rightJumps), candidates.size(), spread);
+    }
+
+    private static Boundary findDepthBoundary(
+            Image depthImage,
+            int centerX,
+            int y,
+            double centerDepthMm,
+            double toleranceMm) {
+        int left = walkBoundary(depthImage, centerX, y, centerDepthMm, toleranceMm, -1);
+        int right = walkBoundary(depthImage, centerX, y, centerDepthMm, toleranceMm, 1);
+        if (left >= right) return null;
+        double leftOutside = outsideMedian(depthImage, left, y, -1);
+        double rightOutside = outsideMedian(depthImage, right, y, 1);
+        double leftJump = leftOutside > 0 ? Math.abs(leftOutside - centerDepthMm) : toleranceMm * 2.0;
+        double rightJump = rightOutside > 0 ? Math.abs(rightOutside - centerDepthMm) : toleranceMm * 2.0;
+        return new Boundary(left, right, leftJump, rightJump, 1, 0);
+    }
+
+    private static int walkBoundary(
+            Image depthImage,
+            int startX,
+            int y,
+            double centerDepthMm,
+            double toleranceMm,
+            int direction) {
+        int x = startX;
+        int lastGood = startX;
+        int gap = 0;
+        while (true) {
+            x += direction;
+            if (x < 0 || x >= depthImage.getWidth()) break;
+            double value = rowMedian(depthImage, x, y, ROW_RADIUS);
+            boolean matches = value > 0 && Math.abs(value - centerDepthMm) <= toleranceMm;
+            if (matches) {
+                lastGood = x;
+                gap = 0;
+            } else if (++gap > MAX_GAP_PIXELS) {
+                break;
+            }
+        }
+        return lastGood;
+    }
+
+    private static double outsideMedian(Image depthImage, int boundaryX, int y, int direction) {
+        List<Integer> values = new ArrayList<>();
+        for (int step = 2; step <= 4; step++) {
+            int x = boundaryX + direction * step;
+            if (x < 0 || x >= depthImage.getWidth()) continue;
+            int value = getDepthMm(depthImage, x, y);
+            if (value > 0) values.add(value);
+        }
+        return values.isEmpty() ? 0.0 : median(values);
+    }
+
+    private static double rowMedian(Image depthImage, int x, int y, int radiusY) {
+        List<Integer> values = new ArrayList<>();
+        for (int yy = Math.max(0, y - radiusY); yy <= Math.min(depthImage.getHeight() - 1, y + radiusY); yy++) {
+            int value = getDepthMm(depthImage, x, yy);
+            if (value > 0) values.add(value);
+        }
+        return values.size() < 2 ? 0.0 : median(values);
+    }
+
+    private static int getDepthMm(Image depthImage, int x, int y) {
+        if (x < 0 || y < 0 || x >= depthImage.getWidth() || y >= depthImage.getHeight()) return 0;
+        Image.Plane plane = depthImage.getPlanes()[0];
+        int byteIndex = x * plane.getPixelStride() + y * plane.getRowStride();
+        ByteBuffer buffer = plane.getBuffer().order(ByteOrder.nativeOrder());
+        if (byteIndex < 0 || byteIndex + 1 >= buffer.capacity()) return 0;
+        return Short.toUnsignedInt(buffer.getShort(byteIndex));
+    }
+
+    private static int getConfidence(Image confidence, int x, int y) {
+        if (x < 0 || y < 0 || x >= confidence.getWidth() || y >= confidence.getHeight()) return 0;
+        Image.Plane plane = confidence.getPlanes()[0];
+        int byteIndex = x * plane.getPixelStride() + y * plane.getRowStride();
+        ByteBuffer buffer = plane.getBuffer();
+        if (byteIndex < 0 || byteIndex >= buffer.capacity()) return 0;
+        return Byte.toUnsignedInt(buffer.get(byteIndex));
+    }
+
+    private static double medianConfidence(Image confidence, int centerX, int centerY, int radius) {
+        List<Integer> values = new ArrayList<>();
+        for (int y = Math.max(0, centerY - radius); y <= Math.min(confidence.getHeight() - 1, centerY + radius); y++) {
+            for (int x = Math.max(0, centerX - radius); x <= Math.min(confidence.getWidth() - 1, centerX + radius); x++) {
+                int value = getConfidence(confidence, x, y);
+                if (value > 0) values.add(value);
+            }
+        }
+        return values.isEmpty() ? 0.0 : median(values);
+    }
+
+    private static double median(List<Integer> values) {
+        if (values.isEmpty()) return 0.0;
+        List<Integer> sorted = new ArrayList<>(values);
+        Collections.sort(sorted);
+        int mid = sorted.size() / 2;
+        return sorted.size() % 2 == 1 ? sorted.get(mid) : (sorted.get(mid - 1) + sorted.get(mid)) / 2.0;
+    }
+
+    private static double medianInts(List<Integer> values) { return median(values); }
+
+    private static double medianDoubles(List<Double> values) {
+        if (values.isEmpty()) return 0.0;
+        List<Double> sorted = new ArrayList<>(values);
+        Collections.sort(sorted);
+        int mid = sorted.size() / 2;
+        return sorted.size() % 2 == 1 ? sorted.get(mid) : (sorted.get(mid - 1) + sorted.get(mid)) / 2.0;
+    }
+
+    private static double mad(List<Integer> values, double center) {
+        if (values.isEmpty()) return 0.0;
+        List<Integer> deviations = new ArrayList<>(values.size());
+        for (int value : values) deviations.add((int) Math.round(Math.abs(value - center)));
+        return median(deviations);
+    }
+
+    private static int maxAbsoluteDeviation(List<Integer> values, int center) {
+        int max = 0;
+        for (int value : values) max = Math.max(max, Math.abs(value - center));
+        return max;
+    }
+
+    private static double clamp(double value, double min, double max) {
+        return Math.max(min, Math.min(max, value));
+    }
+
+    private static int clampInt(int value, int min, int max) {
+        return Math.max(min, Math.min(max, value));
+    }
+
+    private static final class RawAnchor {
+        final int x;
+        final int y;
+        final double depthMm;
+        final int confidence;
+
+        RawAnchor(int x, int y, double depthMm, int confidence) {
+            this.x = x;
+            this.y = y;
+            this.depthMm = depthMm;
+            this.confidence = confidence;
+        }
+    }
+
+    private static final class Boundary {
+        final int leftX;
+        final int rightX;
+        final double leftJumpMm;
+        final double rightJumpMm;
+        final int consensusRows;
+        final int maxSpreadPx;
+
+        Boundary(int leftX, int rightX, double leftJumpMm, double rightJumpMm, int consensusRows, int maxSpreadPx) {
+            this.leftX = leftX;
+            this.rightX = rightX;
+            this.leftJumpMm = leftJumpMm;
+            this.rightJumpMm = rightJumpMm;
+            this.consensusRows = consensusRows;
+            this.maxSpreadPx = maxSpreadPx;
+        }
+    }
+}