final RealVector residuals) {
//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) {
//only compute the upper triangular part
for (int l = k; l < nC; ++l) {
normal.setEntry(k, l, normal.getEntry(k, l) +
jacobian.getEntry(i, k) * jacobian.getEntry(i, l));
}
}
}
//copy the upper triangular part to the lower triangular part.
for (int i = 0; i < nC; i++) {
for (int j = 0; j < i; j++) {
normal.setEntry(i, j, normal.getEntry(j, i));
}
}
return new Pair<RealMatrix, RealVector>(normal, jTr);
}