分からない点

レンズ歪は良く推定できるのだけど、gammaの値がおかしい。
また、観測点にノイズがのると全体的に結果がおかしくなる。

おそらく、カメラの回転行列の推定で、単純にマルカート法で更新をかけると
回転行列の直交性が崩れたまま最適化を続けているのが原因の一つだと思っています。
でも、この対策方法が分からないです。だれか教えてください。