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

Example 56 with Point4D_F64

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

the class VisOdomPixelDepthPnP_to_DepthVisualOdometry method getTrackWorld3D.

@Override
public boolean getTrackWorld3D(int index, Point3D_F64 world) {
    try {
        Point4D_F64 p = ((VisOdomBundleAdjustment.BTrack) active.get(index).getCookie()).worldLoc;
        world.setTo(p.x / p.w, p.y / p.w, p.z / p.w);
        return true;
    } catch (RuntimeException ignore) {
    /* not handled */
    }
    world.setTo(((Point2D3D) active.get(index).getCookie()).getLocation());
    return true;
}

Example 57 with Point4D_F64

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

the class MetricSanityChecks method checkPhysicalConstraints.

/**
 * Checks physical constraints for one inlier set in a {@link SceneWorkingGraph}. Features are triangulated
 * directly from observations. Raw counts for each type of error can be found for this function.
 *
 * @param dbSimilar Use to get feature locations in the image
 * @param scene The scene
 * @param wview The view to check
 * @param setIdx Which inlier set in the view
 * @return true if nothing went wrong or false if a very nasty error was detected
 */
public boolean checkPhysicalConstraints(LookUpSimilarImages dbSimilar, SceneWorkingGraph scene, SceneWorkingGraph.View wview, int setIdx) {
    failedTriangulate = 0;
    failedBehind = 0;
    failedImageBounds = 0;
    failedReprojection = 0;
    SceneWorkingGraph.InlierInfo inliers = wview.inliers.get(setIdx);
    int numFeatures = inliers.getInlierCount();
    badFeatures.resetResize(numFeatures, false);
    List<SceneWorkingGraph.View> listViews = new ArrayList<>();
    List<RemoveBrownPtoN_F64> listNormalize = new ArrayList<>();
    List<Se3_F64> listMotion = new ArrayList<>();
    List<DogArray<Point2D_F64>> listFeatures = new ArrayList<>();
    List<Point2D_F64> listViewPixels = new ArrayList<>();
    Se3_F64 view1_to_world = wview.world_to_view.invert(null);
    for (int i = 0; i < inliers.views.size; i++) {
        SceneWorkingGraph.View w = scene.lookupView(inliers.views.get(i).id);
        SceneWorkingGraph.Camera c = scene.getViewCamera(w);
        if (c.intrinsic.f <= 0.0) {
            if (verbose != null)
                verbose.println("Negative focal length. view='" + w.pview.id + "'");
            return false;
        }
        listViews.add(w);
        // TODO switch to known camera if available
        var normalize = new RemoveBrownPtoN_F64();
        normalize.setK(c.intrinsic.f, c.intrinsic.f, 0, 0, 0).setDistortion(c.intrinsic.k1, c.intrinsic.k2);
        listNormalize.add(normalize);
        listMotion.add(view1_to_world.concat(w.world_to_view, null));
        SceneWorkingGraph.Camera wcamera = scene.getViewCamera(w);
        var features = new DogArray<>(Point2D_F64::new);
        dbSimilar.lookupPixelFeats(w.pview.id, features);
        double cx = wcamera.prior.cx;
        double cy = wcamera.prior.cy;
        features.forEach(p -> p.setTo(p.x - cx, p.y - cy));
        listFeatures.add(features);
    }
    List<Point2D_F64> pixelNorms = BoofMiscOps.createListFilled(inliers.views.size, Point2D_F64::new);
    Point4D_F64 foundX = new Point4D_F64();
    Point4D_F64 viewX = new Point4D_F64();
    Point2D_F64 predictdPixel = new Point2D_F64();
    SceneWorkingGraph.Camera wviewCamera = scene.getViewCamera(wview);
    for (int inlierIdx = 0; inlierIdx < numFeatures; inlierIdx++) {
        listViewPixels.clear();
        for (int viewIdx = 0; viewIdx < listViews.size(); viewIdx++) {
            Point2D_F64 p = listFeatures.get(viewIdx).get(inliers.observations.get(viewIdx).get(inlierIdx));
            listViewPixels.add(p);
            listNormalize.get(viewIdx).compute(p.x, p.y, pixelNorms.get(viewIdx));
        }
        if (!triangulator.triangulate(pixelNorms, listMotion, foundX)) {
            failedTriangulate++;
            badFeatures.set(inlierIdx, true);
            continue;
        }
        boolean badObservation = false;
        for (int viewIdx = 0; viewIdx < listViews.size(); viewIdx++) {
            Se3_F64 view1_to_view = listMotion.get(viewIdx);
            SceneWorkingGraph.View w = listViews.get(viewIdx);
            SePointOps_F64.transform(view1_to_view, foundX, viewX);
            if (PerspectiveOps.isBehindCamera(viewX)) {
                badObservation = true;
                failedBehind++;
            }
            wviewCamera.intrinsic.project(viewX.x, viewX.y, viewX.z, predictdPixel);
            double reprojectionError = predictdPixel.distance2(listViewPixels.get(viewIdx));
            if (reprojectionError > maxReprojectionErrorSq) {
                badObservation = true;
                failedReprojection++;
            }
            SceneWorkingGraph.Camera wcamera = scene.getViewCamera(w);
            int width = wcamera.prior.width;
            int height = wcamera.prior.height;
            double cx = wcamera.prior.cx;
            double cy = wcamera.prior.cy;
            if (!BoofMiscOps.isInside(width, height, predictdPixel.x + cx, predictdPixel.y + cy)) {
                badObservation = true;
                failedImageBounds++;
            }
        }
        badFeatures.set(inlierIdx, badObservation);
    }
    if (verbose != null)
        verbose.printf("view.id='%s' inlierIdx=%d, errors: behind=%d bounds=%d reprojection=%d tri=%d, obs=%d\n", wview.pview.id, setIdx, failedBehind, failedImageBounds, failedReprojection, failedTriangulate, numFeatures);
    return true;
}