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

Example 6 with Point4D_F64

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

the class WrapVisOdomMonoStereoDepthPnP method getTrackWorld3D.

@Override
public boolean getTrackWorld3D(int index, Point3D_F64 world) {
    Point4D_F64 p = alg.getVisibleTracks().get(index).worldLoc;
    world.setTo(p.x / p.w, p.y / p.w, p.z / p.w);
    return true;
}

Example 7 with Point4D_F64

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

the class TestResolveSceneScaleAmbiguity method triangulate.

@Test
void triangulate() {
    double scale = 1.5;
    ResolveSceneScaleAmbiguity.SceneInfo scene = createInfo(scale, 1, 2, 3);
    var alg = new ResolveSceneScaleAmbiguity();
    alg.initialize(listFeatures3D.size());
    assertTrue(alg.triangulate(scene, 10));
    var expected = new Point3D_F64();
    listWorldToView.get(1).transform(listFeatures3D.get(10), expected);
    Point4D_F64 found = scene.location;
    assertEquals(scale * expected.z, found.z / found.w, UtilEjml.TEST_F64);
}

Example 8 with Point4D_F64

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

the class MetricSanityChecks method checkPhysicalConstraints.

public boolean checkPhysicalConstraints(SceneStructureMetric structure, SceneObservations observations, List<CameraPinholeBrown> listPriors) {
    BoofMiscOps.checkEq(listPriors.size(), structure.views.size);
    for (int i = 0; i < structure.cameras.size; i++) {
        BundlePinholeSimplified pinhole = (BundlePinholeSimplified) structure.cameras.get(i).model;
        if (pinhole.f < 0.0f) {
            if (verbose != null)
                verbose.println("Bad focal length. f=" + pinhole.f);
            return false;
        }
    }
    badFeatures.resetResize(structure.points.size, false);
    var worldP = new Point4D_F64(0, 0, 0, 1);
    var viewP = new Point4D_F64();
    var observedPixel = new Point2D_F64();
    var predictdPixel = new Point2D_F64();
    for (int viewIdx = 0; viewIdx < observations.views.size; viewIdx++) {
        int cameraIdx = structure.views.get(viewIdx).camera;
        BundlePinholeSimplified pinhole = (BundlePinholeSimplified) Objects.requireNonNull(structure.cameras.get(cameraIdx).model);
        CameraPinholeBrown priorCamera = listPriors.get(viewIdx);
        int width = priorCamera.width;
        int height = priorCamera.height;
        // Used to compensates for the lens model having its origin at the image center
        float cx = (float) priorCamera.cx;
        float cy = (float) priorCamera.cy;
        // Number of times each test failed in this particular view
        int failedBehind = 0;
        int failedImageBounds = 0;
        int failedReprojection = 0;
        Se3_F64 world_to_view = structure.getParentToView(viewIdx);
        SceneObservations.View oview = observations.views.get(viewIdx);
        for (int i = 0; i < oview.size(); i++) {
            // If true then this feature failed one of the constraints test in tis value
            boolean badObservation = false;
            oview.getPixel(i, observedPixel);
            SceneStructureCommon.Point p = structure.points.get(oview.getPointId(i));
            worldP.x = p.getX();
            worldP.y = p.getY();
            worldP.z = p.getZ();
            if (structure.isHomogenous()) {
                worldP.w = p.getW();
            }
            // worldP.w = 1 was already set for 3D points
            SePointOps_F64.transform(world_to_view, worldP, viewP);
            if (PerspectiveOps.isBehindCamera(viewP)) {
                badObservation = true;
                failedBehind++;
            }
            pinhole.project(viewP.x, viewP.y, viewP.z, predictdPixel);
            double reprojectionError = predictdPixel.distance2(observedPixel);
            if (reprojectionError > maxReprojectionErrorSq) {
                badObservation = true;
                failedReprojection++;
            }
            if (!BoofMiscOps.isInside(width, height, predictdPixel.x + cx, predictdPixel.y + cy)) {
                badObservation = true;
                failedImageBounds++;
            }
            if (badObservation) {
                badFeatures.set(oview.getPointId(i), true);
            }
        }
        if (verbose != null)
            verbose.printf("view[%d] errors: behind=%d bounds=%d reprojection=%d, obs=%d\n", viewIdx, failedBehind, failedImageBounds, failedReprojection, oview.size());
    }
    return true;
}

