public void init(int N, boolean isFundamental) {
// define the camera's motion
worldToCamera = new Se3_F64();
worldToCamera
.getR()
.set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
worldToCamera.getT().set(0.1, -0.1, 0.01);
// randomly generate points in space
worldPts = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);
// transform points into second camera's reference frame
pairs = new ArrayList<AssociatedPair>();
currentObs = new ArrayList<Point2D_F64>();
for (Point3D_F64 p1 : worldPts) {
Point3D_F64 p2 = SePointOps_F64.transform(worldToCamera, 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);
if (isFundamental) {
GeometryMath_F64.mult(K, pair.p1, pair.p1);
GeometryMath_F64.mult(K, pair.p2, pair.p2);
}
currentObs.add(pair.p2);
}
}
/**
* Finds the values of a,b,c which minimize
*
* <p>sum (a*x(+)_i + b*y(+)_i + c - x(-)_i)^2
*
* <p>See page 306
*
* @return Affine transform
*/
private SimpleMatrix computeAffineH(
List<AssociatedPair> observations, DenseMatrix64F H, DenseMatrix64F Hzero) {
SimpleMatrix A = new SimpleMatrix(observations.size(), 3);
SimpleMatrix b = new SimpleMatrix(A.numRows(), 1);
Point2D_F64 c = new Point2D_F64();
Point2D_F64 k = new Point2D_F64();
for (int i = 0; i < observations.size(); i++) {
AssociatedPair a = observations.get(i);
GeometryMath_F64.mult(Hzero, a.p1, k);
GeometryMath_F64.mult(H, a.p2, c);
A.setRow(i, 0, k.x, k.y, 1);
b.set(i, 0, c.x);
}
SimpleMatrix x = A.solve(b);
SimpleMatrix Ha = SimpleMatrix.identity(3);
Ha.setRow(0, 0, x.getMatrix().data);
return Ha;
}
/** Give it a grid and see if it computed a legitimate homography */
@Test
public void basicTest() {
// create a grid an apply an arbitrary transform to it
PlanarCalibrationTarget config = GenericCalibrationGrid.createStandardConfig();
DenseMatrix64F R = RotationMatrixGenerator.eulerXYZ(0.02, -0.05, 0.01, null);
Vector3D_F64 T = new Vector3D_F64(0, 0, -1000);
Se3_F64 motion = new Se3_F64(R, T);
List<Point2D_F64> observations = GenericCalibrationGrid.observations(motion, config);
// compute the homography
Zhang99ComputeTargetHomography alg = new Zhang99ComputeTargetHomography(config);
assertTrue(alg.computeHomography(observations));
DenseMatrix64F H = alg.getHomography();
// test this homography property: x2 = H*x1
List<Point2D_F64> gridPoints = config.points;
for (int i = 0; i < observations.size(); i++) {
Point2D_F64 a = GeometryMath_F64.mult(H, gridPoints.get(i), new Point2D_F64());
double diff = a.distance(observations.get(i));
assertEquals(0, diff, 1e-8);
}
}
@Override
public int computeResiduals(AssociatedPair p, double[] residuals, int index) {
GeometryMath_F64.mult(H, p.p1, temp);
double top1 = error1(p.p1.x, p.p1.y, p.p2.x, p.p2.y);
double top2 = error2(p.p1.x, p.p1.y, p.p2.x, p.p2.y);
computeJacobian(p.p1, p.p2);
// JJ = J*J'
CommonOps.multTransB(J, J, JJ);
// solve JJ'*x = -e
e.data[0] = -top1;
e.data[1] = -top2;
if (solver.setA(JJ)) {
solver.solve(e, x);
// -J'(J*J')^-1*e
CommonOps.multTransA(J, x, error);
residuals[index++] = error.data[0];
residuals[index++] = error.data[1];
residuals[index++] = error.data[2];
residuals[index++] = error.data[3];
} else {
residuals[index++] = 0;
residuals[index++] = 0;
residuals[index++] = 0;
residuals[index++] = 0;
}
return index;
}
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;
}
@Override
public double computeResidual(AssociatedPair observation) {
double bottom = 0;
GeometryMath_F64.mult(F, observation.keyLoc, temp);
bottom += temp.x * temp.x + temp.y * temp.y;
GeometryMath_F64.multTran(F, observation.currLoc, temp);
bottom += temp.x * temp.x + temp.y * temp.y;
bottom = Math.sqrt(bottom);
if (bottom <= UtilEjml.EPS) {
return Double.MAX_VALUE;
} else {
GeometryMath_F64.multTran(F, observation.currLoc, temp);
return (temp.x * observation.keyLoc.x + temp.y * observation.keyLoc.y + temp.z) / bottom;
}
}
@Override
public boolean generate(List<PointVectorNN> dataSet, Cylinder3D_F64 output) {
PointVectorNN pa = dataSet.get(0);
PointVectorNN pb = dataSet.get(1);
PointVectorNN pc = dataSet.get(2);
lineA.p = pa.p;
lineA.slope = pa.normal;
lineB.p = pb.p;
lineB.slope = pb.normal;
// With perfect data, the closest point between the two lines defined by a point and its normal
// will lie on the axis of the cylinder
ClosestPoint3D_F64.closestPoint(lineA, lineB, output.line.p);
// The cylinder's axis will be along the cross product of the two normal vectors
GeometryMath_F64.cross(pa.normal, pb.normal, output.line.slope);
// take the normal of the slope so that it can detect if it has a value of zero, which happens
// if the
// two input vectors are the same
double n = output.line.slope.norm();
// n should actually always be one since the point normals are normalized to one.
if (n < 1e-8) return false;
output.line.slope.x /= n;
output.line.slope.y /= n;
output.line.slope.z /= n;
// set the radius to be the average distance point is from the cylinder's axis
double ra = Distance3D_F64.distance(output.line, pa.p);
double rb = Distance3D_F64.distance(output.line, pb.p);
double rc = Distance3D_F64.distance(output.line, pc.p);
output.radius = (ra + rb) / 2.0;
// sanity check using the model parameters
if (!check.valid(output)) return false;
// sanity check the model using points
return checkModel(output, pc, ra, rb, rc);
}