Search in sources :

Example 1 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified in project BoofCV by lessthanoptimal.

the class ExampleStereoUncalibrated method main.

public static void main(String[] args) {
    // Solution below is unstable. Turning concurrency off so that it always produces a valid solution
    // The two view case is very challenging and I've not seen a stable algorithm yet
    BoofConcurrency.USE_CONCURRENT = false;
    // Successful
    String name = "bobcats_";
    // String name = "mono_wall_";
    // String name = "minecraft_cave1_";
    // String name = "chicken_";
    // String name = "books_";
    // Successful Failures
    // String name = "triflowers_";
    // Failures
    // String name = "rock_leaves_";
    // String name = "minecraft_distant_";
    // String name = "rockview_";
    // String name = "pebbles_";
    // String name = "skull_";
    // String name = "turkey_";
    BufferedImage buff01 = UtilImageIO.loadImageNotNull(UtilIO.pathExample("triple/" + name + "01.jpg"));
    BufferedImage buff02 = UtilImageIO.loadImageNotNull(UtilIO.pathExample("triple/" + name + "02.jpg"));
    Planar<GrayU8> color01 = ConvertBufferedImage.convertFrom(buff01, true, ImageType.pl(3, GrayU8.class));
    Planar<GrayU8> color02 = ConvertBufferedImage.convertFrom(buff02, true, ImageType.pl(3, GrayU8.class));
    GrayU8 image01 = ConvertImage.average(color01, null);
    GrayU8 image02 = ConvertImage.average(color02, null);
    // Find a set of point feature matches
    List<AssociatedPair> matches = ExampleComputeFundamentalMatrix.computeMatches(buff01, buff02);
    // Prune matches using the epipolar constraint. use a low threshold to prune more false matches
    var inliers = new ArrayList<AssociatedPair>();
    DMatrixRMaj F = ExampleComputeFundamentalMatrix.robustFundamental(matches, inliers, 0.1);
    // Perform self calibration using the projective view extracted from F
    // Note that P1 = [I|0]
    System.out.println("Self calibration");
    DMatrixRMaj P2 = MultiViewOps.fundamentalToProjective(F);
    // Take a crude guess at the intrinsic parameters. Bundle adjustment will fix this later.
    int width = buff01.getWidth(), height = buff02.getHeight();
    double fx = width / 2;
    double fy = fx;
    double cx = width / 2;
    double cy = height / 2;
    // Compute a transform from projective to metric by assuming we know the camera's calibration
    var estimateV = new EstimatePlaneAtInfinityGivenK();
    estimateV.setCamera1(fx, fy, 0, cx, cy);
    estimateV.setCamera2(fx, fy, 0, cx, cy);
    // plane at infinity
    var v = new Vector3D_F64();
    if (!estimateV.estimatePlaneAtInfinity(P2, v))
        throw new RuntimeException("Failed!");
    DMatrixRMaj K = PerspectiveOps.pinholeToMatrix(fx, fy, 0, cx, cy);
    DMatrixRMaj H = MultiViewOps.createProjectiveToMetric(K, v.x, v.y, v.z, 1, null);
    var P2m = new DMatrixRMaj(3, 4);
    CommonOps_DDRM.mult(P2, H, P2m);
    // Decompose and get the initial estimate for translation
    var tmp = new DMatrixRMaj(3, 3);
    var view1_to_view2 = new Se3_F64();
    MultiViewOps.decomposeMetricCamera(P2m, tmp, view1_to_view2);
    // ------------------------- Setting up bundle adjustment
    // bundle adjustment will provide a more refined and accurate estimate of these parameters
    System.out.println("Configuring bundle adjustment");
    // Construct bundle adjustment data structure
    var structure = new SceneStructureMetric(false);
    var observations = new SceneObservations();
    // We will assume that the camera has fixed intrinsic parameters
    structure.initialize(1, 2, inliers.size());
    observations.initialize(2);
    var bp = new BundlePinholeSimplified();
    bp.f = fx;
    structure.setCamera(0, false, bp);
    // The first view is the world coordinate system
    structure.setView(0, 0, true, new Se3_F64());
    // Second view was estimated previously
    structure.setView(1, 0, false, view1_to_view2);
    for (int i = 0; i < inliers.size(); i++) {
        AssociatedPair t = inliers.get(i);
        // substract out the camera center from points. This allows a simple camera model to be used and
        // errors in the this coordinate tend to be non-fatal
        observations.getView(0).add(i, (float) (t.p1.x - cx), (float) (t.p1.y - cy));
        observations.getView(1).add(i, (float) (t.p2.x - cx), (float) (t.p2.y - cy));
        // each point is visible in both of the views
        structure.connectPointToView(i, 0);
        structure.connectPointToView(i, 1);
    }
    // initial location of points is found through triangulation
    MultiViewOps.triangulatePoints(structure, observations);
    // ------------------ Running Bundle Adjustment
    System.out.println("Performing bundle adjustment");
    var configLM = new ConfigLevenbergMarquardt();
    configLM.dampeningInitial = 1e-3;
    configLM.hessianScaling = false;
    var configSBA = new ConfigBundleAdjustment();
    configSBA.configOptimizer = configLM;
    // Create and configure the bundle adjustment solver
    BundleAdjustment<SceneStructureMetric> bundleAdjustment = FactoryMultiView.bundleSparseMetric(configSBA);
    // prints out useful debugging information that lets you know how well it's converging
    bundleAdjustment.setVerbose(System.out, null);
    // Specifies convergence criteria
    bundleAdjustment.configure(1e-6, 1e-6, 100);
    // Scaling improve accuracy of numerical calculations
    var bundleScale = new ScaleSceneStructure();
    bundleScale.applyScale(structure, observations);
    bundleAdjustment.setParameters(structure, observations);
    bundleAdjustment.optimize(structure);
    // Sometimes pruning outliers help improve the solution. In the stereo case the errors are likely
    // to already fatal
    var pruner = new PruneStructureFromSceneMetric(structure, observations);
    pruner.pruneObservationsByErrorRank(0.85);
    pruner.prunePoints(1);
    bundleAdjustment.setParameters(structure, observations);
    bundleAdjustment.optimize(structure);
    bundleScale.undoScale(structure, observations);
    System.out.println("\nCamera");
    for (int i = 0; i < structure.cameras.size; i++) {
        System.out.println(structure.cameras.data[i].getModel().toString());
    }
    System.out.println("\n\nworldToView");
    for (int i = 0; i < structure.views.size; i++) {
        System.out.println(structure.getParentToView(i).toString());
    }
    // display the inlier matches found using the robust estimator
    System.out.println("\n\nComputing Stereo Disparity");
    BundlePinholeSimplified cp = structure.getCameras().get(0).getModel();
    var intrinsic = new CameraPinholeBrown();
    intrinsic.fsetK(cp.f, cp.f, 0, cx, cy, width, height);
    intrinsic.fsetRadial(cp.k1, cp.k2);
    Se3_F64 leftToRight = structure.getParentToView(1);
    computeStereoCloud(image01, image02, color01, color02, intrinsic, intrinsic, leftToRight, 0, 250);
}
Also used : AssociatedPair(boofcv.struct.geo.AssociatedPair) EstimatePlaneAtInfinityGivenK(boofcv.alg.geo.selfcalib.EstimatePlaneAtInfinityGivenK) CameraPinholeBrown(boofcv.struct.calib.CameraPinholeBrown) BundlePinholeSimplified(boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified) ArrayList(java.util.ArrayList) DMatrixRMaj(org.ejml.data.DMatrixRMaj) BufferedImage(java.awt.image.BufferedImage) ConvertBufferedImage(boofcv.io.image.ConvertBufferedImage) ConfigBundleAdjustment(boofcv.factory.geo.ConfigBundleAdjustment) Vector3D_F64(georegression.struct.point.Vector3D_F64) ConfigLevenbergMarquardt(org.ddogleg.optimization.lm.ConfigLevenbergMarquardt) GrayU8(boofcv.struct.image.GrayU8) Se3_F64(georegression.struct.se.Se3_F64)

