Examples with ElevateViewInfo boofcv.struct.calib.ElevateViewInfo used on opensource projects

Example 6 with ElevateViewInfo

use of boofcv.struct.calib.ElevateViewInfo in project BoofCV by lessthanoptimal.

the class ThreeViewEstimateMetricScene method projectiveToMetric.

/**
 * Estimates the transform from projective to metric geometry
 *
 * @return true if successful
 */
boolean projectiveToMetric() {
    // homography from projective to metric
    listPinhole.clear();
    boolean successfulSelfCalibration = false;
    if (manualFocalLength <= 0) {
        // Estimate calibration parameters
        var config = new ConfigSelfCalibDualQuadratic();
        // var config = new ConfigSelfCalibEssentialGuess();
        // config.numberOfSamples = 200;
        // config.fixedFocus = true;
        // config.sampleMin = 0.6;
        // config.sampleMax = 1.5;
        ProjectiveToMetricCameras selfcalib = FactoryMultiView.projectiveToMetric(config);
        List<ElevateViewInfo> views = new ArrayList<>();
        for (int i = 0; i < 3; i++) {
            views.add(new ElevateViewInfo(width, height, i));
        }
        List<DMatrixRMaj> cameras = new ArrayList<>();
        cameras.add(P2);
        cameras.add(P3);
        DogArray<AssociatedTuple> observations = new DogArray<>(() -> new AssociatedTupleN(3));
        MultiViewOps.convertTr(ransac.getMatchSet(), observations);
        var results = new MetricCameras();
        boolean success = selfcalib.process(views, cameras, observations.toList(), results);
        if (success) {
            successfulSelfCalibration = true;
            listPinhole.addAll(results.intrinsics.toList());
            listWorldToView.get(0).reset();
            listWorldToView.get(1).setTo(results.motion_1_to_k.get(0));
            listWorldToView.get(2).setTo(results.motion_1_to_k.get(1));
            if (verbose != null)
                verbose.println("Auto calibration success");
        } else {
            if (verbose != null)
                verbose.println("Auto calibration failed");
        }
    }
    if (!successfulSelfCalibration) {
        // Use provided focal length or guess using an "average" focal length across cameras
        double focalLength = manualFocalLength <= 0 ? (double) (Math.max(width, height) / 2) : manualFocalLength;
        if (verbose != null)
            verbose.println("Assuming fixed focal length for all views. f=" + focalLength);
        final var estimateH = new TwoViewToCalibratingHomography();
        DMatrixRMaj F21 = MultiViewOps.projectiveToFundamental(P2, null);
        estimateH.initialize(F21, P2);
        DMatrixRMaj K = PerspectiveOps.pinholeToMatrix(focalLength, focalLength, 0, 0, 0);
        DogArray<AssociatedPair> pairs = new DogArray<>(AssociatedPair::new);
        MultiViewOps.convertTr(ransac.getMatchSet(), 0, 1, pairs);
        if (!estimateH.process(K, K, pairs.toList()))
            throw new RuntimeException("Failed to estimate H given 'known' intrinsics");
        // Use the found calibration homography to find motion estimates
        DMatrixRMaj H = estimateH.getCalibrationHomography();
        listPinhole.clear();
        for (int i = 0; i < 3; i++) {
            listPinhole.add(PerspectiveOps.matrixToPinhole(K, width, height, null));
        }
        listWorldToView.get(0).reset();
        MultiViewOps.projectiveToMetric(P2, H, listWorldToView.get(1), K);
        MultiViewOps.projectiveToMetric(P3, H, listWorldToView.get(2), K);
    }
    if (verbose != null) {
        verbose.println("Initial Intrinsic Estimate:");
        for (int i = 0; i < 3; i++) {
            CameraPinhole r = listPinhole.get(i);
            verbose.printf("  fx = %6.1f, fy = %6.1f, skew = %6.3f\n", r.fx, r.fy, r.skew);
        }
        verbose.println("Initial Motion Estimate:");
    }
    // scale is arbitrary. Set max translation to 1
    double maxT = 0;
    for (int i = 0; i < listWorldToView.size(); i++) {
        Se3_F64 world_to_view = listWorldToView.get(i);
        maxT = Math.max(maxT, world_to_view.T.norm());
    }
    for (int i = 0; i < listWorldToView.size(); i++) {
        Se3_F64 world_to_view = listWorldToView.get(i);
        world_to_view.T.scale(1.0 / maxT);
        if (verbose != null) {
            Rodrigues_F64 rod = ConvertRotation3D_F64.matrixToRodrigues(world_to_view.R, null);
            verbose.println("  T=" + world_to_view.T + "  R=" + rod);
        }
    }
    return true;
}

