Examples with Stereo2D3D boofcv.struct.sfm.Stereo2D3D used on opensource projects

Example 1 with Stereo2D3D

use of boofcv.struct.sfm.Stereo2D3D in project BoofCV by lessthanoptimal.

the class VisOdomDualTrackPnP method refineMotionEstimate.

/**
 * Non-linear refinement of motion estimate
 */
private void refineMotionEstimate() {
    // use observations from the inlier set
    List<Stereo2D3D> data = new ArrayList<>();
    int N = matcher.getMatchSet().size();
    for (int i = 0; i < N; i++) {
        int index = matcher.getInputIndex(i);
        PointTrack l = candidates.get(index);
        LeftTrackInfo info = l.getCookie();
        PointTrack r = info.right;
        Stereo2D3D stereo = info.location;
        // compute normalized image coordinate for track in left and right image
        leftImageToNorm.compute(l.x, l.y, info.location.leftObs);
        rightImageToNorm.compute(r.x, r.y, info.location.rightObs);
        data.add(stereo);
    }
    // refine the motion estimate using non-linear optimization
    Se3_F64 keyToCurr = currToKey.invert(null);
    Se3_F64 found = new Se3_F64();
    if (modelRefiner.fitModel(data, keyToCurr, found)) {
        found.invert(currToKey);
    }
}

Example 2 with Stereo2D3D

use of boofcv.struct.sfm.Stereo2D3D in project BoofCV by lessthanoptimal.

the class VisOdomDualTrackPnP method estimateMotion.

/**
 * Given the set of active tracks, estimate the cameras motion robustly
 *
 * @return true if successful
 */
private boolean estimateMotion() {
    CameraModel leftCM = cameraModels.get(CAMERA_LEFT);
    CameraModel rightCM = cameraModels.get(CAMERA_RIGHT);
    // Perform motion estimation relative to the most recent key frame
    previousLeft.frame_to_world.invert(world_to_prev);
    // Put observation and prior knowledge into a format the model matcher will understand
    listStereo2D3D.reserve(candidates.size());
    listStereo2D3D.reset();
    for (int candidateIdx = 0; candidateIdx < candidates.size(); candidateIdx++) {
        PointTrack l = candidates.get(candidateIdx);
        Stereo2D3D stereo = listStereo2D3D.grow();
        // Get the track location
        TrackInfo bt = l.getCookie();
        PointTrack r = bt.visualRight;
        // Get the 3D coordinate of the point in the 'previous' frame
        SePointOps_F64.transform(world_to_prev, bt.worldLoc, prevLoc4);
        PerspectiveOps.homogenousTo3dPositiveZ(prevLoc4, 1e8, 1e-8, stereo.location);
        // compute normalized image coordinate for track in left and right image
        leftCM.pixelToNorm.compute(l.pixel.x, l.pixel.y, stereo.leftObs);
        rightCM.pixelToNorm.compute(r.pixel.x, r.pixel.y, stereo.rightObs);
    // TODO Could this transform be done just once?
    }
    // Robustly estimate left camera motion
    if (!matcher.process(listStereo2D3D.toList()))
        return false;
    if (modelRefiner != null) {
        modelRefiner.fitModel(matcher.getMatchSet(), matcher.getModelParameters(), previous_to_current);
    } else {
        previous_to_current.setTo(matcher.getModelParameters());
    }
    // Convert the found transforms back to world
    previous_to_current.invert(current_to_previous);
    current_to_previous.concat(previousLeft.frame_to_world, currentLeft.frame_to_world);
    right_to_left.concat(currentLeft.frame_to_world, currentRight.frame_to_world);
    return true;
}

Example 3 with Stereo2D3D

use of boofcv.struct.sfm.Stereo2D3D in project BoofCV by lessthanoptimal.

the class VisOdomStereoQuadPnP method robustMotionEstimate.

/**
 * Robustly estimate the motion
 */
