1) fixed monster bug in costructLinearSystemQT. The jacobian was computerd based on a wrong parameter
2) added incremental formulation in the calculation of the linear update. Much more stable
3) tested the aligment when the initial guess is not at the origin
4) added small program that does the matching on a local map