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

Example 31 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project MAVSlam by ecmnet.

the class StreamRealSenseTest method start.

@Override
public void start(Stage primaryStage) {
    primaryStage.setTitle("BoofCV RealSense Demo");
    FlowPane root = new FlowPane();
    root.getChildren().add(ivrgb);
    ivrgb.setOnMouseMoved(event -> {
        MouseEvent ev = event;
        mouse_x = (int) ev.getX();
        mouse_y = (int) ev.getY();
    });
    // RealSenseInfo info = new RealSenseInfo(320,240, RealSenseInfo.MODE_RGB);
    RealSenseInfo info = new RealSenseInfo(640, 480, RealSenseInfo.MODE_RGB);
    try {
        realsense = new StreamRealSenseVisDepth(0, info);
    } catch (Exception e) {
        System.out.println("REALSENSE:" + e.getMessage());
        return;
    }
    mouse_x = info.width / 2;
    mouse_y = info.height / 2;
    primaryStage.setScene(new Scene(root, info.width, info.height));
    primaryStage.show();
    PkltConfig configKlt = new PkltConfig();
    configKlt.pyramidScaling = new int[] { 1, 2, 4, 8 };
    configKlt.templateRadius = 3;
    PointTrackerTwoPass<GrayU8> tracker = FactoryPointTrackerTwoPass.klt(configKlt, new ConfigGeneralDetector(900, 2, 1), GrayU8.class, GrayS16.class);
    DepthSparse3D<GrayU16> sparseDepth = new DepthSparse3D.I<GrayU16>(1e-3);
    // declares the algorithm
    MAVDepthVisualOdometry<GrayU8, GrayU16> visualOdometry = FactoryMAVOdometry.depthDepthPnP(1.2, 120, 2, 200, 50, true, sparseDepth, tracker, GrayU8.class, GrayU16.class);
    visualOdometry.setCalibration(realsense.getIntrinsics(), new DoNothingPixelTransform_F32());
    output = new BufferedImage(info.width, info.height, BufferedImage.TYPE_3BYTE_BGR);
    wirgb = new WritableImage(info.width, info.height);
    ivrgb.setImage(wirgb);
    realsense.registerListener(new Listener() {

        int fps;

        float mouse_depth;

        float md;

        int mc;

        int mf = 0;

        int fpm;

        @Override
        public void process(Planar<GrayU8> rgb, GrayU16 depth, long timeRgb, long timeDepth) {
            if ((System.currentTimeMillis() - tms) > 250) {
                tms = System.currentTimeMillis();
                if (mf > 0)
                    fps = fpm / mf;
                if (mc > 0)
                    mouse_depth = md / mc;
                mc = 0;
                md = 0;
                mf = 0;
                fpm = 0;
            }
            mf++;
            fpm += (int) (1f / ((timeRgb - oldTimeDepth) / 1000f) + 0.5f);
            oldTimeDepth = timeRgb;
            if (!visualOdometry.process(rgb.getBand(0), depth)) {
                bus1.writeObject(position);
                System.out.println("VO Failed!");
                visualOdometry.reset();
                return;
            }
            Se3_F64 leftToWorld = visualOdometry.getCameraToWorld();
            Vector3D_F64 T = leftToWorld.getT();
            AccessPointTracks3D points = (AccessPointTracks3D) visualOdometry;
            ConvertBufferedImage.convertTo(rgb, output, false);
            Graphics c = output.getGraphics();
            int count = 0;
            float total = 0;
            int dx = 0, dy = 0;
            int dist = 999;
            int x, y;
            int index = -1;
            for (int i = 0; i < points.getAllTracks().size(); i++) {
                if (points.isInlier(i)) {
                    c.setColor(Color.BLUE);
                    x = (int) points.getAllTracks().get(i).x;
                    y = (int) points.getAllTracks().get(i).y;
                    int d = depth.get(x, y);
                    if (d > 0) {
                        int di = (int) Math.sqrt((x - mouse_x) * (x - mouse_x) + (y - mouse_y) * (y - mouse_y));
                        if (di < dist) {
                            index = i;
                            dx = x;
                            dy = y;
                            dist = di;
                        }
                        total++;
                        if (d < 500) {
                            c.setColor(Color.RED);
                            count++;
                        }
                        c.drawRect(x, y, 1, 1);
                    }
                }
            }
            if (depth != null) {
                if (index > -1)
                    System.out.println(visualOdometry.getTrackLocation(index));
                mc++;
                md = md + depth.get(dx, dy) / 1000f;
                c.setColor(Color.GREEN);
                c.drawOval(dx - 3, dy - 3, 6, 6);
            }
            c.setColor(Color.CYAN);
            c.drawString("Fps:" + fps, 10, 20);
            c.drawString(String.format("Loc: %4.2f %4.2f %4.2f", T.x, T.y, T.z), 10, info.height - 10);
            c.drawString(String.format("Depth: %3.2f", mouse_depth), info.width - 85, info.height - 10);
            position.x = T.x;
            position.y = T.y;
            position.z = T.z;
            position.tms = timeRgb;
            bus1.writeObject(position);
            if ((count / total) > 0.6f) {
                c.setColor(Color.RED);
                c.drawString("WARNING!", info.width - 70, 20);
            }
            c.dispose();
            Platform.runLater(() -> {
                SwingFXUtils.toFXImage(output, wirgb);
            });
        }
    }).start();
}

