/**
* 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;
}
/** Apply a rotation such that the epipole is equal to [f,0,1)\ */
private SimpleMatrix rotateEpipole(Point3D_F64 epipole, int x0, int y0) {
// compute rotation which will set
// x * sin(theta) + y * cos(theta) = 0
double x = epipole.x / epipole.z - x0;
double y = epipole.y / epipole.z - y0;
double theta = Math.atan2(-y, x);
double c = Math.cos(theta);
double s = Math.sin(theta);
SimpleMatrix R = new SimpleMatrix(3, 3);
R.setRow(0, 0, c, -s);
R.setRow(1, 0, s, c);
R.set(2, 2, 1);
return R;
}