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

Example 66 with Point3D_F64

use of georegression.struct.point.Point3D_F64 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 67 with Point3D_F64

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

the class VisualizeStereoVisualOdometryApp method drawFeatures.

private void drawFeatures(AccessPointTracks3D tracker, BufferedImage image) {
    numInliers = 0;
    Graphics2D g2 = image.createGraphics();
    List<Point2D_F64> points = tracker.getAllTracks();
    if (points.size() == 0)
        return;
    double[] ranges = new double[points.size()];
    for (int i = 0; i < points.size(); i++) {
        ranges[i] = tracker.getTrackLocation(i).z;
    }
    Arrays.sort(ranges);
    double maxRange = ranges[(int) (ranges.length * 0.8)];
    for (int i = 0; i < points.size(); i++) {
        Point2D_F64 pixel = points.get(i);
        if (showTracks && tracker.isNew(i)) {
            VisualizeFeatures.drawPoint(g2, (int) pixel.x, (int) pixel.y, 3, Color.GREEN);
            continue;
        }
        if (tracker.isInlier(i)) {
            if (showInliers)
                VisualizeFeatures.drawPoint(g2, (int) pixel.x, (int) pixel.y, 7, Color.BLUE, false);
            numInliers++;
        }
        if (!showTracks)
            continue;
        Point3D_F64 p3 = tracker.getTrackLocation(i);
        double r = p3.z / maxRange;
        if (r < 0)
            r = 0;
        else if (r > 1)
            r = 1;
        int color = (255 << 16) | ((int) (255 * r) << 8);
        VisualizeFeatures.drawPoint(g2, (int) pixel.x, (int) pixel.y, 3, new Color(color));
    }
    numTracks = points.size();
// g2.setColor(Color.BLACK);
// g2.fillRect(25,15,80,45);
// g2.setColor(Color.CYAN);
// g2.drawString("Total: " + numTracks, 30, 30);
// g2.drawString("Inliers: "+numInliers,30,50);
}

Example 68 with Point3D_F64

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

the class ImplPerspectiveOps_F64 method renderPixel.

public static Point2D_F64 renderPixel(Se3_F64 worldToCamera, DMatrixRMaj K, Point3D_F64 X) {
    Point3D_F64 X_cam = new Point3D_F64();
    SePointOps_F64.transform(worldToCamera, X, X_cam);
    // see if it's behind the camera
    if (X_cam.z <= 0)
        return null;
    Point2D_F64 norm = new Point2D_F64(X_cam.x / X_cam.z, X_cam.y / X_cam.z);
    if (K == null)
        return norm;
    // convert into pixel coordinates
    return GeometryMath_F64.mult(K, norm, norm);
}

Example 69 with Point3D_F64

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

the class PnPLepetitEPnP method selectWorldControlPoints.

/**
 * Selects control points along the data's axis and the data's centroid.  If the data is determined
 * to be planar then only 3 control points are selected.
 *
 * The data's axis is determined by computing the covariance matrix then performing SVD.  The axis
 * is contained along the
 */
public void selectWorldControlPoints(List<Point3D_F64> worldPts, FastQueue<Point3D_F64> controlWorldPts) {
    UtilPoint3D_F64.mean(worldPts, meanWorldPts);
    // covariance matrix elements, summed up here for speed
    double c11 = 0, c12 = 0, c13 = 0, c22 = 0, c23 = 0, c33 = 0;
    final int N = worldPts.size();
    for (int i = 0; i < N; i++) {
        Point3D_F64 p = worldPts.get(i);
        double dx = p.x - meanWorldPts.x;
        double dy = p.y - meanWorldPts.y;
        double dz = p.z - meanWorldPts.z;
        c11 += dx * dx;
        c12 += dx * dy;
        c13 += dx * dz;
        c22 += dy * dy;
        c23 += dy * dz;
        c33 += dz * dz;
    }
    c11 /= N;
    c12 /= N;
    c13 /= N;
    c22 /= N;
    c23 /= N;
    c33 /= N;
    DMatrixRMaj covar = new DMatrixRMaj(3, 3, true, c11, c12, c13, c12, c22, c23, c13, c23, c33);
    // find the data's orientation and check to see if it is planar
    svd.decompose(covar);
    double[] singularValues = svd.getSingularValues();
    DMatrixRMaj V = svd.getV(null, false);
    SingularOps_DDRM.descendingOrder(null, false, singularValues, 3, V, false);
    // planar check
    if (singularValues[0] < singularValues[2] * 1e13) {
        numControl = 4;
    } else {
        numControl = 3;
    }
    // put control points along the data's major axises
    controlWorldPts.reset();
    for (int i = 0; i < numControl - 1; i++) {
        double m = Math.sqrt(singularValues[1]) * magicNumber;
        double vx = V.unsafe_get(0, i) * m;
        double vy = V.unsafe_get(1, i) * m;
        double vz = V.unsafe_get(2, i) * m;
        controlWorldPts.grow().set(meanWorldPts.x + vx, meanWorldPts.y + vy, meanWorldPts.z + vz);
    }
    // set a control point to be the centroid
    controlWorldPts.grow().set(meanWorldPts.x, meanWorldPts.y, meanWorldPts.z);
}

Example 70 with Point3D_F64

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

the class PnPLepetitEPnP method score.

/**
 * Score a solution based on distance between control points.  Closer the camera
 * control points are from the world control points the better the score.  This is
 * similar to how optimization score works and not the way recommended in the original
 * paper.
 */
private double score(double[] betas) {
    UtilLepetitEPnP.computeCameraControl(betas, nullPts, solutionPts, numControl);
    int index = 0;
    double score = 0;
    for (int i = 0; i < numControl; i++) {
        Point3D_F64 si = solutionPts.get(i);
        Point3D_F64 wi = controlWorldPts.get(i);
        for (int j = i + 1; j < numControl; j++, index++) {
            double ds = si.distance(solutionPts.get(j));
            double dw = wi.distance(controlWorldPts.get(j));
            score += (ds - dw) * (ds - dw);
        }
    }
    return score;
}