Example 9 with Point4D_F64

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

the class VisOdomBundleAdjustment method setupBundleStructure.

/**
 * Converts input data into a format that bundle adjustment can understand
 */
private void setupBundleStructure() {
    // Need to count the total number of tracks that will be feed into bundle adjustment
    int totalBundleTracks = selectedTracks.size();
    // Initialize data structures
    final SceneStructureMetric structure = bundle.getStructure();
    final SceneObservations observations = bundle.getObservations();
    observations.initialize(frames.size);
    structure.initialize(cameras.size, frames.size, totalBundleTracks);
    for (int cameraIdx = 0; cameraIdx < cameras.size; cameraIdx++) {
        structure.setCamera(cameraIdx, true, cameras.get(cameraIdx).bundleCamera);
    }
    // TODO make the first frame at origin. This is done to avoid numerical after traveling a good distance
    for (int frameIdx = 0; frameIdx < frames.size; frameIdx++) {
        BFrame bf = frames.get(frameIdx);
        bf.frame_to_world.invert(world_to_view);
        structure.setView(frameIdx, bf.camera.index, frameIdx == 0, world_to_view);
        // save the index since it's needed in the next loop
        frames.get(frameIdx).listIndex = frameIdx;
    }
    // A feature is only passed to SBA if it is active and more than one view has seen it
    // this requires it to have a different index
    int featureBundleIdx = 0;
    for (int trackIdx = 0; trackIdx < tracks.size; trackIdx++) {
        BTrack bt = tracks.get(trackIdx);
        if (!bt.selected) {
            continue;
        }
        Point4D_F64 p = bt.worldLoc;
        structure.setPoint(featureBundleIdx, p.x, p.y, p.z, p.w);
        for (int obsIdx = 0; obsIdx < bt.observations.size; obsIdx++) {
            BObservation o = bt.observations.get(obsIdx);
            SceneObservations.View view = observations.getView(o.frame.listIndex);
            view.add(featureBundleIdx, (float) o.pixel.x, (float) o.pixel.y);
        }
        featureBundleIdx++;
    }
    // Sanity check
    if (featureBundleIdx != structure.points.size)
        throw new RuntimeException("BUG! tracks feed in and points don't match");
}

Example 10 with Point4D_F64

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

the class TestVisOdomBundleAdjustment method addTrack.

@Test
void addTrack() {
    VisOdomBundleAdjustment<BTrack> alg = createAlgSingleCamera();
    BTrack trackA = alg.addTrack(1, 2, 3, 3);
    BTrack trackB = alg.addTrack(1, 2, 3, 4);
    BTrack trackC = alg.addTrack(1, 2, 3, 5);
    assertEquals(3, alg.tracks.size);
    assertTrue(alg.tracks.contains(trackA));
    assertTrue(alg.tracks.contains(trackB));
    assertTrue(alg.tracks.contains(trackC));
    assertEquals(0.0, trackB.worldLoc.distance(new Point4D_F64(1, 2, 3, 4)), UtilEjml.TEST_F64);
    assertEquals(0.0, trackC.worldLoc.distance(new Point4D_F64(1, 2, 3, 5)), UtilEjml.TEST_F64);
    trackA.observations.grow();
    trackB.observations.grow();
    // Test recycle
    alg.reset();
    trackA = alg.addTrack(2, 2, 3, 3);
    trackB = alg.addTrack(2, 2, 3, 4);
    trackC = alg.addTrack(2, 2, 3, 5);
    assertTrue(alg.tracks.contains(trackA));
    assertTrue(alg.tracks.contains(trackB));
    assertTrue(alg.tracks.contains(trackC));
    assertEquals(0, trackB.observations.size);
    assertEquals(0.0, trackB.worldLoc.distance(new Point4D_F64(2, 2, 3, 4)), UtilEjml.TEST_F64);
    assertEquals(0, trackC.observations.size);
    assertEquals(0.0, trackC.worldLoc.distance(new Point4D_F64(2, 2, 3, 5)), UtilEjml.TEST_F64);
}