Examples with Rodrigues_F64 georegression.struct.so.Rodrigues_F64 used on opensource projects

Example 1 with Rodrigues_F64

use of georegression.struct.so.Rodrigues_F64 in project BoofCV by lessthanoptimal.

the class PointCloudViewerPanelFX method setCameraToWorld.

public void setCameraToWorld(Se3_F64 cameraToWorld) {
    camera.setTranslateX(cameraToWorld.T.x);
    camera.setTranslateY(cameraToWorld.T.y);
    camera.setTranslateZ(cameraToWorld.T.z);
    Rodrigues_F64 rod = new Rodrigues_F64();
    ConvertRotation3D_F64.matrixToRodrigues(cameraToWorld.R, rod);
    Point3D V = new Point3D(rod.unitAxisRotation.x, rod.unitAxisRotation.y, rod.unitAxisRotation.z);
    camera.setRotationAxis(V);
    camera.setRotate(UtilAngle.degree(rod.theta));
}

Example 2 with Rodrigues_F64

use of georegression.struct.so.Rodrigues_F64 in project BoofCV by lessthanoptimal.

the class ThreeViewEstimateMetricScene method pruneOutliers.

/**
 * Prunes the features with the largest reprojection error
 */
private void pruneOutliers(BundleAdjustment<SceneStructureMetric> bundleAdjustment) {
    // see if it's configured to not prune
    if (pruneFraction == 1.0)
        return;
    if (verbose != null)
        verbose.println("Pruning Outliers");
    PruneStructureFromSceneMetric pruner = new PruneStructureFromSceneMetric(structure, observations);
    pruner.pruneObservationsByErrorRank(pruneFraction);
    pruner.pruneViews(10);
    pruner.pruneUnusedMotions();
    pruner.prunePoints(1);
    bundleAdjustment.setParameters(structure, observations);
    double before = bundleAdjustment.getFitScore();
    bundleAdjustment.optimize(structure);
    if (verbose != null)
        verbose.println("   before " + before + " after " + bundleAdjustment.getFitScore());
    if (verbose != null) {
        verbose.println("\nCamera");
        for (int i = 0; i < structure.cameras.size; i++) {
            verbose.println("  " + Objects.requireNonNull(structure.cameras.data[i].getModel()).toString());
        }
        verbose.println("\nworldToView");
        for (int i = 0; i < structure.views.size; i++) {
            if (verbose != null) {
                Se3_F64 se = structure.getParentToView(i);
                Rodrigues_F64 rod = ConvertRotation3D_F64.matrixToRodrigues(se.R, null);
                verbose.println("  T=" + se.T + "  R=" + rod);
            }
        }
    }
}

Example 3 with Rodrigues_F64

use of georegression.struct.so.Rodrigues_F64 in project BoofCV by lessthanoptimal.

the class MockLookupSimilarImagesCircleAround method init.

/**
 * Configures the scene
 *
 * @param numViews number of views to create
 * @param numViewConnect Specifies 1/2 the number of views each view will be connected to.
 */