Example 2 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified in project BoofCV by lessthanoptimal.

the class ExampleTrifocalStereoUncalibrated method main.

public static void main(String[] args) {
    String name = "rock_leaves_";
    // String name = "mono_wall_";
    // String name = "minecraft_cave1_";
    // String name = "minecraft_distant_";
    // String name = "bobcats_";
    // String name = "chicken_";
    // String name = "turkey_";
    // String name = "rockview_";
    // String name = "pebbles_";
    // String name = "books_";
    // String name = "skull_";
    // String name = "triflowers_";
    BufferedImage buff01 = UtilImageIO.loadImageNotNull(UtilIO.pathExample("triple/" + name + "01.jpg"));
    BufferedImage buff02 = UtilImageIO.loadImageNotNull(UtilIO.pathExample("triple/" + name + "02.jpg"));
    BufferedImage buff03 = UtilImageIO.loadImageNotNull(UtilIO.pathExample("triple/" + name + "03.jpg"));
    Planar<GrayU8> color01 = ConvertBufferedImage.convertFrom(buff01, true, ImageType.pl(3, GrayU8.class));
    Planar<GrayU8> color02 = ConvertBufferedImage.convertFrom(buff02, true, ImageType.pl(3, GrayU8.class));
    Planar<GrayU8> color03 = ConvertBufferedImage.convertFrom(buff03, true, ImageType.pl(3, GrayU8.class));
    GrayU8 image01 = ConvertImage.average(color01, null);
    GrayU8 image02 = ConvertImage.average(color02, null);
    GrayU8 image03 = ConvertImage.average(color03, null);
    // using SURF features. Robust and fairly fast to compute
    DetectDescribePoint<GrayU8, TupleDesc_F64> detDesc = FactoryDetectDescribe.surfStable(new ConfigFastHessian(0, 4, 1000, 1, 9, 4, 2), null, null, GrayU8.class);
    // Associate features across all three views using previous example code
    var associateThree = new ExampleAssociateThreeView();
    associateThree.initialize(detDesc);
    associateThree.detectFeatures(image01, 0);
    associateThree.detectFeatures(image02, 1);
    associateThree.detectFeatures(image03, 2);
    System.out.println("features01.size = " + associateThree.features01.size);
    System.out.println("features02.size = " + associateThree.features02.size);
    System.out.println("features03.size = " + associateThree.features03.size);
    int width = image01.width, height = image01.height;
    System.out.println("Image Shape " + width + " x " + height);
    double cx = width / 2;
    double cy = height / 2;
    // The self calibration step requires that the image coordinate system be in the image center
    associateThree.locations01.forEach(p -> p.setTo(p.x - cx, p.y - cy));
    associateThree.locations02.forEach(p -> p.setTo(p.x - cx, p.y - cy));
    associateThree.locations03.forEach(p -> p.setTo(p.x - cx, p.y - cy));
    // Converting data formats for the found features into what can be processed by SFM algorithms
    // Notice how the image center is subtracted from the coordinates? In many cases a principle point
    // of zero is assumed. This is a reasonable assumption in almost all modern cameras. Errors in
    // the principle point tend to materialize as translations and are non fatal.
    // Associate features in the three views using image information alone
    DogArray<AssociatedTripleIndex> associatedIdx = associateThree.threeViewPairwiseAssociate();
    // Convert the matched indexes into AssociatedTriple which contain the actual pixel coordinates
    var associated = new DogArray<>(AssociatedTriple::new);
    associatedIdx.forEach(p -> associated.grow().setTo(associateThree.locations01.get(p.a), associateThree.locations02.get(p.b), associateThree.locations03.get(p.c)));
    System.out.println("Total Matched Triples = " + associated.size);
    var model = new TrifocalTensor();
    List<AssociatedTriple> inliers = ExampleComputeTrifocalTensor.computeTrifocal(associated, model);
    System.out.println("Remaining after RANSAC " + inliers.size());
    // Show remaining associations from RANSAC
    var triplePanel = new AssociatedTriplePanel();
    triplePanel.setPixelOffset(cx, cy);
    triplePanel.setImages(buff01, buff02, buff03);
    triplePanel.setAssociation(inliers);
    ShowImages.showWindow(triplePanel, "Associations", true);
    // estimate using all the inliers
    // No need to re-scale the input because the estimator automatically adjusts the input on its own
    var configTri = new ConfigTrifocal();
    configTri.which = EnumTrifocal.ALGEBRAIC_7;
    configTri.converge.maxIterations = 100;
    Estimate1ofTrifocalTensor trifocalEstimator = FactoryMultiView.trifocal_1(configTri);
    if (!trifocalEstimator.process(inliers, model))
        throw new RuntimeException("Estimator failed");
    model.print();
    DMatrixRMaj P1 = CommonOps_DDRM.identity(3, 4);
    DMatrixRMaj P2 = new DMatrixRMaj(3, 4);
    DMatrixRMaj P3 = new DMatrixRMaj(3, 4);
    MultiViewOps.trifocalToCameraMatrices(model, P2, P3);
    // Most of the time this refinement step makes little difference, but in some edges cases it appears
    // to help convergence
    System.out.println("Refining projective camera matrices");
    RefineThreeViewProjective refineP23 = FactoryMultiView.threeViewRefine(null);
    if (!refineP23.process(inliers, P2, P3, P2, P3))
        throw new RuntimeException("Can't refine P2 and P3!");
    var selfcalib = new SelfCalibrationLinearDualQuadratic(1.0);
    selfcalib.addCameraMatrix(P1);
    selfcalib.addCameraMatrix(P2);
    selfcalib.addCameraMatrix(P3);
    var listPinhole = new ArrayList<CameraPinhole>();
    GeometricResult result = selfcalib.solve();
    if (GeometricResult.SOLVE_FAILED != result) {
        for (int i = 0; i < 3; i++) {
            Intrinsic c = selfcalib.getIntrinsics().get(i);
            CameraPinhole p = new CameraPinhole(c.fx, c.fy, 0, 0, 0, width, height);
            listPinhole.add(p);
        }
    } else {
        System.out.println("Self calibration failed!");
        for (int i = 0; i < 3; i++) {
            CameraPinhole p = new CameraPinhole(width / 2, width / 2, 0, 0, 0, width, height);
            listPinhole.add(p);
        }
    }
    // parameter
    for (int i = 0; i < 3; i++) {
        CameraPinhole r = listPinhole.get(i);
        System.out.println("fx=" + r.fx + " fy=" + r.fy + " skew=" + r.skew);
    }
    System.out.println("Projective to metric");
    // convert camera matrix from projective to metric
    // storage for rectifying homography
    var H = new DMatrixRMaj(4, 4);
    if (!MultiViewOps.absoluteQuadraticToH(selfcalib.getQ(), H))
        throw new RuntimeException("Projective to metric failed");
    var K = new DMatrixRMaj(3, 3);
    var worldToView = new ArrayList<Se3_F64>();
    for (int i = 0; i < 3; i++) {
        worldToView.add(new Se3_F64());
    }
    // ignore K since we already have that
    MultiViewOps.projectiveToMetric(P1, H, worldToView.get(0), K);
    MultiViewOps.projectiveToMetric(P2, H, worldToView.get(1), K);
    MultiViewOps.projectiveToMetric(P3, H, worldToView.get(2), K);
    // scale is arbitrary. Set max translation to 1
    adjustTranslationScale(worldToView);
    // Construct bundle adjustment data structure
    var structure = new SceneStructureMetric(false);
    structure.initialize(3, 3, inliers.size());
    var observations = new SceneObservations();
    observations.initialize(3);
    for (int i = 0; i < listPinhole.size(); i++) {
        BundlePinholeSimplified bp = new BundlePinholeSimplified();
        bp.f = listPinhole.get(i).fx;
        structure.setCamera(i, false, bp);
        structure.setView(i, i, i == 0, worldToView.get(i));
    }
    for (int i = 0; i < inliers.size(); i++) {
        AssociatedTriple t = inliers.get(i);
        observations.getView(0).add(i, (float) t.p1.x, (float) t.p1.y);
        observations.getView(1).add(i, (float) t.p2.x, (float) t.p2.y);
        observations.getView(2).add(i, (float) t.p3.x, (float) t.p3.y);
        structure.connectPointToView(i, 0);
        structure.connectPointToView(i, 1);
        structure.connectPointToView(i, 2);
    }
    // Initial estimate for point 3D locations
    triangulatePoints(structure, observations);
    ConfigLevenbergMarquardt configLM = new ConfigLevenbergMarquardt();
    configLM.dampeningInitial = 1e-3;
    configLM.hessianScaling = false;
    ConfigBundleAdjustment configSBA = new ConfigBundleAdjustment();
    configSBA.configOptimizer = configLM;
    // Create and configure the bundle adjustment solver
    BundleAdjustment<SceneStructureMetric> bundleAdjustment = FactoryMultiView.bundleSparseMetric(configSBA);
    // prints out useful debugging information that lets you know how well it's converging
    // bundleAdjustment.setVerbose(System.out,0);
    // convergence criteria
    bundleAdjustment.configure(1e-6, 1e-6, 100);
    bundleAdjustment.setParameters(structure, observations);
    bundleAdjustment.optimize(structure);
    // See if the solution is physically possible. If not fix and run bundle adjustment again
    checkBehindCamera(structure, observations, bundleAdjustment);
    // It's very difficult to find the best solution due to the number of local minimum. In the three view
    // case it's often the problem that a small translation is virtually identical to a small rotation.
    // Convergence can be improved by considering that possibility
    // Now that we have a decent solution, prune the worst outliers to improve the fit quality even more
    var pruner = new PruneStructureFromSceneMetric(structure, observations);
    pruner.pruneObservationsByErrorRank(0.7);
    pruner.pruneViews(10);
    pruner.pruneUnusedMotions();
    pruner.prunePoints(1);
    bundleAdjustment.setParameters(structure, observations);
    bundleAdjustment.optimize(structure);
    System.out.println("Final Views");
    for (int i = 0; i < 3; i++) {
        BundlePinholeSimplified cp = structure.getCameras().get(i).getModel();
        Vector3D_F64 T = structure.getParentToView(i).T;
        System.out.printf("[ %d ] f = %5.1f T=%s\n", i, cp.f, T.toString());
    }
    System.out.println("\n\nComputing Stereo Disparity");
    BundlePinholeSimplified cp = structure.getCameras().get(0).getModel();
    var intrinsic01 = new CameraPinholeBrown();
    intrinsic01.fsetK(cp.f, cp.f, 0, cx, cy, width, height);
    intrinsic01.fsetRadial(cp.k1, cp.k2);
    cp = structure.getCameras().get(1).getModel();
    var intrinsic02 = new CameraPinholeBrown();
    intrinsic02.fsetK(cp.f, cp.f, 0, cx, cy, width, height);
    intrinsic02.fsetRadial(cp.k1, cp.k2);
    Se3_F64 leftToRight = structure.getParentToView(1);
    // TODO dynamic max disparity
    computeStereoCloud(image01, image02, color01, color02, intrinsic01, intrinsic02, leftToRight, 0, 250);
}
Also used : ConfigFastHessian(boofcv.abst.feature.detect.interest.ConfigFastHessian) ConfigTrifocal(boofcv.factory.geo.ConfigTrifocal) Estimate1ofTrifocalTensor(boofcv.abst.geo.Estimate1ofTrifocalTensor) TrifocalTensor(boofcv.struct.geo.TrifocalTensor) ExampleComputeTrifocalTensor(boofcv.examples.sfm.ExampleComputeTrifocalTensor) CameraPinholeBrown(boofcv.struct.calib.CameraPinholeBrown) BundlePinholeSimplified(boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified) DMatrixRMaj(org.ejml.data.DMatrixRMaj) ArrayList(java.util.ArrayList) CameraPinhole(boofcv.struct.calib.CameraPinhole) BufferedImage(java.awt.image.BufferedImage) ConvertBufferedImage(boofcv.io.image.ConvertBufferedImage) SelfCalibrationLinearDualQuadratic(boofcv.alg.geo.selfcalib.SelfCalibrationLinearDualQuadratic) SceneStructureMetric(boofcv.abst.geo.bundle.SceneStructureMetric) AssociatedTriple(boofcv.struct.geo.AssociatedTriple) ExampleAssociateThreeView(boofcv.examples.features.ExampleAssociateThreeView) Estimate1ofTrifocalTensor(boofcv.abst.geo.Estimate1ofTrifocalTensor) AssociatedTripleIndex(boofcv.struct.feature.AssociatedTripleIndex) ConfigLevenbergMarquardt(org.ddogleg.optimization.lm.ConfigLevenbergMarquardt) GrayU8(boofcv.struct.image.GrayU8) TupleDesc_F64(boofcv.struct.feature.TupleDesc_F64) RefineThreeViewProjective(boofcv.abst.geo.RefineThreeViewProjective) PruneStructureFromSceneMetric(boofcv.abst.geo.bundle.PruneStructureFromSceneMetric) DogArray(org.ddogleg.struct.DogArray) DetectDescribePoint(boofcv.abst.feature.detdesc.DetectDescribePoint) ConfigBundleAdjustment(boofcv.factory.geo.ConfigBundleAdjustment) Vector3D_F64(georegression.struct.point.Vector3D_F64) AssociatedTriplePanel(boofcv.gui.feature.AssociatedTriplePanel) SceneObservations(boofcv.abst.geo.bundle.SceneObservations) GeometricResult(boofcv.alg.geo.GeometricResult) Intrinsic(boofcv.alg.geo.selfcalib.SelfCalibrationLinearDualQuadratic.Intrinsic) Se3_F64(georegression.struct.se.Se3_F64)

