/**
* Extracts the epipoles from the trifocal tensor. Extracted epipoles will have a norm of 1 as an
* artifact of using SVD.
*
* @param tensor Input: Trifocal tensor. Not Modified
* @param e2 Output: Epipole in image 2. Homogeneous coordinates. Modified
* @param e3 Output: Epipole in image 3. Homogeneous coordinates. Modified
*/
public void process(TrifocalTensor tensor, Point3D_F64 e2, Point3D_F64 e3) {
svd.decompose(tensor.T1);
SingularOps.nullVector(svd, true, v1);
SingularOps.nullVector(svd, false, u1);
svd.decompose(tensor.T2);
SingularOps.nullVector(svd, true, v2);
SingularOps.nullVector(svd, false, u2);
svd.decompose(tensor.T3);
SingularOps.nullVector(svd, true, v3);
SingularOps.nullVector(svd, false, u3);
for (int i = 0; i < 3; i++) {
U.set(i, 0, u1.get(i));
U.set(i, 1, u2.get(i));
U.set(i, 2, u3.get(i));
V.set(i, 0, v1.get(i));
V.set(i, 1, v2.get(i));
V.set(i, 2, v3.get(i));
}
svd.decompose(U);
SingularOps.nullVector(svd, false, tempE);
e2.set(tempE.get(0), tempE.get(1), tempE.get(2));
svd.decompose(V);
SingularOps.nullVector(svd, false, tempE);
e3.set(tempE.get(0), tempE.get(1), tempE.get(2));
}
public static Point3D_F64 createPt(Sphere3D_F64 sphere, double phi, double theta) {
Point3D_F64 p = new Point3D_F64();
p.set(0, 0, sphere.radius);
Rodrigues_F64 rodX = new Rodrigues_F64(phi, new Vector3D_F64(1, 0, 0));
DenseMatrix64F rotX = ConvertRotation3D_F64.rodriguesToMatrix(rodX, null);
Rodrigues_F64 rodZ = new Rodrigues_F64(theta, new Vector3D_F64(0, 0, 1));
DenseMatrix64F rotZ = ConvertRotation3D_F64.rodriguesToMatrix(rodZ, null);
GeometryMath_F64.mult(rotX, p, p);
GeometryMath_F64.mult(rotZ, p, p);
p.x += sphere.center.x;
p.y += sphere.center.y;
p.z += sphere.center.z;
return p;
}
private void checkPlaneToWorld(PlaneNormal3D_F64 planeN) {
planeN.getN().normalize();
PlaneGeneral3D_F64 planeG = UtilPlane3D_F64.convert(planeN, null);
List<Point2D_F64> points2D = UtilPoint2D_F64.random(-2, 2, 100, rand);
Se3_F64 planeToWorld = UtilPlane3D_F64.planeToWorld(planeG, null);
Point3D_F64 p3 = new Point3D_F64();
Point3D_F64 l3 = new Point3D_F64();
Point3D_F64 k3 = new Point3D_F64();
for (Point2D_F64 p : points2D) {
p3.set(p.x, p.y, 0);
SePointOps_F64.transform(planeToWorld, p3, l3);
// see if it created a valid transform
SePointOps_F64.transformReverse(planeToWorld, l3, k3);
assertEquals(0, k3.distance(p3), GrlConstants.DOUBLE_TEST_TOL);
assertEquals(0, UtilPlane3D_F64.evaluate(planeG, l3), GrlConstants.DOUBLE_TEST_TOL);
}
}
