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

Example 6 with SceneObservations

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

the class VisOdomStereoQuadPnP method performBundleAdjustment.

/**
 * Optimize cameras and feature locations at the same time
 */
private void performBundleAdjustment(Se3_F64 key_to_curr) {
    if (bundle.configConverge.maxIterations <= 0)
        return;
    // Must only process inlier tracks here
    inliers.clear();
    for (int trackIdx = 0; trackIdx < consistentTracks.size(); trackIdx++) {
        TrackQuad t = consistentTracks.get(trackIdx);
        if (t.leftCurrIndex != -1 && t.inlier)
            inliers.add(t);
    }
    // Copy the scene into a data structure bundle adjustment understands
    SceneStructureMetric structure = bundle.getStructure();
    SceneObservations observations = bundle.getObservations();
    observations.initialize(4);
    structure.initialize(2, 4, inliers.size());
    // known relative view relating left to right cameras
    int baseline = structure.addMotion(true, left_to_right);
    structure.setCamera(0, true, stereoParameters.left);
    structure.setCamera(1, true, stereoParameters.right);
    // view[0].left
    structure.setView(0, 0, true, listWorldToView.get(0));
    // view[0].right
    structure.setView(1, 1, baseline, 0);
    // view[1].left
    structure.setView(2, 0, false, listWorldToView.get(2));
    // view[1].right
    structure.setView(3, 1, baseline, 2);
    for (int trackIdx = 0; trackIdx < inliers.size(); trackIdx++) {
        TrackQuad t = inliers.get(trackIdx);
        Point3D_F64 X = t.X;
        structure.setPoint(trackIdx, X.x, X.y, X.z);
        observations.getView(0).add(trackIdx, (float) t.v0.x, (float) t.v0.y);
        observations.getView(1).add(trackIdx, (float) t.v1.x, (float) t.v1.y);
        observations.getView(2).add(trackIdx, (float) t.v2.x, (float) t.v2.y);
        observations.getView(3).add(trackIdx, (float) t.v3.x, (float) t.v3.y);
    }
    if (!bundle.process())
        return;
    // Update the state of tracks and the current views
    for (int trackIdx = 0; trackIdx < inliers.size(); trackIdx++) {
        TrackQuad t = inliers.get(trackIdx);
        structure.points.get(trackIdx).get(t.X);
    }
    // Reminder: World here refers to key left view
    key_to_curr.setTo(structure.getParentToView(2));
}

Example 7 with SceneObservations

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

the class VisOdomBundleAdjustment method setupBundleStructure.

/**
 * Converts input data into a format that bundle adjustment can understand
 */
private void setupBundleStructure() {
    // Need to count the total number of tracks that will be feed into bundle adjustment
    int totalBundleTracks = selectedTracks.size();
    // Initialize data structures
    final SceneStructureMetric structure = bundle.getStructure();
    final SceneObservations observations = bundle.getObservations();
    observations.initialize(frames.size);
    structure.initialize(cameras.size, frames.size, totalBundleTracks);
    for (int cameraIdx = 0; cameraIdx < cameras.size; cameraIdx++) {
        structure.setCamera(cameraIdx, true, cameras.get(cameraIdx).bundleCamera);
    }
    // TODO make the first frame at origin. This is done to avoid numerical after traveling a good distance
    for (int frameIdx = 0; frameIdx < frames.size; frameIdx++) {
        BFrame bf = frames.get(frameIdx);
        bf.frame_to_world.invert(world_to_view);
        structure.setView(frameIdx, bf.camera.index, frameIdx == 0, world_to_view);
        // save the index since it's needed in the next loop
        frames.get(frameIdx).listIndex = frameIdx;
    }
    // A feature is only passed to SBA if it is active and more than one view has seen it
    // this requires it to have a different index
    int featureBundleIdx = 0;
    for (int trackIdx = 0; trackIdx < tracks.size; trackIdx++) {
        BTrack bt = tracks.get(trackIdx);
        if (!bt.selected) {
            continue;
        }
        Point4D_F64 p = bt.worldLoc;
        structure.setPoint(featureBundleIdx, p.x, p.y, p.z, p.w);
        for (int obsIdx = 0; obsIdx < bt.observations.size; obsIdx++) {
            BObservation o = bt.observations.get(obsIdx);
            SceneObservations.View view = observations.getView(o.frame.listIndex);
            view.add(featureBundleIdx, (float) o.pixel.x, (float) o.pixel.y);
        }
        featureBundleIdx++;
    }
    // Sanity check
    if (featureBundleIdx != structure.points.size)
        throw new RuntimeException("BUG! tracks feed in and points don't match");
}

Example 8 with SceneObservations

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

the class CalibrateStereoPlanar method computeErrors.

public List<ImageResults> computeErrors() {
    final SceneStructureMetric structure = bundleUtils.getStructure();
    final SceneObservations observations = bundleUtils.getObservations();
    List<ImageResults> errors = new ArrayList<>();
    double[] parameters = new double[structure.getParameterCount()];
    double[] residuals = new double[observations.getObservationCount() * 2];
    CodecSceneStructureMetric codec = new CodecSceneStructureMetric();
    codec.encode(structure, parameters);
    BundleAdjustmentMetricResidualFunction function = new BundleAdjustmentMetricResidualFunction();
    function.configure(structure, observations);
    function.process(parameters, residuals);
    int idx = 0;
    for (int i = 0; i < observations.viewsRigid.size; i++) {
        SceneObservations.View v = observations.viewsRigid.data[i];
        ImageResults r = new ImageResults(v.size());
        double sumX = 0;
        double sumY = 0;
        double meanErrorMag = 0;
        double maxError = 0;
        for (int j = 0; j < v.size(); j++) {
            double x = residuals[idx++];
            double y = residuals[idx++];
            double nerr = r.pointError[j] = Math.sqrt(x * x + y * y);
            meanErrorMag += nerr;
            maxError = Math.max(maxError, nerr);
            sumX += x;
            sumY += y;
        }
        r.biasX = sumX / v.size();
        r.biasY = sumY / v.size();
        r.meanError = meanErrorMag / v.size();
        r.maxError = maxError;
        errors.add(r);
    }
    return errors;
}

