Examples with Point4D_F64 georegression.struct.point.Point4D_F64 used on opensource projects

Example 41 with Point4D_F64

use of georegression.struct.point.Point4D_F64 in project BoofCV by lessthanoptimal.

the class TestPruneStructureFromSceneProjective method createPerfectScene.

private void createPerfectScene() {
    structure = new SceneStructureProjective(true);
    structure.initialize(10, 500);
    CameraPinhole intrinsic = new CameraPinhole(400, 410, 0.1, 500, 501, 1000, 1000);
    DMatrixRMaj K = PerspectiveOps.pinholeToMatrix(intrinsic, (DMatrixRMaj) null);
    for (int viewIdx = 0; viewIdx < structure.views.size; viewIdx++) {
        Se3_F64 worldToView = new Se3_F64();
        if (viewIdx > 0) {
            worldToView.T.x = rand.nextGaussian() * 0.5;
            worldToView.T.y = rand.nextGaussian() * 0.5;
            worldToView.T.z = rand.nextGaussian() * 0.05;
            // increased rotation variance until a few of the points weren't always visible
            ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, rand.nextGaussian() * 0.5, rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1, worldToView.R);
        }
        DMatrixRMaj cameraMatrix = PerspectiveOps.createCameraMatrix(worldToView.R, worldToView.T, K, null);
        structure.setView(viewIdx, viewIdx == 0, cameraMatrix, intrinsic.width, intrinsic.height);
    }
    List<Point4D_F64> points = UtilPoint4D_F64.randomN(center, 1.0, 0.5, structure.points.size, rand);
    for (int i = 0; i < points.size(); i++) {
        Point4D_F64 p = points.get(i);
        double s = rand.nextGaussian();
        if (// make sure it isn't scaled by zero
        Math.abs(s) < 1e-5)
            s = rand.nextDouble() + 0.01;
        p.scale(s);
        structure.points.data[i].set(p.x, p.y, p.z, p.w);
    }
    createRestOfScene();
}

Example 42 with Point4D_F64

use of georegression.struct.point.Point4D_F64 in project BoofCV by lessthanoptimal.

the class TestPositiveDepthConstraintCheckH method positive_NearlyPureRotation.

@Test
void positive_NearlyPureRotation() {
    // create transform from A to B
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0, -0.05, 0, null);
    Vector3D_F64 T = new Vector3D_F64(1e-4, 0, 0);
    Se3_F64 fromAtoB = new Se3_F64(R, T);
    // point in front of both cameras
    Point4D_F64 pt = new Point4D_F64(0, 0, 1e3, 1e-5);
    // create observations of the point in calibrated coordinates
    Point2D_F64 obsA = new Point2D_F64(0, 0);
    Point4D_F64 pt_inB = SePointOps_F64.transform(fromAtoB, pt, (Point4D_F64) null);
    Point2D_F64 obsB = new Point2D_F64(pt_inB.x / pt_inB.z, pt_inB.y / pt_inB.z);
    var alg = new PositiveDepthConstraintCheckH();
    assertTrue(alg.checkConstraint(obsA, obsB, fromAtoB));
}

Example 43 with Point4D_F64

use of georegression.struct.point.Point4D_F64 in project BoofCV by lessthanoptimal.

the class TestPositiveDepthConstraintCheckH method negative_NearlyPureRotation.

@Test
void negative_NearlyPureRotation() {
    // create transform from A to B
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0, -0.05, 0, null);
    Vector3D_F64 T = new Vector3D_F64(1e-4, 0, 0);
    Se3_F64 fromAtoB = new Se3_F64(R, T);
    // point in front of both cameras
    Point4D_F64 pt = new Point4D_F64(0, 0, -1e3, 1e-5);
    // create observations of the point in calibrated coordinates
    Point2D_F64 obsA = new Point2D_F64(0, 0);
    Point4D_F64 pt_inB = SePointOps_F64.transform(fromAtoB, pt, (Point4D_F64) null);
    Point2D_F64 obsB = new Point2D_F64(pt_inB.x / pt_inB.z, pt_inB.y / pt_inB.z);
    var alg = new PositiveDepthConstraintCheckH();
    assertFalse(alg.checkConstraint(obsA, obsB, fromAtoB));
}

