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

Example 36 with Point4D_F64

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

the class TestTriangulateMetricLinearDLT method triangulate_two.

/**
 * Create 2 perfect observations and solve for the position
 */
@Test
void triangulate_two() {
    createScene();
    TriangulateMetricLinearDLT alg = new TriangulateMetricLinearDLT();
    Point4D_F64 found = new Point4D_F64();
    alg.triangulate(obsNorm.get(0), obsNorm.get(1), motionWorldToCamera.get(1), found);
    assertEquals(worldPoint.x, found.x / found.w, UtilEjml.TEST_F64);
    assertEquals(worldPoint.y, found.y / found.w, UtilEjml.TEST_F64);
    assertEquals(worldPoint.z, found.z / found.w, UtilEjml.TEST_F64);
}

Example 37 with Point4D_F64

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

the class MultiViewOps method sceneToCloudH.

/**
 * Converts the points in the scene into a homogenous point cloud. Results are passed in to the lambda.
 * It will work with a scene in homogenous or 3D coordinates.
 *
 * @param scene (Input) The scene
 * @param func (Output) Results are passed in to this function with their index and 3D point.
 */
public static void sceneToCloudH(SceneStructureMetric scene, BoofLambdas.ProcessIndex<Point4D_F64> func) {
    Point4D_F64 out = new Point4D_F64();
    final boolean homogenous = scene.isHomogenous();
    for (int pointIdx = 0; pointIdx < scene.points.size; pointIdx++) {
        SceneStructureCommon.Point point = scene.points.get(pointIdx);
        double x = point.coordinate[0];
        double y = point.coordinate[1];
        double z = point.coordinate[2];
        double w = homogenous ? point.coordinate[3] : 1.0;
        out.setTo(x, y, z, w);
        func.process(pointIdx, out);
    }
}

Example 38 with Point4D_F64

use of georegression.struct.point.Point4D_F64 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 39 with Point4D_F64

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

the class PruneStructureFromSceneProjective method pruneObservationsByErrorRank.

/**
 * Computes reprojection error for all features. Sorts the resulting residuals by magnitude.
 * Prunes observations which have the largest errors first. After calling this function you should
 * call {@link #prunePoints(int)} and {@link #pruneViews(int)} to ensure the scene is still valid.
 *
 * @param inlierFraction Fraction of observations to keep. 0 to 1. 1 = no change. 0 = everything is pruned.
 */
public void pruneObservationsByErrorRank(double inlierFraction) {
    Point2D_F64 observation = new Point2D_F64();
    Point2D_F64 predicted = new Point2D_F64();
    Point3D_F64 X3 = new Point3D_F64();
    Point4D_F64 X4 = new Point4D_F64();
    // Create a list of observation errors
    List<Errors> errors = new ArrayList<>();
    for (int viewIndex = 0; viewIndex < observations.views.size; viewIndex++) {
        SceneObservations.View v = observations.views.data[viewIndex];
        SceneStructureProjective.View view = structure.views.data[viewIndex];
        for (int indexInView = 0; indexInView < v.point.size; indexInView++) {
            int pointID = v.point.data[indexInView];
            SceneStructureCommon.Point f = structure.points.data[pointID];
            // Get observation in image pixels
            v.getPixel(indexInView, observation);
            // Get feature location in world and predict the pixel observation
            if (structure.homogenous) {
                f.get(X4);
                PerspectiveOps.renderPixel(view.worldToView, X4, predicted);
            } else {
                f.get(X3);
                PerspectiveOps.renderPixel(view.worldToView, X3, predicted);
            }
            Errors e = new Errors();
            e.view = viewIndex;
            e.pointIndexInView = indexInView;
            e.error = predicted.distance2(observation);
            errors.add(e);
        }
    }
    errors.sort(Comparator.comparingDouble(a -> a.error));
    // Mark observations which are to be removed. Can't remove yet since the indexes will change
    int index0 = (int) (errors.size() * inlierFraction + 0.5);
    for (int i = index0; i < errors.size(); i++) {
        Errors e = errors.get(i);
        SceneObservations.View v = observations.views.data[e.view];
        v.setPixel(e.pointIndexInView, Float.NaN, Float.NaN);
    }
    // Remove all marked features
    removeMarkedObservations();
}

Example 40 with Point4D_F64

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

the class TestPruneStructureFromSceneProjective method checkAllObservationsArePerfect.

/**
 * See if all the observations are perfect. This acts as a sanity check on the scenes structure after modification
 */
private void checkAllObservationsArePerfect() {
    Point4D_F64 X = new Point4D_F64();
    Point2D_F64 x = new Point2D_F64();
    Point2D_F64 y = new Point2D_F64();
    for (int viewIdx = 0; viewIdx < structure.views.size; viewIdx++) {
        SceneStructureProjective.View vs = structure.views.data[viewIdx];
        DMatrixRMaj P = vs.worldToView;
        SceneObservations.View vo = observations.views.data[viewIdx];
        for (int i = 0; i < vo.point.size; i++) {
            int pointIdx = vo.point.get(i);
            structure.points.data[pointIdx].get(X);
            GeometryMath_F64.mult(P, X, x);
            vo.getPixel(i, y);
            assertEquals(0, x.x - y.x, UtilEjml.TEST_F64_SQ);
            assertEquals(0, x.y - y.y, UtilEjml.TEST_F64_SQ);
        }
    }
}