Examples with DogArray org.ddogleg.struct.DogArray used on opensource projects

Example 71 with DogArray

use of org.ddogleg.struct.DogArray in project BoofCV by lessthanoptimal.

the class GenericDetectMultiFiducialCalibrationChecks method basicPinhole.

@Test
void basicPinhole() {
    DetectMultiFiducialCalibration detector = createDetector();
    // CameraPinholeBrown model = CalibrationIO.load(getClass().getResource("pinhole_radial.yaml"));
    CameraPinholeBrown model = new CameraPinholeBrown().fsetK(600, 600, 0, 400, 400, 800, 800);
    SimulatePlanarWorld simulator = new SimulatePlanarWorld();
    simulator.setCamera(model);
    DogArray<List<PointIndex2D_F64>> markerPoints = new DogArray<>(ArrayList::new);
    for (int i = 0; i < Math.min(2, detector.getTotalUniqueMarkers()); i++) {
        GrayF32 pattern = renderPattern(i, markerPoints.grow());
        Se3_F64 markerToWorld = SpecialEuclideanOps_F64.eulerXyz((-0.5 + i) * 0.32, 0, .5, 0, Math.PI, 0, null);
        simulator.addSurface(markerToWorld, simulatedTargetWidth, pattern);
    }
    // marker systems since that requires encoding that should remove that ambiguity
    for (int cameraOriIdx = 0; cameraOriIdx < 10; cameraOriIdx++) {
        double roll = 2.0 * Math.PI * cameraOriIdx / 10;
        simulator.setWorldToCamera(SpecialEuclideanOps_F64.eulerXyz(0, 0, 0, 0, 0, roll, null));
        simulator.render();
        // Process the image and see if it detected everything
        detector.process(simulator.getOutput());
        // ShowImages.showWindow(simulator.getOutput(), "Rotated "+cameraOriIdx);
        // BoofMiscOps.sleep(2_000);
        // Number of markers which are not anonymous
        int totalIdentified = 0;
        for (int i = 0; i < detector.getDetectionCount(); i++) {
            if (detector.getMarkerID(i) >= 0)
                totalIdentified++;
        }
        if (visualizeFailures && 2 != totalIdentified) {
            UtilImageIO.saveImage(simulator.getOutput(), "failed.png");
            ShowImages.showWindow(simulator.getOutput(), "Simulated");
            BoofMiscOps.sleep(10_000);
        }
        assertEquals(2, totalIdentified);
        for (int i = 0; i < detector.getDetectionCount(); i++) {
            int markerID = detector.getMarkerID(i);
            // Ignore anonymous markers
            if (markerID < 0)
                continue;
            // See if it detected the expected number of calibration points on the marker
            assertEquals(markerPoints.get(markerID).size(), detector.getDetectedPoints(i).size());
        // TODO check the actual coordinates
        }
    }
}

Example 72 with DogArray

use of org.ddogleg.struct.DogArray in project BoofCV by lessthanoptimal.

the class TestChessboardCornerClusterToGrid method createGrid.

private DogArray<Node> createGrid(int rows, int cols, boolean cornerSquare) {
    DogArray<Node> corners = new DogArray<>(Node::new);
    // declare the grid
    for (int row = 0; row < rows; row++) {
        for (int col = 0; col < cols; col++) {
            Node n = corners.grow();
            n.index = row * cols + col;
            n.corner = new ChessboardCorner();
            n.corner.x = col * 30;
            n.corner.y = row * 30;
            n.corner.orientation = Math.PI / 4;
            if ((row % 2 + col % 2) == 1)
                n.corner.orientation -= Math.PI / 2;
            if (!cornerSquare)
                n.corner.orientation *= -1;
            if (row > 0) {
                Node p = corners.get((row - 1) * cols + col);
                p.edges[1] = n;
                n.edges[3] = p;
            }
            if (col > 0) {
                Node p = corners.get(row * cols + col - 1);
                p.edges[0] = n;
                n.edges[2] = p;
            }
        }
    }
    return corners;
}

Example 73 with DogArray

use of org.ddogleg.struct.DogArray in project BoofCV by lessthanoptimal.

the class TestChessboardCornerClusterToGrid method sortEdgesCCW_shift.