Example 3 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified in project BoofCV by lessthanoptimal.

the class TestBundleAdjustmentOps method convert_bundleSimple_brown.

@Test
void convert_bundleSimple_brown() {
    var src = new BundlePinholeSimplified(10, 1, 2);
    var dst = new CameraPinholeBrown().fsetK(1, 2, 3, 4, 5, 6, 7).fsetRadial(-1, -2).fsetTangental(0.1, 0.2);
    assertSame(dst, BundleAdjustmentOps.convert(src, width, height, dst));
    assertArrayEquals(new double[] { src.k1, src.k2 }, dst.radial);
    assertEquals(0.0, dst.t1);
    assertEquals(0.0, dst.t2);
    assertEquals(src.f, dst.fx);
    assertEquals(src.f, dst.fy);
    assertEquals(width / 2, dst.cx);
    assertEquals(height / 2, dst.cy);
    assertEquals(0.0, dst.skew);
    assertEquals(width, dst.width);
    assertEquals(height, dst.height);
}
Also used : CameraPinholeBrown(boofcv.struct.calib.CameraPinholeBrown) BundlePinholeSimplified(boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified) Test(org.junit.jupiter.api.Test)

Example 4 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified in project BoofCV by lessthanoptimal.

the class ThreeViewEstimateMetricScene method findBestValidSolution.