Example 7 with ElevateViewInfo

use of boofcv.struct.calib.ElevateViewInfo in project BoofCV by lessthanoptimal.

the class ProjectiveToMetricCameraDualQuadratic method averageCommonCameras.

/**
 * If multiple views use the same camera the found intrinsics will be averaged across those views.
 */
void averageCommonCameras(List<ElevateViewInfo> views, FastAccess<SelfCalibrationLinearDualQuadratic.Intrinsic> solutions, int numCameras) {
    cameraCounts.resetResize(numCameras, 0);
    workCameras.resetResize(numCameras);
    for (int i = 0; i < views.size(); i++) {
        ElevateViewInfo info = views.get(i);
        CameraPinhole merged = workCameras.get(info.cameraID);
        SelfCalibrationLinearDualQuadratic.Intrinsic estimated = solutions.get(i);
        merged.fx += estimated.fx;
        merged.fy += estimated.fy;
        merged.skew += estimated.skew;
        cameraCounts.data[info.cameraID]++;
        if (verbose != null) {
            verbose.printf("view[%d] fx=%.1f fy=%.1f skew=%.2f\n", i, estimated.fx, estimated.fy, estimated.skew);
        }
    }
    for (int i = 0; i < numCameras; i++) {
        CameraPinhole merged = workCameras.get(i);
        int divisor = cameraCounts.get(i);
        if (divisor == 1)
            continue;
        merged.fx /= divisor;
        merged.fy /= divisor;
        merged.skew /= divisor;
        // Principle point must be zero. This is here to emphasize that
        merged.cx = 0.0;
        merged.cy = 0.0;
    }
    if (verbose != null) {
        for (int i = 0; i < numCameras; i++) {
            CameraPinhole cam = workCameras.get(i);
            verbose.printf("camera[%d] fx=%.1f fy=%.1f skew=%.2f, count=%d\n", i, cam.fx, cam.fy, cam.skew, cameraCounts.get(i));
        }
    }
}

Example 8 with ElevateViewInfo

use of boofcv.struct.calib.ElevateViewInfo in project BoofCV by lessthanoptimal.

the class ProjectiveToMetricCameraDualQuadratic method refineCamerasAlgebraic.

/**
 * This refines intrinsic parameters by minimizing algebraic error. If anything goes wrong it doesn't update
 * the intrinsics. Also updates the rectifying homography.
 */