/**
 * Need to shift elements after sorting to ensure edges are every 90 degrees
 */
@Test
void sortEdgesCCW_shift() {
    DogArray<Node> corners = new DogArray<>(Node::new);
    // Add nodes with 3 edges
    for (int jump = 0; jump < 3; jump++) {
        Node target = corners.grow();
        target.corner = new ChessboardCorner();
        target.corner.x = 10;
        target.corner.y = 12;
        for (int i = 0; i < 3; i++) {
            double theta = i * Math.PI / 2;
            if (i > jump)
                theta += Math.PI / 2;
            double c = Math.cos(theta);
            double s = Math.sin(theta);
            double r = 4;
            Node n = new Node();
            n.corner = new ChessboardCorner();
            n.corner.x = target.getX() + r * c;
            n.corner.y = target.getY() + r * s;
            target.edges[i] = n;
        }
        // shuffle to make it a better test
        shuffle(target.edges);
    }
    // add Nodes with two edges
    for (int count = 0; count < 10; count++) {
        Node target = corners.grow();
        target.corner = new ChessboardCorner();
        target.corner.x = 10;
        target.corner.y = 12;
        for (int i = 0; i < 2; i++) {
            double theta = i * Math.PI / 2;
            double c = Math.cos(theta);
            double s = Math.sin(theta);
            double r = 4;
            Node n = new Node();
            n.corner = new ChessboardCorner();
            n.corner.x = target.getX() + r * c;
            n.corner.y = target.getY() + r * s;
            target.edges[i] = n;
        }
        // shuffle to make it a better test
        shuffle(target.edges);
    }
    ChessboardCornerClusterToGrid alg = new ChessboardCornerClusterToGrid();
    alg.sortEdgesCCW(corners);
    for (Node n : corners.toList()) {
        Node m0 = n.edges[0];
        double theta0 = Math.atan2(m0.getY() - n.getY(), m0.getX() - n.getX());
        for (int i = 0; i < 4; i++) {
            Node m = n.edges[i];
            if (m == null)
                continue;
            double thetaI = Math.atan2(m.getY() - n.getY(), m.getX() - n.getX());
            assertEquals(i * Math.PI / 2.0, UtilAngle.distanceCCW(theta0, thetaI), 0.001);
        }
    }
}

Example 74 with DogArray

use of org.ddogleg.struct.DogArray in project BoofCV by lessthanoptimal.

the class TestChessboardCornerClusterToGrid method sortEdgesCCW.

@Test
void sortEdgesCCW() {
    DogArray<Node> corners = new DogArray<>(Node::new);
    for (int nodeIdx = 0; nodeIdx < 6; nodeIdx++) {
        Node target = corners.grow();
        target.corner = new ChessboardCorner();
        target.corner.x = 10;
        target.corner.y = 12;
        // always 3 edges 90 degrees apart
        for (int i = 0; i < 3; i++) {
            double theta = -(i + nodeIdx) * Math.PI / 2.0 + nodeIdx;
            double c = Math.cos(theta);
            double s = Math.sin(theta);
            double r = 4;
            Node n = new Node();
            n.corner = new ChessboardCorner();
            n.corner.x = target.getX() + r * c;
            n.corner.y = target.getY() + r * s;
            target.edges[i + 1] = n;
        }
        shuffle(target.edges);
    }
    ChessboardCornerClusterToGrid alg = new ChessboardCornerClusterToGrid();
    alg.sortEdgesCCW(corners);
    for (Node n : corners.toList()) {
        int tested = 0;
        for (int i = 0; i < 4; i++) {
            int j = (i + 1) % 4;
            if (n.edges[i] == null || n.edges[j] == null)
                continue;
            Node m0 = n.edges[i];
            Node m1 = n.edges[j];
            double theta0 = Math.atan2(m0.getY() - n.getY(), m0.getX() - n.getX());
            double theta1 = Math.atan2(m1.getY() - n.getY(), m1.getX() - n.getX());
            double diff = UtilAngle.distanceCCW(theta0, theta1);
            assertTrue(UtilAngle.dist(diff, Math.PI / 2.0) < 0.01);
            tested++;
        }
        assertEquals(2, tested);
    }
}

Example 75 with DogArray

