@Override
public void decode(double[] param, CalibratedPoseAndPoint outputModel) {
int paramIndex = 0;
// first decode the transformation
for (int i = 0; i < numViews; i++) {
// don't decode if it is already known
if (knownView[i]) continue;
Se3_F64 se = outputModel.getWorldToCamera(i);
rotation.setParamVector(param[paramIndex++], param[paramIndex++], param[paramIndex++]);
RotationMatrixGenerator.rodriguesToMatrix(rotation, se.getR());
Vector3D_F64 T = se.getT();
T.x = param[paramIndex++];
T.y = param[paramIndex++];
T.z = param[paramIndex++];
}
// now decode the points
for (int i = 0; i < numPoints; i++) {
Point3D_F64 p = outputModel.getPoint(i);
p.x = param[paramIndex++];
p.y = param[paramIndex++];
p.z = param[paramIndex++];
}
}
/**
* 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));
}
private void back_and_forth(double mirror) {
CameraUniversalOmni model = createModel(mirror);
UniOmniPtoS_F64 pixelToUnit = new UniOmniPtoS_F64();
pixelToUnit.setModel(model);
UniOmniStoP_F64 unitToPixel = new UniOmniStoP_F64();
unitToPixel.setModel(model);
List<Point2D_F64> listPixels = new ArrayList<>();
listPixels.add(new Point2D_F64(320, 240));
listPixels.add(new Point2D_F64(320, 200));
listPixels.add(new Point2D_F64(320, 280));
listPixels.add(new Point2D_F64(280, 240));
listPixels.add(new Point2D_F64(360, 240));
listPixels.add(new Point2D_F64(280, 240));
listPixels.add(new Point2D_F64(240, 180));
for (Point2D_F64 pixel : listPixels) {
Point3D_F64 circle = new Point3D_F64(10, 10, 10);
pixelToUnit.compute(pixel.x, pixel.y, circle); // directly forward on unit sphere
// it should be on the unit circle
assertEquals(1.0, circle.norm(), GrlConstants.DOUBLE_TEST_TOL);
Point2D_F64 found = new Point2D_F64();
unitToPixel.compute(circle.x, circle.y, circle.z, found);
assertEquals(pixel.x, found.x, GrlConstants.DOUBLE_TEST_TOL_SQRT);
assertEquals(pixel.y, found.y, GrlConstants.DOUBLE_TEST_TOL_SQRT);
}
}
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);
}
}
/**
* Randomly generate points on a plane by randomly selecting two vectors on the plane using cross
* products
*/
private List<Point3D_F64> randPointOnPlane(PlaneNormal3D_F64 plane, int N) {
Vector3D_F64 v = new Vector3D_F64(-2, 0, 1);
Vector3D_F64 a = UtilTrig_F64.cross(plane.n, v);
a.normalize();
Vector3D_F64 b = UtilTrig_F64.cross(plane.n, a);
b.normalize();
List<Point3D_F64> ret = new ArrayList<Point3D_F64>();
for (int i = 0; i < N; i++) {
double v0 = rand.nextGaussian();
double v1 = rand.nextGaussian();
Point3D_F64 p = new Point3D_F64();
p.x = plane.p.x + v0 * a.x + v1 * b.x;
p.y = plane.p.y + v0 * a.y + v1 * b.y;
p.z = plane.p.z + v0 * a.z + v1 * b.z;
ret.add(p);
}
return ret;
}