private boolean robustMotionEstimate() {
    // create a list of observations and known 3D locations for motion finding
    modelFitData.reset();
    // use 0 -> 1 stereo associations to estimate each feature's 3D position
    for (int i = 0; i < consistentTracks.size(); i++) {
        TrackQuad quad = consistentTracks.get(i);
        Stereo2D3D data = modelFitData.grow();
        leftPixelToNorm.compute(quad.v2.x, quad.v2.y, data.leftObs);
        rightPixelToNorm.compute(quad.v3.x, quad.v3.y, data.rightObs);
        data.location.setTo(quad.X);
    }
    // robustly match the data
    if (!matcher.process(modelFitData.toList())) {
        return false;
    }
    // mark features which are inliers
    int numInliers = matcher.getMatchSet().size();
    for (int i = 0; i < numInliers; i++) {
        consistentTracks.get(matcher.getInputIndex(i)).inlier = true;
    }
    return true;
}

Example 4 with Stereo2D3D

use of boofcv.struct.sfm.Stereo2D3D in project BoofCV by lessthanoptimal.

the class PnPStereoJacobianRodrigues method process.

@Override
public void process(double[] input, DMatrixRMaj J) {
    this.output = J.data;
    // initialize data structures
    rodrigues.setParamVector(input[0], input[1], input[2]);
    rodJacobian.process(input[0], input[1], input[2]);
    worldToLeft.T.x = input[3];
    worldToLeft.T.y = input[4];
    worldToLeft.T.z = input[5];
    ConvertRotation3D_F64.rodriguesToMatrix(rodrigues, worldToLeft.getR());
    // compute the gradient for each observation
    for (int i = 0; i < observations.size(); i++) {
        Stereo2D3D o = observations.get(i);
        // --------- Left camera observations
        SePointOps_F64.transform(worldToLeft, o.location, cameraPt);
        indexX = 4 * 6 * i;
        indexY = indexX + 6;
        // add gradient from rotation
        addRodriguesJacobian(rodJacobian.Rx, o.location, cameraPt);
        addRodriguesJacobian(rodJacobian.Ry, o.location, cameraPt);
        addRodriguesJacobian(rodJacobian.Rz, o.location, cameraPt);
        // add gradient from translation
        addTranslationJacobian(cameraPt);
        // --------- Right camera observations
        SePointOps_F64.transform(leftToRight, cameraPt, cameraPt);
        indexX = indexY;
        indexY = indexY + 6;
        CommonOps_DDRM.mult(leftToRight.getR(), rodJacobian.Rx, rotR);
        addRodriguesJacobian(rotR, o.location, cameraPt);
        CommonOps_DDRM.mult(leftToRight.getR(), rodJacobian.Ry, rotR);
        addRodriguesJacobian(rotR, o.location, cameraPt);
        CommonOps_DDRM.mult(leftToRight.getR(), rodJacobian.Rz, rotR);
        addRodriguesJacobian(rotR, o.location, cameraPt);
        addTranslationJacobian(leftToRight.getR(), cameraPt);
    }
}

Example 5 with Stereo2D3D

use of boofcv.struct.sfm.Stereo2D3D in project BoofCV by lessthanoptimal.

the class TestPnPStereoDistanceReprojectionSq method checkBehind.

public void checkBehind(double Tz, double Pz) {
    // Point location in world frame
    Point3D_F64 X = new Point3D_F64(0.1, -0.04, Pz);
    DMatrixRMaj K_left = PerspectiveOps.pinholeToMatrix(param.left, (DMatrixRMaj) null);
    DMatrixRMaj K_right = PerspectiveOps.pinholeToMatrix(param.right, (DMatrixRMaj) null);
    // create a noisy observed
    Point2D_F64 obsLeft = PerspectiveOps.renderPixel(worldToLeft, K_left, X, null);
    Point2D_F64 obsRight = PerspectiveOps.renderPixel(worldToRight, K_right, X, null);
    // convert to normalized image coordinates
    PerspectiveOps.convertPixelToNorm(K_left, obsLeft, obsLeft);
    PerspectiveOps.convertPixelToNorm(K_right, obsRight, obsRight);
    PnPStereoDistanceReprojectionSq alg = new PnPStereoDistanceReprojectionSq();
    StereoParameters param = new StereoParameters(this.param.left, this.param.right, new Se3_F64());
    param.right_to_left.getT().setTo(0.1, 0, Tz);
    alg.setStereoParameters(param);
    alg.setModel(new Se3_F64());
    double found = alg.distance(new Stereo2D3D(obsLeft, obsRight, X));
    assertEquals(found, Double.MAX_VALUE);
}