Divided calc_error into calc_residuals and calc_norm
Moved those funtions in cost_functions.h
Reusing them in ReprojectionErrorWithQuaternions*
triangulation 3D points