/**
 * Tries a bunch of stuff to ensure that it can find the best solution which is physically possible
 */
private void findBestValidSolution(BundleAdjustment<SceneStructureMetric> bundleAdjustment) {
    // Specifies convergence criteria
    bundleAdjustment.configure(convergeSBA.ftol, convergeSBA.gtol, convergeSBA.maxIterations);
    bundleAdjustment.setParameters(structure, observations);
    bundleAdjustment.optimize(structure);
    // ensure that the points are in front of the camera and are a valid solution
    if (checkBehindCamera(structure)) {
        if (verbose != null)
            verbose.println("  #1 Points Behind. Flipping view");
        flipAround(structure, observations);
        bundleAdjustment.setParameters(structure, observations);
        bundleAdjustment.optimize(structure);
    }
    double bestScore = bundleAdjustment.getFitScore();
    if (verbose != null)
        verbose.println("First Pass: SBA score " + bestScore);
    List<Se3_F64> bestPose = new ArrayList<>();
    List<BundlePinholeSimplified> bestCameras = new ArrayList<>();
    for (int i = 0; i < structure.views.size; i++) {
        BundlePinholeSimplified c = Objects.requireNonNull(structure.cameras.data[i].getModel());
        bestPose.add(structure.getParentToView(i).copy());
        bestCameras.add(c.copy());
    }
    for (int i = 0; i < structure.cameras.size; i++) {
        BundlePinholeSimplified c = Objects.requireNonNull(structure.cameras.data[i].getModel());
        c.f = listPinhole.get(i).fx;
        c.k1 = c.k2 = 0;
    }
    // flip rotation assuming that it was done wrong
    for (int i = 1; i < structure.views.size; i++) {
        CommonOps_DDRM.transpose(structure.getParentToView(i).R);
    }
    triangulatePoints(structure, observations);
    bundleAdjustment.setParameters(structure, observations);
    bundleAdjustment.optimize(structure);
    if (checkBehindCamera(structure)) {
        if (verbose != null)
            verbose.println("  #2 Points Behind. Flipping view");
        flipAround(structure, observations);
        bundleAdjustment.setParameters(structure, observations);
        bundleAdjustment.optimize(structure);
    }
    // revert to old settings
    if (verbose != null)
        verbose.println(" First Pass / Transpose(R) = " + bestScore + " / " + bundleAdjustment.getFitScore());
    if (bundleAdjustment.getFitScore() > bestScore) {
        if (verbose != null)
            verbose.println("  recomputing old structure");
        for (int i = 0; i < structure.cameras.size; i++) {
            BundlePinholeSimplified c = Objects.requireNonNull(structure.cameras.data[i].getModel());
            c.setTo(bestCameras.get(i));
            structure.getParentToView(i).setTo(bestPose.get(i));
        }
        triangulatePoints(structure, observations);
        bundleAdjustment.setParameters(structure, observations);
        bundleAdjustment.optimize(structure);
        if (verbose != null)
            verbose.println("  score after reverting = " + bundleAdjustment.getFitScore() + "  original " + bestScore);
    }
}
Also used : BundlePinholeSimplified(boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified) ArrayList(java.util.ArrayList) VerbosePrint(org.ddogleg.struct.VerbosePrint) Se3_F64(georegression.struct.se.Se3_F64)

