use of boofcv.struct.geo.AssociatedPair in project BoofCV by lessthanoptimal.
the class TestDistanceSe3SymmetricSq method testIntrinsicParameters.
/**
* Manually compute the error using a calibration matrix and see if they match
*/
@Test
public void testIntrinsicParameters() {
// intrinsic camera calibration matrix
DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 100, 0.01, 200, 0, 150, 200, 0, 0, 1);
DMatrixRMaj K2 = new DMatrixRMaj(3, 3, true, 105, 0.021, 180, 0, 155, 210, 0, 0, 1);
DMatrixRMaj K_inv = new DMatrixRMaj(3, 3);
DMatrixRMaj K2_inv = new DMatrixRMaj(3, 3);
CommonOps_DDRM.invert(K, K_inv);
CommonOps_DDRM.invert(K2, K2_inv);
Se3_F64 keyToCurr = new Se3_F64();
keyToCurr.getT().set(0.1, -0.1, 0.01);
Point3D_F64 X = new Point3D_F64(0.02, -0.05, 3);
AssociatedPair obs = new AssociatedPair();
AssociatedPair obsP = new AssociatedPair();
obs.p1.x = X.x / X.z;
obs.p1.y = X.y / X.z;
SePointOps_F64.transform(keyToCurr, X, X);
obs.p2.x = X.x / X.z;
obs.p2.y = X.y / X.z;
// translate into pixels
GeometryMath_F64.mult(K, obs.p1, obsP.p1);
GeometryMath_F64.mult(K2, obs.p2, obsP.p2);
// add some noise
obsP.p1.x += 0.25;
obsP.p1.y += 0.25;
obsP.p2.x -= 0.25;
obsP.p2.y -= 0.25;
// convert noisy into normalized coordinates
GeometryMath_F64.mult(K_inv, obsP.p1, obsP.p1);
GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
// triangulate the point's position given noisy data
LineParametric3D_F64 rayA = new LineParametric3D_F64();
LineParametric3D_F64 rayB = new LineParametric3D_F64();
rayA.slope.set(obsP.p1.x, obsP.p1.y, 1);
rayB.p.set(-0.1, 0.1, -0.01);
rayB.slope.set(obsP.p2.x, obsP.p2.y, 1);
ClosestPoint3D_F64.closestPoint(rayA, rayB, X);
// compute predicted given noisy triangulation
AssociatedPair ugh = new AssociatedPair();
ugh.p1.x = X.x / X.z;
ugh.p1.y = X.y / X.z;
SePointOps_F64.transform(keyToCurr, X, X);
ugh.p2.x = X.x / X.z;
ugh.p2.y = X.y / X.z;
// convert everything into pixels
GeometryMath_F64.mult(K, ugh.p1, ugh.p1);
GeometryMath_F64.mult(K2, ugh.p2, ugh.p2);
GeometryMath_F64.mult(K, obsP.p1, obsP.p1);
GeometryMath_F64.mult(K2, obsP.p2, obsP.p2);
double dx1 = ugh.p1.x - obsP.p1.x;
double dy1 = ugh.p1.y - obsP.p1.y;
double dx2 = ugh.p2.x - obsP.p2.x;
double dy2 = ugh.p2.y - obsP.p2.y;
double error = dx1 * dx1 + dy1 * dy1 + dx2 * dx2 + dy2 * dy2;
// convert noisy back into normalized coordinates
GeometryMath_F64.mult(K_inv, obsP.p1, obsP.p1);
GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
DistanceSe3SymmetricSq alg = new DistanceSe3SymmetricSq(triangulate);
alg.setIntrinsic(K.get(0, 0), K.get(1, 1), K.get(0, 1), K2.get(0, 0), K2.get(1, 1), K2.get(0, 1));
alg.setModel(keyToCurr);
assertEquals(error, alg.computeDistance(obsP), 1e-8);
}
use of boofcv.struct.geo.AssociatedPair in project BoofCV by lessthanoptimal.
the class TestDistanceSe3SymmetricSq method testBehindCamera.
@Test
public void testBehindCamera() {
Se3_F64 keyToCurr = new Se3_F64();
keyToCurr.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
keyToCurr.getT().set(0.1, -0.1, 0.01);
Point3D_F64 X = new Point3D_F64(0.1, -0.05, -3);
AssociatedPair obs = new AssociatedPair();
obs.p1.x = X.x / X.z;
obs.p1.y = X.y / X.z;
SePointOps_F64.transform(keyToCurr, X, X);
obs.p2.x = X.x / X.z;
obs.p2.y = X.y / X.z;
alg.setModel(keyToCurr);
assertTrue(Double.MAX_VALUE == alg.computeDistance(obs));
}
use of boofcv.struct.geo.AssociatedPair in project BoofCV by lessthanoptimal.
the class TestPositionFromPairLinear2 method compute.
@Override
public Se3_F64 compute(List<AssociatedPair> obs, List<Point3D_F64> locations) {
List<Point2D_F64> l = new ArrayList<>();
for (AssociatedPair p : obs) l.add(p.p2);
assertTrue(alg.process(motion.getR(), locations, l));
Se3_F64 found = new Se3_F64();
found.R.set(motion.R);
found.T.set(alg.getT());
return found;
}
use of boofcv.struct.geo.AssociatedPair in project BoofCV by lessthanoptimal.
the class TestRectifyFundamental method createScene.
public void createScene() {
DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 500, 0, 250, 0, 520, 270, 0, 0, 1);
// define the camera's motion
motion = new Se3_F64();
motion.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, -0.01, 0.1, 0.05, null));
motion.getT().set(-0.5, 0.1, -0.05);
DMatrixRMaj E = MultiViewOps.createEssential(motion.getR(), motion.getT());
F = MultiViewOps.createFundamental(E, K);
// randomly generate points in space
List<Point3D_F64> worldPts = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);
// transform points into second camera's reference frame
pairs = new ArrayList<>();
for (Point3D_F64 p1 : worldPts) {
Point3D_F64 p2 = SePointOps_F64.transform(motion, p1, null);
AssociatedPair pair = new AssociatedPair();
pair.p1.set(p1.x / p1.z, p1.y / p1.z);
pair.p2.set(p2.x / p2.z, p2.y / p2.z);
pairs.add(pair);
GeometryMath_F64.mult(K, pair.p1, pair.p1);
GeometryMath_F64.mult(K, pair.p2, pair.p2);
}
}
use of boofcv.struct.geo.AssociatedPair in project BoofCV by lessthanoptimal.
the class TestRectifyFundamental method alignY.
/**
* See if the transform align an observation to the same y-axis
*/
@Test
public void alignY() {
createScene();
RectifyFundamental alg = new RectifyFundamental();
alg.process(F, pairs, 500, 520);
// unrectified observations
AssociatedPair unrect = pairs.get(0);
// rectified observations
Point2D_F64 r1 = new Point2D_F64();
Point2D_F64 r2 = new Point2D_F64();
GeometryMath_F64.mult(alg.getRect1(), unrect.p1, r1);
GeometryMath_F64.mult(alg.getRect2(), unrect.p2, r2);
assertEquals(r1.y, r2.y, 1e-8);
}
Aggregations