void refineCamerasAlgebraic(List<ElevateViewInfo> views, List<DMatrixRMaj> cameraMatrices, int numCameras) {
    // Refiner can't handle non-zero skew yet
    if (!selfCalib.zeroSkew) {
        if (verbose != null)
            verbose.println("Skipping refine since skew is not zero");
        return;
    }
    // Just skip everything if it has been turned off
    if (refiner.converge.maxIterations <= 0)
        return;
    // Sanity check the P0 is implicit
    BoofMiscOps.checkEq(views.size(), cameraMatrices.size() + 1);
    // Make sure refiner applies the same constraints that the linear estimator applies
    refiner.knownAspect = selfCalib.isKnownAspect();
    refiner.knownPrinciplePoint = true;
    // Configure the refiner. If multiple views use the same camera this constraint is applied
    refiner.initialize(numCameras, views.size());
    DMatrix3 planeAtInfinity = selfCalib.getPlaneAtInfinity();
    refiner.setPlaneAtInfinity(planeAtInfinity.a1, planeAtInfinity.a2, planeAtInfinity.a3);
    for (int cameraIdx = 0; cameraIdx < numCameras; cameraIdx++) {
        CameraPinhole merged = workCameras.get(cameraIdx);
        refiner.setCamera(cameraIdx, merged.fx, merged.cx, merged.cy, merged.fy / merged.fx);
    }
    for (int viewIdx = 0; viewIdx < views.size(); viewIdx++) {
        if (viewIdx == 0)
            refiner.setProjective(0, P1);
        else
            refiner.setProjective(viewIdx, cameraMatrices.get(viewIdx - 1));
        refiner.setViewToCamera(viewIdx, views.get(viewIdx).cameraID);
    }
    // Refine and change nothing if it fails
    if (!refiner.refine()) {
        if (verbose != null)
            verbose.println("Refine failed! Ignoring results");
        return;
    }
    if (verbose != null) {
        for (int i = 0; i < numCameras; i++) {
            CameraState refined = refiner.getCameras().get(i);
            verbose.printf("refined[%d] fx=%.1f fy=%.1f\n", i, refined.fx, refined.fx * refined.aspectRatio);
        }
    }
    // Save the refined intrinsic parameters
    for (int i = 0; i < views.size(); i++) {
        ElevateViewInfo info = views.get(i);
        CameraState refined = refiner.getCameras().get(info.cameraID);
        CameraPinhole estimated = workCameras.get(info.cameraID);
        estimated.fx = refined.fx;
        estimated.fy = refined.aspectRatio * refined.fx;
    // refiner doesn't support non-zero skew yet
    }
    // Update rectifying homography using the new parameters
    // NOTE: This formulation of H requires P1=[I|0] which is true in this case
    PerspectiveOps.pinholeToMatrix(workCameras.get(views.get(0).cameraID), K);
    MultiViewOps.canonicalRectifyingHomographyFromKPinf(K, refiner.planeAtInfinity, H);
}

Example 9 with ElevateViewInfo

use of boofcv.struct.calib.ElevateViewInfo in project BoofCV by lessthanoptimal.

the class CommonProjectiveToMetricCamerasChecks method real_world_case0.

/**
 * In this situation a scene was created where points appeared behind the camera. Taken from real data
 */