Example 5 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified in project BoofCV by lessthanoptimal.

the class MetricSanityChecks method checkPhysicalConstraints.

public boolean checkPhysicalConstraints(SceneStructureMetric structure, SceneObservations observations, List<CameraPinholeBrown> listPriors) {
    BoofMiscOps.checkEq(listPriors.size(), structure.views.size);
    for (int i = 0; i < structure.cameras.size; i++) {
        BundlePinholeSimplified pinhole = (BundlePinholeSimplified) structure.cameras.get(i).model;
        if (pinhole.f < 0.0f) {
            if (verbose != null)
                verbose.println("Bad focal length. f=" + pinhole.f);
            return false;
        }
    }
    badFeatures.resetResize(structure.points.size, false);
    var worldP = new Point4D_F64(0, 0, 0, 1);
    var viewP = new Point4D_F64();
    var observedPixel = new Point2D_F64();
    var predictdPixel = new Point2D_F64();
    for (int viewIdx = 0; viewIdx < observations.views.size; viewIdx++) {
        int cameraIdx = structure.views.get(viewIdx).camera;
        BundlePinholeSimplified pinhole = (BundlePinholeSimplified) Objects.requireNonNull(structure.cameras.get(cameraIdx).model);
        CameraPinholeBrown priorCamera = listPriors.get(viewIdx);
        int width = priorCamera.width;
        int height = priorCamera.height;
        // Used to compensates for the lens model having its origin at the image center
        float cx = (float) priorCamera.cx;
        float cy = (float) priorCamera.cy;
        // Number of times each test failed in this particular view
        int failedBehind = 0;
        int failedImageBounds = 0;
        int failedReprojection = 0;
        Se3_F64 world_to_view = structure.getParentToView(viewIdx);
        SceneObservations.View oview = observations.views.get(viewIdx);
        for (int i = 0; i < oview.size(); i++) {
            // If true then this feature failed one of the constraints test in tis value
            boolean badObservation = false;
            oview.getPixel(i, observedPixel);
            SceneStructureCommon.Point p = structure.points.get(oview.getPointId(i));
            worldP.x = p.getX();
            worldP.y = p.getY();
            worldP.z = p.getZ();
            if (structure.isHomogenous()) {
                worldP.w = p.getW();
            }
            // worldP.w = 1 was already set for 3D points
            SePointOps_F64.transform(world_to_view, worldP, viewP);
            if (PerspectiveOps.isBehindCamera(viewP)) {
                badObservation = true;
                failedBehind++;
            }
            pinhole.project(viewP.x, viewP.y, viewP.z, predictdPixel);
            double reprojectionError = predictdPixel.distance2(observedPixel);
            if (reprojectionError > maxReprojectionErrorSq) {
                badObservation = true;
                failedReprojection++;
            }
            if (!BoofMiscOps.isInside(width, height, predictdPixel.x + cx, predictdPixel.y + cy)) {
                badObservation = true;
                failedImageBounds++;
            }
            if (badObservation) {
                badFeatures.set(oview.getPointId(i), true);
            }
        }
        if (verbose != null)
            verbose.printf("view[%d] errors: behind=%d bounds=%d reprojection=%d, obs=%d\n", viewIdx, failedBehind, failedImageBounds, failedReprojection, oview.size());
    }
    return true;
}
Also used : CameraPinholeBrown(boofcv.struct.calib.CameraPinholeBrown) BundlePinholeSimplified(boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified) SceneStructureCommon(boofcv.abst.geo.bundle.SceneStructureCommon) VerbosePrint(org.ddogleg.struct.VerbosePrint) Point2D_F64(georegression.struct.point.Point2D_F64) SceneObservations(boofcv.abst.geo.bundle.SceneObservations) Point4D_F64(georegression.struct.point.Point4D_F64) Se3_F64(georegression.struct.se.Se3_F64)

