Improve 3d reconstruction algo