public MockLookupSimilarImagesCircleAround init(int numViews, int numViewConnect) {
    for (int viewCnt = 0; viewCnt < numViews; viewCnt++) {
        viewIds.add("View_" + viewCnt);
    }
    DMatrixRMaj K = PerspectiveOps.pinholeToMatrix(intrinsic, (DMatrixRMaj) null);
    DMatrixRMaj K_zero = K.copy();
    K_zero.set(0, 2, 0.0);
    K_zero.set(1, 2, 0.0);
    // Randomly add points around the coordinate system's origin
    feats3D = UtilPoint3D_F64.random(new Point3D_F64(0, 0, 0), -0.5, 0.5, numFeatures, rand);
    // Radius of the cameras circling the origin
    double pathRadius = 2;
    // render pixel coordinates of all points
    for (int viewCnt = 0; viewCnt < numViews; viewCnt++) {
        Se3_F64 camera_to_world = new Se3_F64();
        Se3_F64 world_to_camera = new Se3_F64();
        double yaw = 2.0 * Math.PI * viewCnt / numViews;
        // camera lie on the (X,Z) plane with +y pointed down.
        // This is done to make the camera coordinate system and the world coordinate system have a more close
        // relationship
        camera_to_world.T.x = Math.cos(yaw) * pathRadius;
        // geometric diversity for self calibration
        camera_to_world.T.y = rand.nextGaussian() * pathRadius * 0.1;
        camera_to_world.T.z = Math.sin(yaw) * pathRadius;
        // camera is pointing in the opposite direction of it's world location
        ConvertRotation3D_F64.rodriguesToMatrix(new Rodrigues_F64(yaw + Math.PI / 2, 0, -1, 0), camera_to_world.R);
        camera_to_world.invert(world_to_camera);
        // Create the camera matrix P
        DMatrixRMaj P = PerspectiveOps.createCameraMatrix(world_to_camera.R, world_to_camera.T, K, null);
        DMatrixRMaj P_zero = PerspectiveOps.createCameraMatrix(world_to_camera.R, world_to_camera.T, K_zero, null);
        // save information on the view
        listOriginToView.add(world_to_camera);
        listCameraMatrices.add(P);
        listCameraMatricesZeroPrinciple.add(P_zero);
        // Observed features in the view
        List<Point2D_F64> viewPixels = new ArrayList<>();
        viewObs.add(viewPixels);
        // create look up table from view to feature
        // we don't want features to have same index because that's not realistic and would hide bugs
        int[] v2f = PrimitiveArrays.fillCounting(numFeatures);
        PrimitiveArrays.shuffle(v2f, 0, numFeatures, rand);
        viewToFeat.add(v2f);
        // save reverse table for fast lookup later
        int[] f2v = new int[numFeatures];
        for (int j = 0; j < numFeatures; j++) {
            f2v[v2f[j]] = j;
        }
        featToView.add(f2v);
        // note the featIdx is the index of the feature in the view
        for (int featCnt = 0; featCnt < feats3D.size(); featCnt++) {
            Point3D_F64 X = feats3D.get(v2f[featCnt]);
            Point2D_F64 pixel = PerspectiveOps.renderPixel(world_to_camera, intrinsic, X, null);
            if (pixel == null)
                throw new RuntimeException("Out of FOV");
            viewPixels.add(pixel);
        }
    }
    // Create the pairwise graph
    for (int i = 0; i < numViews; i++) {
        View v = graph.createNode(viewIds.get(i));
        v.totalObservations = numFeatures;
    }
    // Only connect neighbors to each other
    for (int viewIdxI = 0; viewIdxI < numViews; viewIdxI++) {
        View vi = graph.nodes.get(viewIdxI);
        for (int neighborOffset = 1; neighborOffset <= numViewConnect; neighborOffset++) {
            int viewIdxJ = viewIdxI + neighborOffset;
            if (viewIdxJ >= numViews)
                break;
            // next view while wrapping around
            View vj = graph.nodes.get(viewIdxJ);
            // mix of the src/dst to exercise more code during testing
            boolean standardOrder = (viewIdxI + neighborOffset) % 2 == 0;
            PairwiseImageGraph.Motion m = standardOrder ? graph.connect(vi, vj) : graph.connect(vj, vi);
            m.score3D = 5.0 / 7.0;
            m.is3D = true;
            int[] tableI = featToView.get(viewIdxI);
            int[] tableJ = featToView.get(viewIdxJ);
            for (int i = 0; i < numFeatures; i++) {
                if (standardOrder) {
                    m.inliers.grow().setTo(tableI[i], tableJ[i], 0.0);
                } else {
                    m.inliers.grow().setTo(tableJ[i], tableI[i], 0.0);
                }
            }
        }
    }
    return this;
}

Example 4 with Rodrigues_F64

use of georegression.struct.so.Rodrigues_F64 in project BoofCV by lessthanoptimal.

the class MockLookupSimilarImagesRealistic method pathCircle.

public MockLookupSimilarImagesRealistic pathCircle(int numViews, int numViewConnect) {
    // Radius of the cameras circling the origin
    double pathRadius = 2;
    for (Point3D_F64 X : UtilPoint3D_F64.random(new Point3D_F64(0, 0, 0), -0.5, 0.5, numFeatures, rand)) {
        Feature f = new Feature();
        f.world.setTo(X);
        f.featureIdx = points.size();
        points.add(f);
    }
    List<Se3_F64> list_camera_to_world = new ArrayList<>();
    for (int viewCnt = 0; viewCnt < numViews; viewCnt++) {
        Se3_F64 camera_to_world = new Se3_F64();
        double yaw = 2.0 * Math.PI * viewCnt / numViews;
        // camera lie on the (X,Z) plane with +y pointed down.
        // This is done to make the camera coordinate system and the world coordinate system have a more close
        // relationship
        camera_to_world.T.x = Math.cos(yaw) * pathRadius;
        // geometric diversity for self calibration
        camera_to_world.T.y = rand.nextGaussian() * pathRadius * 0.1;
        camera_to_world.T.z = Math.sin(yaw) * pathRadius;
        // camera is pointing in the opposite direction of it's world location
        ConvertRotation3D_F64.rodriguesToMatrix(new Rodrigues_F64(yaw + Math.PI / 2, 0, -1, 0), camera_to_world.R);
        list_camera_to_world.add(camera_to_world);
    }
    generate(list_camera_to_world, numViewConnect);
    return this;
}

Example 5 with Rodrigues_F64

use of georegression.struct.so.Rodrigues_F64 in project BoofCV by lessthanoptimal.

the class BoofTesting method assertEquals.

public static void assertEquals(Se3_F64 expected, Se3_F64 found, double tolAngle, double tolT) {
    var R = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.multTransA(expected.R, found.R, R);
    Rodrigues_F64 rod = ConvertRotation3D_F64.matrixToRodrigues(R, null);
    assertEquals(0.0, rod.theta, tolAngle);
    assertEquals(0.0, found.T.distance(expected.T), tolT);
}