Example 32 with Vector3D_F64

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

the class PnPRodriguesCodec method encode.

@Override
public void encode(Se3_F64 input, double[] output) {
    // otherwise Rodrigues will have issues with the noise
    if (!svd.decompose(input.getR()))
        throw new RuntimeException("SVD failed");
    DMatrixRMaj U = svd.getU(null, false);
    DMatrixRMaj V = svd.getV(null, false);
    CommonOps_DDRM.multTransB(U, V, R);
    // extract Rodrigues coordinates
    ConvertRotation3D_F64.matrixToRodrigues(R, rotation);
    output[0] = rotation.unitAxisRotation.x * rotation.theta;
    output[1] = rotation.unitAxisRotation.y * rotation.theta;
    output[2] = rotation.unitAxisRotation.z * rotation.theta;
    Vector3D_F64 T = input.getT();
    output[3] = T.x;
    output[4] = T.y;
    output[5] = T.z;
}

Example 33 with Vector3D_F64

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

the class PnPRodriguesCodec method decode.

@Override
public void decode(double[] input, Se3_F64 outputModel) {
    rotation.setParamVector(input[0], input[1], input[2]);
    ConvertRotation3D_F64.rodriguesToMatrix(rotation, outputModel.getR());
    Vector3D_F64 T = outputModel.getT();
    T.x = input[3];
    T.y = input[4];
    T.z = input[5];
}

Example 34 with Vector3D_F64

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

the class PoseFromPairLinear6 method computeTransform.

/**
 * Computes the null space of A and extracts the transform.
 */
private void computeTransform(DMatrixRMaj A) {
    if (!solveNullspace.process(A, 1, x))
        throw new RuntimeException("SVD failed?");
    DMatrixRMaj R = motion.getR();
    Vector3D_F64 T = motion.getT();
    // extract the results
    System.arraycopy(x.data, 0, R.data, 0, 9);
    T.x = x.data[9];
    T.y = x.data[10];
    T.z = x.data[11];
}

Example 35 with Vector3D_F64

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

the class PixelDepthLinear method depth2View.

/**
 * Computes pixel depth in image 'a' from two observations.
 *
 * @param a Observation in first frame.  In calibrated coordinates. Not modified.
 * @param b Observation in second frame.  In calibrated coordinates. Not modified.
 * @param fromAtoB Transform from frame a to frame b.
 * @return Pixel depth in first frame. In same units as T inside of fromAtoB.
 */
public double depth2View(Point2D_F64 a, Point2D_F64 b, Se3_F64 fromAtoB) {
    DMatrixRMaj R = fromAtoB.getR();
    Vector3D_F64 T = fromAtoB.getT();
    GeometryMath_F64.multCrossA(b, R, temp0);
    GeometryMath_F64.mult(temp0, a, temp1);
    GeometryMath_F64.cross(b, T, temp2);
    return -(temp2.x + temp2.y + temp2.z) / (temp1.x + temp1.y + temp1.z);
}