Examples with SceneStructureMetric boofcv.abst.geo.bundle.SceneStructureMetric used on opensource projects

Example 26 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric 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);
}

Example 27 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class ExampleBundleAdjustment method main.

public static void main(String[] args) throws IOException {
    // Because the Bundle Adjustment in the Large data set is popular, a file reader and writer is included
    // with BoofCV. BoofCV uses two data types to describe the parameters in a bundle adjustment problem
    // BundleAdjustmentSceneStructure is used for camera parameters, camera locations, and 3D points
    // BundleAdjustmentObservations for image observations of 3D points
    // ExampleMultiViewSceneReconstruction gives a better feel for these data structures or you can look
    // at the source code of CodecBundleAdjustmentInTheLarge
    var parser = new CodecBundleAdjustmentInTheLarge();
    parser.parse(new File(UtilIO.pathExample("sfm/problem-16-22106-pre.txt")));
    // Print information which gives you an idea of the problem's scale
    System.out.println("Optimizing " + parser.scene.getParameterCount() + " parameters with " + parser.observations.getObservationCount() + " observations\n\n");
    // Configure the sparse Levenberg-Marquardt solver
    var configLM = new ConfigLevenbergMarquardt();
    // Important tuning parameter. Won't converge to a good solution if picked improperly. Small changes
    // to this problem and speed up or slow down convergence and change the final result. This is true for
    // basically all solvers.
    configLM.dampeningInitial = 1e-3;
    // Improves Jacobian matrix's condition. Recommended in general but not important in this problem
    configLM.hessianScaling = true;
    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, 50);
    // Scaling each variable type so that it takes on a similar numerical value. This aids in optimization
    // Not important for this problem but is for others
    var bundleScale = new ScaleSceneStructure();
    bundleScale.applyScale(parser.scene, parser.observations);
    bundleAdjustment.setParameters(parser.scene, parser.observations);
    // Runs the solver. This will take a few minutes. 7 iterations takes about 3 minutes on my computer
    long startTime = System.currentTimeMillis();
    double errorBefore = bundleAdjustment.getFitScore();
    if (!bundleAdjustment.optimize(parser.scene)) {
        throw new RuntimeException("Bundle adjustment failed?!?");
    }
    // Print out how much it improved the model
    System.out.println();
    System.out.printf("Error reduced by %.1f%%\n", (100.0 * (errorBefore / bundleAdjustment.getFitScore() - 1.0)));
    System.out.println(BoofMiscOps.milliToHuman(System.currentTimeMillis() - startTime));
    // Return parameters to their original scaling. Can probably skip this step.
    bundleScale.undoScale(parser.scene, parser.observations);
    // Visualize the results using a point cloud viewer
    visualizeInPointCloud(parser.scene);
}

Example 28 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class TestSceneStructureMetric method projectToPixel_3D.

@Test
void projectToPixel_3D() {
    var scene = new SceneStructureMetric(false);
    var intrinsic = new CameraPinhole(100, 100, 0, 0, 0, 300, 300);
    var world_to_view = SpecialEuclideanOps_F64.eulerXyz(1, 0, 0, 0, 0.01, -0.04, null);
    scene.initialize(1, 1, 1);
    scene.setView(0, 0, true, world_to_view);
    scene.setCamera(0, true, intrinsic);
    scene.setPoint(0, 0.1, -0.5, 1.1);
    // Point in front of camera
    var pixel = new Point2D_F64();
    assertTrue(scene.projectToPixel(0, 0, new Se3_F64(), new Se3_F64(), new Point3D_F64(), pixel));
    var expected = new Point2D_F64();
    PerspectiveOps.renderPixel(world_to_view, intrinsic, new Point3D_F64(0.1, -0.5, 1.1), expected);
    assertEquals(expected.x, pixel.x, UtilEjml.TEST_F64);
    assertEquals(expected.y, pixel.y, UtilEjml.TEST_F64);
    // sanity check for behind
    scene.setPoint(0, 0.1, -0.5, -1.1);
    assertFalse(scene.projectToPixel(0, 0, new Se3_F64(), new Se3_F64(), new Point3D_F64(), pixel));
}

Example 29 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class ExampleMultiViewSparseReconstruction method compute.

