//since the normal matrix is symmetric, we only need to compute half of it.
final int nR = jacobian.getRowDimension();
final int nC = jacobian.getColumnDimension();
//allocate space for return values
final RealMatrix normal = MatrixUtils.createRealMatrix(nC, nC);
final RealVector jTr = new ArrayRealVector(nC);
//for each measurement
for (int i = 0; i < nR; ++i) {
//compute JTr for measurement i
for (int j = 0; j < nC; j++) {
jTr.setEntry(j, jTr.getEntry(j) +
residuals.getEntry(i) * jacobian.getEntry(i, j));
}
// add the the contribution to the normal matrix for measurement i
for (int k = 0; k < nC; ++k) {