Example 44 with Point4D_F64

use of georegression.struct.point.Point4D_F64 in project BoofCV by lessthanoptimal.

the class GenericBundleAdjustmentProjectiveChecks method checkReprojectionError.

static void checkReprojectionError(SceneStructureProjective structure, SceneObservations observations, double tol) {
    PointIndex2D_F64 o = new PointIndex2D_F64();
    Point2D_F64 predicted = new Point2D_F64();
    if (structure.homogenous) {
        Point4D_F64 p4 = new Point4D_F64();
        Point3D_F64 p3 = new Point3D_F64();
        for (int indexView = 0; indexView < observations.views.size; indexView++) {
            SceneObservations.View v = observations.views.data[indexView];
            for (int j = 0; j < v.point.size; j++) {
                v.getPixel(j, o);
                structure.points.data[o.index].get(p4);
                p3.x = p4.x / p4.w;
                p3.y = p4.y / p4.w;
                p3.z = p4.z / p4.w;
                PerspectiveOps.renderPixel(structure.views.data[indexView].worldToView, p3, predicted);
                double residual = o.p.distance(predicted);
                if (Math.abs(residual) > tol)
                    fail("Error is too large. " + residual);
            }
        }
    } else {
        Point3D_F64 p3 = new Point3D_F64();
        for (int indexView = 0; indexView < observations.views.size; indexView++) {
            SceneObservations.View v = observations.views.data[indexView];
            for (int j = 0; j < v.point.size; j++) {
                v.getPixel(j, o);
                structure.points.data[o.index].get(p3);
                PerspectiveOps.renderPixel(structure.views.data[indexView].worldToView, p3, predicted);
                double residual = o.p.distance(predicted);
                if (Math.abs(residual) > tol)
                    fail("Error is too large. " + residual);
            }
        }
    }
}

Example 45 with Point4D_F64

use of georegression.struct.point.Point4D_F64 in project BoofCV by lessthanoptimal.

the class RefineThreeViewProjectiveGeometric method initializeStructure.

/**
 * Sets up data structures for SBA
 */
private void initializeStructure(List<AssociatedTriple> listObs, DMatrixRMaj P2, DMatrixRMaj P3) {
    List<DMatrixRMaj> cameraMatrices = new ArrayList<>();
    cameraMatrices.add(P1);
    cameraMatrices.add(P2);
    cameraMatrices.add(P3);
    List<Point2D_F64> triangObs = new ArrayList<>();
    triangObs.add(null);
    triangObs.add(null);
    triangObs.add(null);
    structure = new SceneStructureProjective(true);
    structure.initialize(3, listObs.size());
    observations = new SceneObservations();
    observations.initialize(3);
    structure.setView(0, true, P1, 0, 0);
    structure.setView(1, false, P2, 0, 0);
    structure.setView(2, false, P3, 0, 0);
    boolean needsPruning = false;
    Point4D_F64 X = new Point4D_F64();
    for (int i = 0; i < listObs.size(); i++) {
        AssociatedTriple t = listObs.get(i);
        triangObs.set(0, t.p1);
        triangObs.set(1, t.p2);
        triangObs.set(2, t.p3);
        // simulated 3D scenes
        if (triangulator.triangulate(triangObs, cameraMatrices, X)) {
            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.points.get(i).set(X.x, X.y, X.z, X.w);
        } else {
            needsPruning = true;
        }
    }
    if (needsPruning) {
        PruneStructureFromSceneProjective pruner = new PruneStructureFromSceneProjective(structure, observations);
        pruner.prunePoints(1);
    }
}