public void compute(String videoName, boolean sequential) {
    // Turn on threaded code for bundle adjustment
    DDoglegConcurrency.USE_CONCURRENT = true;
    // Create a directory to store the work space
    String path = UtilIO.pathExample("mvs/" + videoName);
    workDirectory = "mvs_work/" + FilenameUtils.getBaseName(videoName);
    // Attempt to reload intermediate results if previously computed
    if (!rebuild) {
        try {
            pairwise = MultiViewIO.load(new File(workDirectory, "pairwise.yaml").getPath(), (PairwiseImageGraph) null);
        } catch (UncheckedIOException ignore) {
        }
        try {
            working = MultiViewIO.load(new File(workDirectory, "working.yaml").getPath(), pairwise, null);
        } catch (UncheckedIOException ignore) {
        }
        try {
            scene = MultiViewIO.load(new File(workDirectory, "structure.yaml").getPath(), (SceneStructureMetric) null);
        } catch (UncheckedIOException ignore) {
        }
    }
    // Convert the video into an image sequence. Later on we will need to access the images in random order
    var imageDirectory = new File(workDirectory, "images");
    if (imageDirectory.exists()) {
        imageFiles = UtilIO.listSmart(String.format("glob:%s/images/*.png", workDirectory), true, (f) -> true);
    } else {
        checkTrue(imageDirectory.mkdirs(), "Failed to image directory");
        SimpleImageSequence<InterleavedU8> sequence = DefaultMediaManager.INSTANCE.openVideo(path, ImageType.IL_U8);
        System.out.println("----------------------------------------------------------------------------");
        System.out.println("### Decoding Video");
        BoofMiscOps.profile(() -> {
            int frame = 0;
            while (sequence.hasNext()) {
                InterleavedU8 image = sequence.next();
                File imageFile = new File(imageDirectory, String.format("frame%04d.png", frame++));
                imageFiles.add(imageFile.getPath());
                // This is commented out for what appears to be a JRE bug.
                // V  [libjvm.so+0xdc4059]  SWPointer::SWPointer(MemNode*, SuperWord*, Node_Stack*, bool)
                UtilImageIO.saveImage(image, imageFile.getPath());
            }
        }, "Video Decoding");
    }
    // Only determine the visual relationship between images if needed
    if (pairwise == null || working == null) {
        if (sequential) {
            similarImagesFromSequence();
        } else {
            similarImagesFromUnsorted();
        }
    }
    if (pairwise == null)
        computePairwiseGraph();
    if (working == null)
        metricFromPairwise();
    if (scene == null)
        bundleAdjustmentRefine();
    var rod = new Rodrigues_F64();
    System.out.println("----------------------------------------------------------------------------");
    for (PairwiseImageGraph.View pv : pairwise.nodes.toList()) {
        if (!working.containsView(pv.id))
            continue;
        SceneWorkingGraph.View wv = working.lookupView(pv.id);
        int order = working.listViews.indexOf(wv);
        ConvertRotation3D_F64.matrixToRodrigues(wv.world_to_view.R, rod);
        BundlePinholeSimplified intrinsics = working.getViewCamera(wv).intrinsic;
        System.out.printf("view[%2d]='%2s' f=%6.1f k1=%6.3f k2=%6.3f T={%5.1f,%5.1f,%5.1f} R=%4.2f\n", order, wv.pview.id, intrinsics.f, intrinsics.k1, intrinsics.k2, wv.world_to_view.T.x, wv.world_to_view.T.y, wv.world_to_view.T.z, rod.theta);
    }
    System.out.println("   Views used: " + scene.views.size + " / " + pairwise.nodes.size);
}

Example 30 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class CalibrationPlanarGridZhang99 method performBundleAdjustment.

/**
 * Use non-linear optimization to improve the parameter estimates
 */
public boolean performBundleAdjustment() {
    // Configure the sparse Levenberg-Marquardt solver
    ConfigLevenbergMarquardt configLM = new ConfigLevenbergMarquardt();
    configLM.hessianScaling = false;
    ConfigBundleAdjustment configSBA = new ConfigBundleAdjustment();
    configSBA.configOptimizer = configLM;
    BundleAdjustment<SceneStructureMetric> bundleAdjustment;
    if (robust) {
        configLM.mixture = 0;
        bundleAdjustment = FactoryMultiView.bundleDenseMetric(true, configSBA);
    } else {
        bundleAdjustment = FactoryMultiView.bundleSparseMetric(configSBA);
    }
    bundleAdjustment.setVerbose(verbose, null);
    // Specifies convergence criteria
    bundleAdjustment.configure(1e-20, 1e-20, 200);
    bundleAdjustment.setParameters(structure, observations);
    return bundleAdjustment.optimize(structure);
}