Nice code. The varaible 'az_coorection' really confused me. It is computed as atan2(extrinsic_matrix[0][0], extrinsic_matrix[1][0]). Why?