@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++];
}
}
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;
}
/**
* 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;
}