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

Example 16 with SceneObservations

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

the class CommonBundleAdjustmentMetricSchurJacobian method twoViewsOneMotion.

public void twoViewsOneMotion(boolean homogenous, boolean knownMotion) {
    SceneStructureMetric structure = createSceneStereo(rand, homogenous);
    SceneObservations observations = createObservations(rand, structure);
    structure.motions.forIdx((i, m) -> m.known = knownMotion);
    var param = new double[structure.getParameterCount()];
    new CodecSceneStructureMetric().encode(structure, param);
    var alg = createAlg();
    var jac = createJacobian(alg);
    var func = new BundleAdjustmentMetricResidualFunction();
    alg.configure(structure, observations);
    func.configure(structure, observations);
    // DerivativeChecker.jacobianPrint(func, jac, param, 100*UtilEjml.TEST_F64_SQ );
    assertTrue(DerivativeChecker.jacobian(func, jac, param, 100 * UtilEjml.TEST_F64_SQ));
}

Example 17 with SceneObservations

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

the class TestBundleAdjustmentProjectiveSchurJacobian_DDRM method compareToNumerical_Homogenous.

@Test
void compareToNumerical_Homogenous() {
    SceneStructureProjective structure = createSceneH(rand);
    SceneObservations observations = createObservations(rand, structure);
    double[] param = new double[structure.getParameterCount()];
    new CodecSceneStructureProjective().encode(structure, param);
    BundleAdjustmentProjectiveSchurJacobian_DDRM alg = new BundleAdjustmentProjectiveSchurJacobian_DDRM();
    FunctionNtoMxN<DMatrixRMaj> jac = new SchurJacobian_to_NtoMxN.DDRM(alg);
    BundleAdjustmentProjectiveResidualFunction func = new BundleAdjustmentProjectiveResidualFunction();
    alg.configure(structure, observations);
    func.configure(structure, observations);
    // DerivativeChecker.jacobianPrint(func, jac, param, 0.1 );
    assertTrue(DerivativeChecker.jacobian(func, jac, param, 0.1));
}

Example 18 with SceneObservations

use of boofcv.abst.geo.bundle.SceneObservations 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 19 with SceneObservations

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

the class CodecBundleAdjustmentInTheLarge method parse.

public void parse(File file) throws IOException {
    InputStream stream = UtilIO.openStream(file.getPath());
    if (stream == null)
        throw new IOException("Can't open file: " + file.getPath());
    BufferedReader reader = new BufferedReader(new InputStreamReader(stream, UTF_8));
    String[] words = reader.readLine().split("\\s+");
    if (words.length != 3)
        throw new IOException("Unexpected number of words on first line");
    int numCameras = Integer.parseInt(words[0]);
    int numPoints = Integer.parseInt(words[1]);
    int numObservations = Integer.parseInt(words[2]);
    scene = new SceneStructureMetric(false);
    scene.initialize(numCameras, numCameras, numPoints);
    observations = new SceneObservations();
    observations.initialize(numCameras);
    for (int i = 0; i < numObservations; i++) {
        words = reader.readLine().split("\\s+");
        if (words.length != 4)
            throw new IOException("Unexpected number of words in obs");
        int cameraID = Integer.parseInt(words[0]);
        int pointID = Integer.parseInt(words[1]);
        float pixelX = Float.parseFloat(words[2]);
        float pixelY = Float.parseFloat(words[3]);
        if (pointID >= numPoints) {
            throw new RuntimeException("Out of bounds pointID");
        }
        if (cameraID >= numCameras) {
            throw new RuntimeException("Out of bounds cameraID");
        }
        observations.getView(cameraID).add(pointID, pixelX, pixelY);
    }
    Se3_F64 worldToCameraGL = new Se3_F64();
    Rodrigues_F64 rod = new Rodrigues_F64();
    for (int i = 0; i < numCameras; i++) {
        rod.unitAxisRotation.x = Double.parseDouble(reader.readLine());
        rod.unitAxisRotation.y = Double.parseDouble(reader.readLine());
        rod.unitAxisRotation.z = Double.parseDouble(reader.readLine());
        rod.theta = rod.unitAxisRotation.norm();
        if (rod.theta != 0)
            rod.unitAxisRotation.divide(rod.theta);
        worldToCameraGL.T.x = Double.parseDouble(reader.readLine());
        worldToCameraGL.T.y = Double.parseDouble(reader.readLine());
        worldToCameraGL.T.z = Double.parseDouble(reader.readLine());
        ConvertRotation3D_F64.rodriguesToMatrix(rod, worldToCameraGL.R);
        BundlePinholeSnavely camera = new BundlePinholeSnavely();
        camera.f = Double.parseDouble(reader.readLine());
        camera.k1 = Double.parseDouble(reader.readLine());
        camera.k2 = Double.parseDouble(reader.readLine());
        scene.setCamera(i, false, camera);
        scene.setView(i, i, false, worldToCameraGL);
    }
    Point3D_F64 P = new Point3D_F64();
    for (int i = 0; i < numPoints; i++) {
        P.x = Float.parseFloat(reader.readLine());
        P.y = Float.parseFloat(reader.readLine());
        P.z = Float.parseFloat(reader.readLine());
        // GeometryMath_F64.mult(glToCv.R,P,P);
        scene.setPoint(i, P.x, P.y, P.z);
    }
    for (int i = 0; i < observations.views.size; i++) {
        View v = observations.getView(i);
        for (int j = 0; j < v.point.size; j++) {
            scene.connectPointToView(v.getPointId(j), i);
        }
    }
    reader.close();
    observations.checkOneObservationPerView();
}

Example 20 with SceneObservations

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

the class TestBundleAdjustmentProjectiveResidualFunction method changeInParamChangesOutput.

void changeInParamChangesOutput(boolean homogenous) {
    SceneStructureProjective structure = homogenous ? createSceneH(rand) : createScene3D(rand);
    double[] param = new double[structure.getParameterCount()];
    new CodecSceneStructureProjective().encode(structure, param);
    // Create random observations
    SceneObservations obs = createObservations(rand, structure);
    BundleAdjustmentProjectiveResidualFunction alg = new BundleAdjustmentProjectiveResidualFunction();
    alg.configure(structure, obs);
    double[] original = new double[alg.getNumOfOutputsM()];
    double[] found = new double[alg.getNumOfOutputsM()];
    alg.process(param, original);
    for (int paramIndex = 0; paramIndex < original.length; paramIndex++) {
        double v = param[paramIndex];
        param[paramIndex] += 0.001;
        alg.process(param, found);
        boolean identical = true;
        for (int i = 0; i < found.length; i++) {
            if (Math.abs(original[i] - found[i]) > UtilEjml.TEST_F64) {
                identical = false;
                break;
            }
        }
        assertFalse(identical);
        param[paramIndex] = v;
    }
}