Examples with AccessPointTracks3D boofcv.abst.sfm.AccessPointTracks3D used on opensource projects

Example 1 with AccessPointTracks3D

use of boofcv.abst.sfm.AccessPointTracks3D in project BoofCV by lessthanoptimal.

the class ExampleVisualOdometryDepth method inlierPercent.

/**
 * If the algorithm implements AccessPointTracks3D, then count the number of inlier features
 * and return a string.
 */
public static String inlierPercent(VisualOdometry alg) {
    if (!(alg instanceof AccessPointTracks3D))
        return "";
    AccessPointTracks3D access = (AccessPointTracks3D) alg;
    int count = 0;
    int N = access.getAllTracks().size();
    for (int i = 0; i < N; i++) {
        if (access.isInlier(i))
            count++;
    }
    return String.format("%%%5.3f", 100.0 * count / N);
}

Example 2 with AccessPointTracks3D

use of boofcv.abst.sfm.AccessPointTracks3D in project BoofCV by lessthanoptimal.

the class ExampleVisualOdometryMonocularPlane method inlierPercent.

/**
 * If the algorithm implements AccessPointTracks3D, then count the number of inlier features
 * and return a string.
 */
public static String inlierPercent(VisualOdometry<?> alg) {
    if (!(alg instanceof AccessPointTracks3D))
        return "";
    AccessPointTracks3D access = (AccessPointTracks3D) alg;
    int count = 0;
    int N = access.getAllTracks().size();
    for (int i = 0; i < N; i++) {
        if (access.isInlier(i))
            count++;
    }
    return String.format("%%%5.3f", 100.0 * count / N);
}

Example 3 with AccessPointTracks3D

use of boofcv.abst.sfm.AccessPointTracks3D in project BoofCV by lessthanoptimal.

the class ExampleVisualOdometryStereo method inlierPercent.

/**
 * If the algorithm implements AccessPointTracks3D, then count the number of inlier features
 * and return a string.
 */
public static String inlierPercent(StereoVisualOdometry alg) {
    if (!(alg instanceof AccessPointTracks3D))
        return "";
    AccessPointTracks3D access = (AccessPointTracks3D) alg;
    int count = 0;
    int N = access.getAllTracks().size();
    for (int i = 0; i < N; i++) {
        if (access.isInlier(i))
            count++;
    }
    return String.format("%%%5.3f", 100.0 * count / N);
}

Example 4 with AccessPointTracks3D

use of boofcv.abst.sfm.AccessPointTracks3D in project BoofCV by lessthanoptimal.

the class VisualizeMonocularPlaneVisualOdometryApp method updateAlgGUI.

@Override
protected void updateAlgGUI(I frame1, final BufferedImage buffImage1, final double fps) {
    if (!noFault) {
        numFaults++;
        return;
    }
    showTracks = guiInfo.isShowAll();
    showInliers = guiInfo.isShowInliers();
    if (alg instanceof AccessPointTracks3D)
        drawFeatures((AccessPointTracks3D) alg, buffImage1);
    final Se3_F64 leftToWorld = alg.getCameraToWorld().copy();
    SwingUtilities.invokeLater(new Runnable() {

        public void run() {
            guiLeft.setImage(buffImage1);
            guiLeft.autoSetPreferredSize();
            guiLeft.repaint();
            guiInfo.setCameraToWorld(leftToWorld);
            guiInfo.setNumFaults(numFaults);
            guiInfo.setNumTracks(numTracks);
            guiInfo.setNumInliers(numInliers);
            guiInfo.setFps(fps);
        }
    });
    double r = 0.15;
    Point3D_F64 p1 = new Point3D_F64(-r, -r, 0);
    Point3D_F64 p2 = new Point3D_F64(r, -r, 0);
    Point3D_F64 p3 = new Point3D_F64(r, r, 0);
    Point3D_F64 p4 = new Point3D_F64(-r, r, 0);
    SePointOps_F64.transform(leftToWorld, p1, p1);
    SePointOps_F64.transform(leftToWorld, p2, p2);
    SePointOps_F64.transform(leftToWorld, p3, p3);
    SePointOps_F64.transform(leftToWorld, p4, p4);
    guiCam3D.add(p1, p2, p3, p4);
    guiCam3D.repaint();
    gui2D.addPoint(leftToWorld.T.x, leftToWorld.T.z);
    gui2D.repaint();
    hasProcessedImage = true;
}

Example 5 with AccessPointTracks3D

use of boofcv.abst.sfm.AccessPointTracks3D in project MAVSlam by ecmnet.

the class VfhFeatureDetector method process.

@Override
public void process(MAVDepthVisualOdometry<GrayU8, GrayU16> odometry, GrayU16 depth, GrayU8 gray) {
    Point2D_F64 xy;
    Point3D_F64 p;
    AccessPointTracks3D points = (AccessPointTracks3D) odometry;
    nearestPoints.clear();
    current_pos.set(model.state.l_x, model.state.l_y, model.state.l_z, model.state.h);
    // current.set(odometry.getCameraToWorld());
    getAttitudeToState(model, current);
    boolean first = true;
    // int i = 0; {
    for (int i = 0; i < points.getAllTracks().size(); i++) {
        if (points.isInlier(i)) {
            // xy is the observation
            xy = points.getAllTracks().get(i);
            // p is the obstacle location in body-frame
            p = odometry.getTrackLocation(i);
            SePointOps_F64.transform(current, p, rel_ned);
            MSP3DUtils.toNED(rel_ned);
            if (// model.raw.di > min_altitude &&
            p.z < max_distance && p.y < min_altitude && p.y > -min_altitude) {
                if (first) {
                    debug1 = (float) (rel_ned.x);
                    debug2 = (float) current.T.z;
                    debug3 = (float) current_pos.x;
                    msg_debug_vect v = new msg_debug_vect(2, 1);
                    v.x = debug1;
                    v.y = debug2;
                    v.z = debug3;
                    control.sendMAVLinkMessage(v);
                    first = false;
                }
                map.update(rel_ned, current_pos);
                nearestPoints.add(xy);
            // break;
            }
        }
    }
}