Examples with Se3_F64 georegression.struct.se.Se3_F64 used on opensource projects

Example 81 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class VisualizeDepthVisualOdometryApp method updateGUI.

protected void updateGUI() {
    if (!noFault) {
        numFaults++;
        return;
    }
    showTracks = statusPanel.isShowAll();
    showInliers = statusPanel.isShowInliers();
    if (renderedDepth == null) {
        renderedDepth = new BufferedImage(imageDepth.width, imageDepth.height, BufferedImage.TYPE_INT_RGB);
    }
    switch(algType) {
        case FEATURE:
            drawFeatures((AccessPointTracks3D) alg, bufferedRGB);
            break;
        case DIRECT:
            fractionInBounds = ((PyramidDirectColorDepth_to_DepthVisualOdometry) alg).getFractionInBounds();
            break;
    }
    final Se3_F64 leftToWorld = ((Se3_F64) alg.getCameraToWorld()).copy();
    // TODO magic value from kinect.  Add to config file?
    VisualizeImageData.disparity(imageDepth, renderedDepth, 0, 10000, 0);
    SwingUtilities.invokeLater(new Runnable() {

        public void run() {
            guiLeft.setImage(bufferedRGB);
            guiDepth.setImage(renderedDepth);
            guiLeft.autoSetPreferredSize();
            guiDepth.autoSetPreferredSize();
            guiLeft.repaint();
            guiDepth.repaint();
            statusPanel.setCameraToWorld(leftToWorld);
            statusPanel.setNumFaults(numFaults);
            statusPanel.setFps(fps);
            statusPanel.setFrameNumber(frameNumber - 1);
            statusPanel.setPaused(streamPaused);
            switch(algType) {
                case FEATURE:
                    {
                        featurePanel.setNumTracks(numTracks);
                        featurePanel.setNumInliers(numInliers);
                    }
                    break;
                case DIRECT:
                    {
                        directPanel.setInBounds(fractionInBounds);
                    }
                    break;
            }
        }
    });
    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();
}

Example 82 with Se3_F64

use of georegression.struct.se.Se3_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 83 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class ExampleOverheadView method main.

public static void main(String[] args) {
    BufferedImage input = UtilImageIO.loadImage(UtilIO.pathExample("road/left01.png"));
    Planar<GrayU8> imageRGB = ConvertBufferedImage.convertFromPlanar(input, null, true, GrayU8.class);
    StereoParameters stereoParam = CalibrationIO.load(UtilIO.pathExample("road/stereo01.yaml"));
    Se3_F64 groundToLeft = CalibrationIO.load(UtilIO.pathExample("road/ground_to_left_01.yaml"));
    CreateSyntheticOverheadView<Planar<GrayU8>> generateOverhead = new CreateSyntheticOverheadViewPL<>(InterpolationType.BILINEAR, 3, GrayU8.class);
    // size of cells in the overhead image in world units
    double cellSize = 0.05;
    // You can use this to automatically select reasonable values for the overhead image
    SelectOverheadParameters selectMapSize = new SelectOverheadParameters(cellSize, 20, 0.5);
    selectMapSize.process(stereoParam.left, groundToLeft);
    int overheadWidth = selectMapSize.getOverheadWidth();
    int overheadHeight = selectMapSize.getOverheadHeight();
    Planar<GrayU8> overheadRGB = new Planar<>(GrayU8.class, overheadWidth, overheadHeight, 3);
    generateOverhead.configure(stereoParam.left, groundToLeft, selectMapSize.getCenterX(), selectMapSize.getCenterY(), cellSize, overheadRGB.width, overheadRGB.height);
    generateOverhead.process(imageRGB, overheadRGB);
    // note that the left/right values are swapped in the overhead image.  This is an artifact of the plane's
    // 2D coordinate system having +y pointing up, while images have +y pointing down.
    BufferedImage output = ConvertBufferedImage.convertTo(overheadRGB, null, true);
    ShowImages.showWindow(input, "Input Image", true);
    ShowImages.showWindow(output, "Overhead Image", true);
}

Example 84 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class Se3FromEssentialGenerator method generate.

/**
 * Computes the camera motion from the set of observations.   The motion is from the first
 * into the second camera frame.
 *
 * @param dataSet Associated pairs in normalized camera coordinates.
 * @param model The best pose according to the positive depth constraint.
 */
@Override
public boolean generate(List<AssociatedPair> dataSet, Se3_F64 model) {
    if (!computeEssential.process(dataSet, E))
        return false;
    // extract the possible motions
    decomposeE.decompose(E);
    List<Se3_F64> candidates = decomposeE.getSolutions();
    // use positive depth constraint to select the best one
    Se3_F64 bestModel = null;
    int bestCount = -1;
    for (int i = 0; i < candidates.size(); i++) {
        Se3_F64 s = candidates.get(i);
        int count = 0;
        for (AssociatedPair p : dataSet) {
            if (depthCheck.checkConstraint(p.p1, p.p2, s)) {
                count++;
            }
        }
        if (count > bestCount) {
            bestCount = count;
            bestModel = s;
        }
    }
    model.set(bestModel);
    return true;
}

Example 85 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class ResidualsTriangulateSimple method process.

@Override
public void process(double[] input, double[] output) {
    point.x = input[0];
    point.y = input[1];
    point.z = input[2];
    for (int i = 0; i < observations.size(); i++) {
        Point2D_F64 p = observations.get(i);
        Se3_F64 m = motionGtoC.get(i);
        SePointOps_F64.transform(m, point, transformed);
        double dx = p.x - transformed.x / transformed.z;
        double dy = p.y - transformed.y / transformed.z;
        output[i] = dx * dx + dy * dy;
    }
}