use of org.ddogleg.struct.DogArray in project BoofCV by lessthanoptimal.

the class TestProjectiveInitializeAllCommon method lookupInfoForMetricElevation.

/**
 * Looks up features then tests using triangulation and checks for perfect results.
 */
@Test
void lookupInfoForMetricElevation() {
    int numViews = 4;
    var dbSimilar = new MockLookupSimilarImages(numViews, 0xDEADBEEF);
    var dbCams = new MockLookUpCameraInfo(800, 800);
    var alg = new ProjectiveInitializeAllCommon();
    alg.utils.dbSimilar = dbSimilar;
    alg.utils.dbCams = dbCams;
    // sanity check that all features are visible in all views. Requirement of metric escalation
    for (int i = 0; i < dbSimilar.viewIds.size(); i++) {
        assertEquals(dbSimilar.numFeatures, dbSimilar.viewObs.get(i).size());
    }
    // dividing number of features by two because only even observations are inliers
    alg.utils.structure.initialize(numViews, dbSimilar.numFeatures / 2);
    alg.utils.observations.initialize(numViews);
    // Transform that makes view[0] identity
    DMatrixRMaj H = new DMatrixRMaj(4, 4);
    MultiViewOps.projectiveToIdentityH(dbSimilar.listCameraMatrices.get(0), H);
    // construct projective SBA scene from metric ground truth
    for (int viewIdx = 0; viewIdx < dbSimilar.viewIds.size(); viewIdx++) {
        DMatrixRMaj P = new DMatrixRMaj(3, 4);
        CommonOps_DDRM.mult(dbSimilar.listCameraMatrices.get(viewIdx), H, P);
        alg.viewsByStructureIndex.add(dbSimilar.graph.nodes.get(viewIdx));
        alg.utils.structure.views.get(viewIdx).worldToView.setTo(P);
        alg.utils.structure.views.get(viewIdx).width = viewIdx;
        // only features with an even ID will be inliers
        DogArray_I32 inliers = alg.inlierIndexes.grow();
        int[] featureIDs = dbSimilar.viewToFeat.get(viewIdx);
        SceneObservations.View oview = alg.utils.observations.views.get(viewIdx);
        for (int obsIdx = 0; obsIdx < featureIDs.length; obsIdx++) {
            int featureID = featureIDs[obsIdx];
            if (featureID % 2 == 1)
                continue;
            inliers.add(oview.size());
            Point2D_F64 pixel = dbSimilar.viewObs.get(viewIdx).get(obsIdx);
            oview.add(featureID / 2, (float) pixel.x, (float) pixel.y);
        }
    }
    // Call the function we are testing
    List<String> viewIds = new ArrayList<>();
    DogArray<ElevateViewInfo> views = new DogArray<>(ElevateViewInfo::new);
    DogArray<DMatrixRMaj> cameraMatrices = new DogArray<>(() -> new DMatrixRMaj(3, 4));
    DogArray<AssociatedTupleDN> observations = new DogArray<>(AssociatedTupleDN::new);
    alg.lookupInfoForMetricElevation(viewIds, views, cameraMatrices, observations);
    // check what can be checked trivially by comparing to the db
    assertEquals(4, viewIds.size());
    assertEquals(4, views.size());
    assertEquals(3, cameraMatrices.size());
    assertEquals(dbSimilar.numFeatures / 2, observations.size());
    for (int viewIdx = 0; viewIdx < 4; viewIdx++) {
        assertEquals(dbSimilar.viewIds.get(viewIdx), viewIds.get(viewIdx));
        assertEquals(viewIdx, views.get(viewIdx).shape.width);
        // only one camera
        assertEquals(0, views.get(viewIdx).cameraID);
    }
    // See if it unscrambled the observations
    for (int obsIdx = 0; obsIdx < dbSimilar.numFeatures / 2; obsIdx++) {
        for (int viewIdx = 0; viewIdx < 4; viewIdx++) {
            Point2D_F64 expected = dbSimilar.viewObs.get(viewIdx).get(dbSimilar.featToView.get(viewIdx)[obsIdx * 2]);
            Point2D_F64 found = observations.get(obsIdx).get(viewIdx);
            assertEquals(0.0, expected.distance(found), UtilEjml.TEST_F32);
        }
    }
}