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

Example 86 with Se3_F64

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

the class FactoryMultiViewRobust method epipolarRansac.

private static Ransac<Se3_F64, AssociatedPair> epipolarRansac(Estimate1ofEpipolar epipolar, CameraPinholeRadial intrinsic, ConfigRansac ransac) {
    TriangulateTwoViewsCalibrated triangulate = FactoryMultiView.triangulateTwoGeometric();
    ModelManager<Se3_F64> manager = new ModelManagerSe3_F64();
    ModelGenerator<Se3_F64, AssociatedPair> generateEpipolarMotion = new Se3FromEssentialGenerator(epipolar, triangulate);
    DistanceFromModel<Se3_F64, AssociatedPair> distanceSe3 = new DistanceSe3SymmetricSq(triangulate, intrinsic.fx, intrinsic.fy, intrinsic.skew, intrinsic.fx, intrinsic.fy, intrinsic.skew);
    double ransacTOL = ransac.inlierThreshold * ransac.inlierThreshold * 2.0;
    return new Ransac<>(ransac.randSeed, manager, generateEpipolarMotion, distanceSe3, ransac.maxIterations, ransacTOL);
}

Example 87 with Se3_F64

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

the class FactoryMultiView method computePnP_1.

/**
 * Created an estimator for the P3P problem that selects a single solution by considering additional
 * observations.
 *
 * <p>NOTE: Observations are in normalized image coordinates NOT pixels.</p>
 *
 * <p>
 * NOTE: EPnP has several tuning parameters and the defaults here might not be the best for your situation.
 * Use {@link #computePnPwithEPnP} if you wish to have access to all parameters.
 * </p>
 *
 * @param which The algorithm which is to be returned.
 * @param numIterations Number of iterations. Only used by some algorithms and recommended number varies
 *                      significantly by algorithm.
 * @param numTest How many additional sample points are used to remove ambiguity in the solutions.  Not used
 *                if only a single solution is found.
 * @return An estimator which returns a single estimate.
 */
public static Estimate1ofPnP computePnP_1(EnumPNP which, int numIterations, int numTest) {
    if (which == EnumPNP.EPNP) {
        PnPLepetitEPnP alg = new PnPLepetitEPnP(0.1);
        alg.setNumIterations(numIterations);
        return new WrapPnPLepetitEPnP(alg);
    }
    FastQueue<Se3_F64> solutions = new FastQueue<>(4, Se3_F64.class, true);
    return new EstimateNto1ofPnP(computePnP_N(which, -1), solutions, numTest);
}

Example 88 with Se3_F64

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

the class TestAssociateStereo2D method setup.

@Before
public void setup() {
    leftToRight = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.01, -0.001, 0.005, leftToRight.getR());
    leftToRight.getT().set(-0.1, 0, 0);
    param = new StereoParameters();
    param.rightToLeft = leftToRight.invert(null);
    param.left = new CameraPinholeRadial(400, 500, 0.1, 160, 120, 320, 240).fsetRadial(0, 0);
    param.right = new CameraPinholeRadial(380, 505, 0.05, 165, 115, 320, 240).fsetRadial(0, 0);
    descLeft = new FastQueue<TupleDesc_F64>(TupleDesc_F64.class, true) {

        @Override
        protected TupleDesc_F64 createInstance() {
            return new TupleDesc_F64(10);
        }
    };
    descRight = new FastQueue<TupleDesc_F64>(TupleDesc_F64.class, true) {

        @Override
        protected TupleDesc_F64 createInstance() {
            return new TupleDesc_F64(10);
        }
    };
    pointsLeft.reset();
    pointsRight.reset();
}

Example 89 with Se3_F64

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

the class TestStereoConsistencyCheck method checkRectification.

@Test
public void checkRectification() {
    Se3_F64 leftToRight = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.01, -0.001, 0.005, leftToRight.getR());
    leftToRight.getT().set(-0.1, 0, 0);
    StereoParameters param = new StereoParameters();
    param.rightToLeft = leftToRight.invert(null);
    param.left = new CameraPinholeRadial(400, 500, 0.1, 160, 120, 320, 240).fsetRadial(0, 0);
    param.right = new CameraPinholeRadial(380, 505, 0.05, 165, 115, 320, 240).fsetRadial(0, 0);
    Point3D_F64 X = new Point3D_F64(0.02, -0.5, 3);
    Point2D_F64 leftP = new Point2D_F64();
    Point2D_F64 rightP = new Point2D_F64();
    SfmTestHelper.renderPointPixel(param, X, leftP, rightP);
    StereoConsistencyCheck alg = new StereoConsistencyCheck(1, 2);
    alg.setCalibration(param);
    alg.checkPixel(leftP, rightP);
    assertEquals(alg.rectLeft.y, alg.rectRight.y, 1e-5);
    assertTrue(alg.rectLeft.x > alg.rectRight.x);
}

Example 90 with Se3_F64

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

the class CommonStereoMotionNPoint method generateScene.

protected void generateScene(int N, Se3_F64 worldToLeft, boolean planar) {
    if (worldToLeft == null) {
        this.worldToLeft = worldToLeft = new Se3_F64();
        ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, 1, -0.2, worldToLeft.getR());
        worldToLeft.getT().set(-0.3, 0.4, 1);
    } else {
        this.worldToLeft = worldToLeft;
    }
    // randomly generate points in space
    if (planar) {
        worldPts = createRandomPlane(rand, 3, N);
    } else {
        worldPts = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);
    }
    cameraLeftPts = new ArrayList<>();
    cameraRightPts = new ArrayList<>();
    // transform points into second camera's reference frame
    pointPose = new ArrayList<>();
    for (Point3D_F64 p1 : worldPts) {
        Point3D_F64 leftPt = SePointOps_F64.transform(worldToLeft, p1, null);
        Point3D_F64 rightPt = SePointOps_F64.transform(leftToRight, leftPt, null);
        Point2D_F64 leftObs = new Point2D_F64(leftPt.x / leftPt.z, leftPt.y / leftPt.z);
        Point2D_F64 rightObs = new Point2D_F64(rightPt.x / rightPt.z, rightPt.y / rightPt.z);
        pointPose.add(new Stereo2D3D(leftObs, rightObs, p1));
        cameraLeftPts.add(leftPt);
        cameraRightPts.add(rightPt);
    }
}