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

Example 21 with Point3D_F64

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

the class TestDecomposeEssential method checkUnique.

/**
 * Makes sure each solution returned is unique by transforming a point.
 */
public static void checkUnique(List<Se3_F64> solutions) {
    Point3D_F64 orig = new Point3D_F64(1, 2, 3);
    for (int i = 0; i < solutions.size(); i++) {
        Point3D_F64 found = SePointOps_F64.transform(solutions.get(i), orig, null);
        for (int j = i + 1; j < solutions.size(); j++) {
            Point3D_F64 alt = SePointOps_F64.transform(solutions.get(j), orig, null);
            GeometryUnitTest.assertNotEquals(found, alt, 1e-4);
        }
    }
}

Example 22 with Point3D_F64

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

the class VisOdomPixelDepthPnP method performSecondPass.

private boolean performSecondPass(List<PointTrack> active, List<Point2D3D> obs) {
    Se3_F64 keyToCurr = motionEstimator.getModelParameters();
    Point3D_F64 cameraPt = new Point3D_F64();
    Point2D_F64 predicted = new Point2D_F64();
    // predict where each track should be given the just estimated motion
    List<PointTrack> all = tracker.getAllTracks(null);
    for (PointTrack t : all) {
        Point2D3D p = t.getCookie();
        SePointOps_F64.transform(keyToCurr, p.location, cameraPt);
        normToPixel.compute(cameraPt.x / cameraPt.z, cameraPt.y / cameraPt.z, predicted);
        tracker.setHint(predicted.x, predicted.y, t);
    }
    // redo tracking with the additional information
    tracker.performSecondPass();
    active.clear();
    obs.clear();
    tracker.getActiveTracks(active);
    for (PointTrack t : active) {
        Point2D3D p = t.getCookie();
        pixelToNorm.compute(t.x, t.y, p.observation);
        obs.add(p);
    }
    return motionEstimator.process(obs);
}

Example 23 with Point3D_F64

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

the class VisualDepthOps method depthTo3D.

/**
 * Creates a point cloud from a depth image and saves the color information.  The depth and color images are
 * assumed to be aligned.
 * @param param Intrinsic camera parameters for depth image
 * @param depth depth image.  each value is in millimeters.
 * @param rgb Color image that's aligned to the depth.
 * @param cloud Output point cloud
 * @param cloudColor Output color for each point in the cloud
 */
public static void depthTo3D(CameraPinholeRadial param, Planar<GrayU8> rgb, GrayU16 depth, FastQueue<Point3D_F64> cloud, FastQueueArray_I32 cloudColor) {
    cloud.reset();
    cloudColor.reset();
    RemoveRadialPtoN_F64 p2n = new RemoveRadialPtoN_F64();
    p2n.setK(param.fx, param.fy, param.skew, param.cx, param.cy).setDistortion(param.radial, param.t1, param.t2);
    Point2D_F64 n = new Point2D_F64();
    GrayU8 colorR = rgb.getBand(0);
    GrayU8 colorG = rgb.getBand(1);
    GrayU8 colorB = rgb.getBand(2);
    for (int y = 0; y < depth.height; y++) {
        int index = depth.startIndex + y * depth.stride;
        for (int x = 0; x < depth.width; x++) {
            int mm = depth.data[index++] & 0xFFFF;
            // skip pixels with no depth information
            if (mm == 0)
                continue;
            // this could all be precomputed to speed it up
            p2n.compute(x, y, n);
            Point3D_F64 p = cloud.grow();
            p.z = mm;
            p.x = n.x * p.z;
            p.y = n.y * p.z;
            int[] color = cloudColor.grow();
            color[0] = colorR.unsafe_get(x, y);
            color[1] = colorG.unsafe_get(x, y);
            color[2] = colorB.unsafe_get(x, y);
        }
    }
}

Example 24 with Point3D_F64

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

the class MonoOverhead_to_MonocularPlaneVisualOdometry method computeTracks.

private void computeTracks() {
    if (computed)
        return;
    if (!(alg.getMotion2D() instanceof AccessPointTracks))
        return;
    AccessPointTracks accessPlane = (AccessPointTracks) alg.getMotion2D();
    List<Point2D_F64> tracksPlane = accessPlane.getAllTracks();
    OverheadView<T> map = alg.getOverhead();
    points3D.reset();
    pixels.reset();
    for (Point2D_F64 worldPt : tracksPlane) {
        // 2D to 3D
        Point3D_F64 p = points3D.grow();
        p.z = worldPt.x * map.cellSize - map.centerX;
        p.x = -(worldPt.y * map.cellSize - map.centerY);
        p.y = 0;
        // 3D world to camera
        SePointOps_F64.transform(planeToCamera, p, p);
        // normalized image coordinates
        normToPixel.compute(p.x / p.z, p.y / p.z, pixels.grow());
    }
    computed = true;
}

Example 25 with Point3D_F64

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

the class BenchmarkStabilityFundamental method createSceneCube.

public void createSceneCube() {
    scene = new ArrayList<>();
    for (int i = 0; i < totalPoints; i++) {
        Point3D_F64 p = new Point3D_F64();
        p.x = (rand.nextDouble() - 0.5) * 2 * sceneRadius;
        p.y = (rand.nextDouble() - 0.5) * 2 * sceneRadius;
        p.z = (rand.nextDouble() - 0.5) * 2 * sceneRadius + sceneCenterZ;
        scene.add(p);
    }
}