結果

上のコードでは、シミュレーションのデータを作って、それでパラメタを推定するものです。
対応点が誤差もなく完全に分かっているという条件が良い設定です。
実際に使うためには、マーカーを高精度に検出する画像処理部分を作る必要があります。

こんな感じの画像を数枚撮影して、白点を検出＆対応付けしたとして、
それから自動でカメラパラメタを計算する。画像はレンズの歪みのため、歪曲している。

カメラパラメタが分かれば、このようにレンズの歪みを取り除ける。
レンズの歪みはカメラの光軸中心やレンズパラメタに依存するので取り除くは大変です。