@Test
void real_world_case0() {
    DMatrixRMaj P2 = new DMatrixRMaj(3, 4, true, 71.2714309, -1.50598476, -354.50553, -.052935998, -1.28683386, 39.1891727, 672.658283, -.994592935, .00056663, -.019338274, 70.0397946, -.000445996);
    DMatrixRMaj P3 = new DMatrixRMaj(3, 4, true, 32.4647875, -1.02054892, -241.805355, -.054715714, -1.8370892, -.061992654, .486096194, -1.00684043, .000185405, -.010046842, 31.8668685, -.000209807);
    DogArray<AssociatedTuple> observations = new DogArray<>(() -> new AssociatedTupleN(3));
    // These are in front of both cameras
    add(-47.208221435546875, -14.024078369140625, -49.9302978515625, 36.35797119140625, -50.079071044921875, 77.59286499023438, observations);
    add(-203.9057159423828, 70.39932250976562, -207.64544677734375, 124.38552856445312, -206.31866455078125, 172.38186645507812, observations);
    add(-362.7781524658203, -218.54442596435547, -361.6542053222656, -160.6702880859375, -363.30285263061523, -107.35969543457031, observations);
    add(-154.99310302734375, 3.35784912109375, -158.14512634277344, 55.362579345703125, -157.7862548828125, 100.77597045898438, observations);
    add(-170.89407348632812, -181.27266693115234, -172.10398864746094, -127.54672241210938, -174.48524475097656, -81.65957641601562, observations);
    add(41.3905029296875, 170.15188598632812, 39.365081787109375, 221.3468017578125, 43.634307861328125, 261.2353515625, observations);
    add(-350.1354789733887, -229.5992660522461, -349.162899017334, -171.76145935058594, -351.1237335205078, -118.83564758300781, observations);
    add(-50.12109375, -14.451873779296875, -52.87139892578125, 35.835052490234375, -53.014801025390625, 77.25506591796875, observations);
    add(-250.23069763183594, -212.5504379272461, -250.5589599609375, -156.41912841796875, -252.87100219726562, -107.27978515625, observations);
    // These are behind at least one camera
    add(154.89532470703125, -21.821807861328125, 151.21435546875, 41.2327880859375, 151.974365234375, 93.64697265625, observations);
    add(226.85003662109375, -95.77021789550781, 221.5345458984375, -35.9564208984375, 219.90155029296875, 12.154052734375, observations);
    add(237.870361328125, -46.12437438964844, 232.88519287109375, 13.570709228515625, 232.98577880859375, 61.028564453125, observations);
    add(162.7314453125, -165.1600341796875, 156.9556884765625, -99.56578063964844, 154.2447509765625, -45.94012451171875, observations);
    add(283.9959716796875, -147.1155242919922, 276.13848876953125, -86.35987854003906, 273.4132080078125, -40.23883056640625, observations);
    add(135.57574462890625, -232.8561019897461, 129.67437744140625, -163.39407348632812, 125.60736083984375, -107.20663452148438, observations);
    add(-21.8720703125, -162.5299530029297, -24.70025634765625, -101.63801574707031, -27.263427734375, -50.05320739746094, observations);
    add(62.40008544921875, -173.78022003173828, 59.92376708984375, -105.06491088867188, 56.91351318359375, -45.15827941894531, observations);
    add(-63.860626220703125, -259.0756492614746, -65.89141845703125, -195.2255096435547, -69.55535888671875, -142.1841278076172, observations);
    List<ElevateViewInfo> views = new ArrayList<>();
    for (int i = 0; i < 3; i++) {
        views.add(new ElevateViewInfo(800, 600, i));
    }
    List<DMatrixRMaj> inputCameras = new ArrayList<>();
    inputCameras.add(P2);
    inputCameras.add(P3);
    var results = new MetricCameras();
    ProjectiveToMetricCameras alg = createEstimator(false);
    assertTrue(alg.process(views, inputCameras, observations.toList(), results));
    // Yes internally most implementations run this function, but the number of invalid was > 0 before
    var checkMatches = new ResolveSignAmbiguityPositiveDepth();
    checkMatches.process(observations.toList(), results);
    assertFalse(checkMatches.signChanged);
    assertEquals(0, checkMatches.bestInvalid);
}

Example 10 with ElevateViewInfo

use of boofcv.struct.calib.ElevateViewInfo in project BoofCV by lessthanoptimal.

the class CommonProjectiveToMetricCamerasChecks method unexpected_number_of_cameras.

/**
 * The implicit camera was added. it should fail
 */
@Test
void unexpected_number_of_cameras() {
    standardScene();
    simulateScene(0);
    List<ElevateViewInfo> views = new ArrayList<>();
    List<DMatrixRMaj> inputCameras = new ArrayList<>();
    for (int i = 0; i < 3; i++) {
        views.add(new ElevateViewInfo(imageWidth, imageHeight, i));
    }
    // extra camera here
    inputCameras.add(P2);
    inputCameras.add(P2);
    inputCameras.add(P3);
    ProjectiveToMetricCameras alg = createEstimator(false);
    assertThrows(RuntimeException.class, () -> alg.process(views, inputCameras, observationsN, new MetricCameras()));
}