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

Example 31 with SceneStructureMetric

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

the class BundleAdjustmentMetricResidualFunction method configure.

/**
 * Specifies the scenes structure and observed feature locations
 */
@Override
public void configure(SceneStructureMetric structure, SceneObservations observations) {
    this.structure = structure;
    this.observations = observations;
    numObservations = observations.getObservationCount();
    numParameters = structure.getParameterCount();
    structure.assignIDsToRigidPoints();
    // declare storage and create a look up table for world to view for all relative views
    mapWorldToView.clear();
    storageSe3.reset();
    for (int viewIdx = 0; viewIdx < structure.views.size; viewIdx++) {
        SceneStructureMetric.View v = structure.views.get(viewIdx);
        if (v.parent == null)
            continue;
        Se3_F64 world_to_view = storageSe3.grow();
        mapWorldToView.put(v, world_to_view);
    }
}

Example 32 with SceneStructureMetric

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

the class MultiViewOps method triangulatePoints.

/**
 * Convenience function for initializing bundle adjustment parameters. Triangulates points using camera
 * position and pixel observations.
 *
 * @param structure camera locations
 * @param observations observations of features in the images
 */
public static void triangulatePoints(SceneStructureMetric structure, SceneObservations observations) {
    TriangulateNViewsMetricH triangulator = FactoryMultiView.triangulateNViewMetricH(ConfigTriangulation.GEOMETRIC());
    List<RemoveBrownPtoN_F64> list_p_to_n = new ArrayList<>();
    for (int i = 0; i < structure.cameras.size; i++) {
        RemoveBrownPtoN_F64 p2n = new RemoveBrownPtoN_F64();
        BundleAdjustmentCamera baseModel = Objects.requireNonNull(structure.cameras.data[i].model);
        if (baseModel instanceof BundlePinholeSimplified) {
            BundlePinholeSimplified cam = (BundlePinholeSimplified) baseModel;
            p2n.setK(cam.f, cam.f, 0, 0, 0).setDistortion(new double[] { cam.k1, cam.k2 }, 0, 0);
        } else if (baseModel instanceof BundlePinhole) {
            BundlePinhole cam = (BundlePinhole) baseModel;
            p2n.setK(cam.fx, cam.fy, cam.skew, cam.cx, cam.cy).setDistortion(new double[] { 0, 0 }, 0, 0);
        } else if (baseModel instanceof BundlePinholeBrown) {
            BundlePinholeBrown cam = (BundlePinholeBrown) baseModel;
            p2n.setK(cam.fx, cam.fy, cam.skew, cam.cx, cam.cy).setDistortion(cam.radial, cam.t1, cam.t2);
        } else {
            throw new RuntimeException("Unknown camera model!");
        }
        list_p_to_n.add(p2n);
    }
    DogArray<Point2D_F64> normObs = new DogArray<>(Point2D_F64::new);
    normObs.resize(3);
    final boolean homogenous = structure.isHomogenous();
    Point4D_F64 X = new Point4D_F64();
    List<Se3_F64> worldToViews = new ArrayList<>();
    for (int i = 0; i < structure.points.size; i++) {
        normObs.reset();
        worldToViews.clear();
        SceneStructureCommon.Point sp = structure.points.get(i);
        for (int j = 0; j < sp.views.size; j++) {
            int viewIdx = sp.views.get(j);
            SceneStructureMetric.View v = structure.views.data[viewIdx];
            worldToViews.add(structure.getParentToView(v));
            // get the observation in pixels
            Point2D_F64 n = normObs.grow();
            int pointidx = observations.views.get(viewIdx).point.indexOf(i);
            observations.views.get(viewIdx).getPixel(pointidx, n);
            // convert to normalized image coordinates
            list_p_to_n.get(v.camera).compute(n.x, n.y, n);
        }
        if (!triangulator.triangulate(normObs.toList(), worldToViews, X)) {
            // this should work unless the input is bad
            throw new RuntimeException("Triangulation failed. Bad input?");
        }
        if (homogenous)
            sp.set(X.x, X.y, X.z, X.w);
        else
            sp.set(X.x / X.w, X.y / X.w, X.z / X.w);
    }
}

Example 33 with SceneStructureMetric

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

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

the class TestMultiViewIO method save_load_SceneStructureMetric.

@Test
void save_load_SceneStructureMetric() {
    for (int trial = 0; trial < 20; trial++) {
        SceneStructureMetric expected = createSceneStructureMetric();
        var output = new ByteArrayOutputStream();
        MultiViewIO.save(expected, new OutputStreamWriter(output, UTF_8));
        var input = new ByteArrayInputStream(output.toByteArray());
        SceneStructureMetric found = MultiViewIO.load(new InputStreamReader(input, UTF_8), (SceneStructureMetric) null);
        assertTrue(expected.isIdentical(found, UtilEjml.TEST_F64));
    }
}

Example 35 with SceneStructureMetric

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

the class GenericCalibrationZhang99 method linearEstimate.

/**
 * See how well it computes an initial guess at the parameters given perfect inputs
 */
@Test
public void linearEstimate() {
    for (CameraConfig config : createCameraForLinearTests(rand)) {
        CalibInputs inputs = createInputs(config.model, 3, rand);
        Zhang99Camera zhangCamera = createGenerator(config, inputs.layout);
        var alg = new CalibrationPlanarGridZhang99(inputs.layout, zhangCamera);
        alg.setZeroSkew(true);
        alg.linearEstimate(inputs.observations);
        SceneStructureMetric structure = alg.getStructure();
        CM found = (CM) zhangCamera.getCameraModel(structure.getCameras().get(0).model);
        checkIntrinsicOnly(config.model, found, 0.01, 0.1, 0.1);
    }
}