Example 9 with SceneObservations

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

the class CalibrateStereoPlanar method refineAll.

/**
 * Jointly refines both cameras together
 *
 * @param parameters (input) initial estimate and is updated if refine is successful
 */
private void refineAll(StereoParameters parameters) {
    Se3_F64 left_to_right = parameters.right_to_left.invert(null);
    final SceneStructureMetric structure = bundleUtils.getStructure();
    final SceneObservations observations = bundleUtils.getObservations();
    final SceneStructureMetric structureLeft = calibLeft.getStructure();
    final SceneStructureMetric structureRight = calibRight.getStructure();
    int numViews = structureLeft.views.size;
    // left and right cameras. n views, and 1 known calibration target
    structure.initialize(2, numViews * 2, numViews + 1, layout.size(), 1);
    // initialize the cameras
    structure.setCamera(0, false, structureLeft.cameras.get(0).model);
    structure.setCamera(1, false, structureRight.cameras.get(0).model);
    // configure the known calibration target
    structure.setRigid(0, true, new Se3_F64(), layout.size());
    SceneStructureMetric.Rigid rigid = structure.rigids.data[0];
    for (int i = 0; i < layout.size(); i++) {
        rigid.setPoint(i, layout.get(i).x, layout.get(i).y, 0);
    }
    // initialize the views. Right views will be relative to left and will share the same baseline
    int left_to_right_idx = structure.addMotion(false, left_to_right);
    for (int viewIndex = 0; viewIndex < numViews; viewIndex++) {
        int world_to_left_idx = structure.addMotion(false, structureLeft.motions.get(viewIndex).motion);
        structure.setView(viewIndex * 2, 0, world_to_left_idx, -1);
        structure.setView(viewIndex * 2 + 1, 1, left_to_right_idx, viewIndex * 2);
    }
    // Add observations for left and right camera
    observations.initialize(structure.views.size, true);
    for (int viewIndex = 0; viewIndex < numViews; viewIndex++) {
        SceneObservations.View oviewLeft = observations.getViewRigid(viewIndex * 2);
        CalibrationObservation left = calibLeft.observations.get(viewIndex);
        for (int j = 0; j < left.size(); j++) {
            PointIndex2D_F64 p = left.get(j);
            oviewLeft.add(p.index, (float) p.p.x, (float) p.p.y);
            structure.connectPointToView(p.index, viewIndex * 2);
        }
    }
    for (int viewIndex = 0; viewIndex < numViews; viewIndex++) {
        SceneObservations.View oviewRight = observations.getViewRigid(viewIndex * 2 + 1);
        CalibrationObservation right = calibRight.observations.get(viewIndex);
        for (int j = 0; j < right.size(); j++) {
            PointIndex2D_F64 p = right.get(j);
            oviewRight.add(p.index, (float) p.p.x, (float) p.p.y);
            structure.connectPointToView(p.index, viewIndex * 2 + 1);
        }
    }
    if (verbose != null)
        verbose.println("Joint bundle adjustment");
    if (!bundleUtils.process())
        return;
    // save the output
    structure.motions.get(left_to_right_idx).motion.invert(parameters.right_to_left);
    BundleAdjustmentOps.convert(((BundlePinholeBrown) structure.cameras.get(0).model), parameters.left.width, parameters.left.height, parameters.left);
    BundleAdjustmentOps.convert(((BundlePinholeBrown) structure.cameras.get(1).model), parameters.left.width, parameters.left.height, parameters.right);
}

Example 10 with SceneObservations

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

the class CalibrationPlanarGridZhang99 method convertIntoBundleStructure.

/**
 * Convert it into a data structure understood by {@link BundleAdjustment}
 */
public void convertIntoBundleStructure(List<Se3_F64> motions, DMatrixRMaj K, List<DMatrixRMaj> homographies, List<CalibrationObservation> calibrationObservations) {
    structure = new SceneStructureMetric(false);
    structure.initialize(1, motions.size(), -1, layout.size(), 1);
    observations = new SceneObservations();
    observations.initialize(motions.size(), true);
    // A single camera is assumed, that's what is being calibrated!
    structure.setCamera(0, false, cameraGenerator.initializeCamera(K, homographies, calibrationObservations));
    // A single rigid planar target is being viewed. It is assumed to be centered at the origin
    structure.setRigid(0, true, new Se3_F64(), layout.size());
    // Where the points are on the calibration target
    SceneStructureMetric.Rigid rigid = structure.rigids.data[0];
    for (int i = 0; i < layout.size(); i++) {
        rigid.setPoint(i, layout.get(i).x, layout.get(i).y, 0);
    }
    // Add the initial estimate of each view's location and the points observed
    for (int viewIdx = 0; viewIdx < motions.size(); viewIdx++) {
        structure.setView(viewIdx, 0, false, motions.get(viewIdx));
        SceneObservations.View v = observations.getViewRigid(viewIdx);
        CalibrationObservation ca = calibrationObservations.get(viewIdx);
        for (int j = 0; j < ca.size(); j++) {
            PointIndex2D_F64 p = ca.get(j);
            v.add(p.index, (float) p.p.x, (float) p.p.y);
            structure.connectPointToView(p.index, viewIdx);
        }
    }
}