Aggregations

BundlePinholeSimplified (boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified)18 Se3_F64 (georegression.struct.se.Se3_F64)10 VerbosePrint (org.ddogleg.struct.VerbosePrint)9 SceneStructureMetric (boofcv.abst.geo.bundle.SceneStructureMetric)6 ArrayList (java.util.ArrayList)5 DogArray (org.ddogleg.struct.DogArray)5 CameraPinholeBrown (boofcv.struct.calib.CameraPinholeBrown)4 Test (org.junit.jupiter.api.Test)4 SceneObservations (boofcv.abst.geo.bundle.SceneObservations)3 AssociatedTriple (boofcv.struct.geo.AssociatedTriple)3 Point4D_F64 (georegression.struct.point.Point4D_F64)3 SceneStructureCommon (boofcv.abst.geo.bundle.SceneStructureCommon)2 RemoveBrownPtoN_F64 (boofcv.alg.distort.brown.RemoveBrownPtoN_F64)2 ConfigBundleAdjustment (boofcv.factory.geo.ConfigBundleAdjustment)2 ConvertBufferedImage (boofcv.io.image.ConvertBufferedImage)2 BoofMiscOps (boofcv.misc.BoofMiscOps)2 GrayU8 (boofcv.struct.image.GrayU8)2 Vector3D_F64 (georegression.struct.point.Vector3D_F64)2 BufferedImage (java.awt.image.BufferedImage)2 Objects (java.util.Objects)2