シリーズ「ハンドアイキャリブレーション入門」の第3章の補足です。第3章では、コーナー位置誤差（画像から検出したコーナーの位置のずれ）・姿勢の数・回転の大きさを変えて、ハンドアイキャリブレーションの精度を調べました。そのなかで、OpenCV の5つの解法のうち2つに、ほかの解法とは違う振る舞いが見つかりました。
- Andreff：並進誤差だけが大きく、姿勢を増やしても減らない
- Daniilidis：ほとんどの試行は問題ないが、まれに大きく外れる
この章では、それぞれの原因を突き止め、対策を示します。どちらも OpenCV でハンドアイキャリブレーションを使う人が、知らずに踏みうる落とし穴です。
シリーズの構成
第3章の実験で見つかった内容を掘り下げるため、「第3章 補足」として追加した記事です。
| 章 | 内容 | ロボット | カメラ |
|---|---|---|---|
| 第1章 | ハンドアイキャリブレーションの考え方 | ― | ― |
| 第2章 | シミュレーションで AX = XB を解き、正しく解けることを確認 | シミュレーション | シミュレーション |
| 第3章 | 精度は何で決まるか：計測誤差・姿勢の数・回転の大きさ | シミュレーション | シミュレーション |
| 第3章 補足（今回・追加） | OpenCV の5つの解法の注意点：Andreff の偏りと Daniilidis の 180° 問題 | シミュレーション | シミュレーション |
| 第4章 | RealSense の内部パラメータは信用できるか | ― | 実測 |
| 第5章 | ボードの位置姿勢の計測はどれくらいばらつくか | ― | 実測 |
| 第6章 | 実測したばらつきを使って、実機での精度を予測する | シミュレーション | 実測した誤差 |
| 番外編（予定） | 市販の安価なロボットアームで実機検証 | 実機 | 実測 |
おさらい：5つの解法
OpenCV の calibrateHandEye には、AX = XB を解く方法が5つあり、引数 method で選びます。名前は、それぞれの方法を提案した論文の著者名です。
| 解法 | 解き方 |
|---|---|
| Tsai | 回転を先に解き、次に並進を解く |
| Park | 回転を先に解き、次に並進を解く |
| Horaud | 回転を先に解き、次に並進を解く |
| Andreff | 回転と並進を同時に解く |
| Daniilidis | 回転と並進を同時に解く |
第3章で見つかった2つの異常
図1は、第3章の実験2（姿勢の数を変えた実験）の結果です。条件は、コーナー位置誤差 0.1px、傾き最大 35°、ロール ±90°で、各条件 200 回の中央値を示しています。
異常1：Andreff の並進誤差が下がらない ほかの解法は、姿勢を増やすと並進誤差が下がり、30姿勢で約 0.26〜0.29mm になります。Andreff だけは、15姿勢を超えても約 0.7mm のままでした。さらに第3章の実験1では、コーナー位置誤差を 0.1px から 0.3px に増やすと、Andreff の並進誤差は 0.66mm から 5.04mm へと約8倍に悪化しました（ほかの解法は約3倍）。
異常2：Daniilidis がまれに大きく外れる Daniilidis の中央値はほかの解法とほぼ同じですが、20姿勢以上のときに、200 回中2回だけ大きく外れました。外れたときの誤差は、並進 3.6〜19.8mm、回転 0.4〜2.3° で、同じデータを使った Park は 0.2〜0.3mm でした。
調べ方
原因を調べるために、次の2つを行いました。
- OpenCV のソースコードを読む：
calibrateHandEyeの中身（calibration_handeye.cpp）で、各解法がどう計算しているかを確認しました。 - OpenCV の計算を Python で再現する：同じ計算を自分で書き、途中の値を取り出せるようにしました。OpenCV の結果との差は 3×10<sup>−12</sup> 以下で、計算を正しく再現できています。
誤差は、次の2つに分けて調べています。
- 偏り：試行を何度繰り返しても、毎回同じ方向に残るずれ。200 回分の誤差を、向きも含めて平均したものの大きさで測ります。姿勢を増やしても減りません。
- ばらつき：試行ごとにランダムに変わるずれ。姿勢を増やすと減ります。
注意点1：OpenCV の Andreff は、並進に偏りが出る
原因：並進を解き直していない
OpenCV の Andreff は、回転（回転行列の9個の数）と並進（3個の数）をまとめて1つの連立一次方程式とし、最小二乗法で解きます。
最小二乗法で解いた9個の数は、正しい回転行列の形になっていません。そこで OpenCV は、後から最も近い回転行列に直します。ところが並進は、直す前の回転と組で求めたものをそのまま使っています。
なぜずれるのか：回転が縮み、並進が埋め合わせる
この連立方程式は、係数そのもの（カメラが測ったボードの回転や位置から作る値）に誤差を含んでいます。係数に誤差がある方程式を最小二乗法で解くと、解が小さめに偏る性質があります。
実際に、直す前の回転行列の大きさ（特異値）は、正しい回転なら 1 のところ、コーナー位置誤差 0.1px で 0.9987、0.3px で 0.988 に縮んでいました。回転と並進を同時に解いているので、回転が縮んだ分のつじつまを並進がずれて埋め合わせます。回転は後から直されますが、並進のずれは残ります。
図2(a) は、係数に誤差があると最小二乗法の答えが小さめに偏ることを、1次元の簡単な例で示したものです。図2(b) は、それによって回転が縮み、足りない分を並進がずれて埋め合わせる様子です（縮み方は分かりやすく誇張しています）。
このずれの大きさは、次の式でおおよそ見積もれます。
偏り ≈ (1 − 特異値) × カメラからボードまでの距離
0.1px のときは、0.0013 × 約 500mm ≈ 0.65mm です。シミュレーションで測った偏り（約 0.64mm）と一致しました。また、コーナー位置誤差を3倍（0.1→0.3px）にすると、偏りは約9倍（0.64→5.65mm）になり、偏りがコーナー位置誤差の2乗に比例することも確かめられました。第3章の実験1で、Andreff だけ曲線的に悪化していたのはこのためです。
| 条件 | 解法 | 並進誤差の中央値 | 偏り | ばらつき |
|---|---|---|---|---|
| 0.1px・15姿勢 | Park | 0.37 mm | 0.03 mm | 0.47 mm |
| 0.1px・15姿勢 | Andreff | 0.66 mm | 0.64 mm | 0.72 mm |
| 0.1px・30姿勢 | Park | 0.26 mm | 0.03 mm | 0.32 mm |
| 0.1px・30姿勢 | Andreff | 0.71 mm | 0.68 mm | 0.49 mm |
| 0.3px・15姿勢 | Park | 1.10 mm | 0.10 mm | 1.39 mm |
| 0.3px・15姿勢 | Andreff | 5.04 mm | 5.65 mm | 3.47 mm |
（各条件 200 回。ばらつきは、誤差が平均のまわりにどれだけ散らばっているかの大きさ）
Andreff の偏りは、15姿勢でも30姿勢でも約 0.65mm で変わりません。姿勢を増やしても誤差が下がらなかったのは、このためです。
対策：直した回転で、並進だけを解き直す
Andreff の回転の精度は、もともとほかの解法と同じです。そこで、OpenCV が返した回転を固定し、並進だけを最小二乗法で解き直します。
import numpy as np
def refit_translation(R_cam2gripper, gripper2base_list, target2cam_list):
"""回転を固定して、並進だけを最小二乗法で解き直す（4×4 の同次変換行列のリストを受け取る）"""
M, y = [], []
n = len(gripper2base_list)
for i in range(n):
for j in range(i + 1, n):
Hg = np.linalg.inv(gripper2base_list[j]) @ gripper2base_list[i]
Hc = target2cam_list[j] @ np.linalg.inv(target2cam_list[i])
M.append(np.eye(3) - Hg[:3, :3])
y.append(Hg[:3, 3] - R_cam2gripper @ Hc[:3, 3])
return np.linalg.lstsq(np.vstack(M), np.concatenate(y), rcond=None)[0]calibrateHandEye（method=cv2.CALIB_HAND_EYE_ANDREFF）が返した回転行列を R_cam2gripper に渡すと、解き直した並進が返ります。OpenCV と同じく、すべての姿勢の組 (i, j) を使っています。
| 条件 | Park | Andreff | Andreff（並進を解き直し） |
|---|---|---|---|
| 0.1px・15姿勢 | 0.37 mm | 0.66 mm | 0.37 mm |
| 0.1px・30姿勢 | 0.26 mm | 0.71 mm | 0.26 mm |
| 0.3px・15姿勢 | 1.10 mm | 5.04 mm | 1.11 mm |
（並進誤差の中央値。各条件 200 回）
解き直すだけで、Park と同じ精度になりました。第3章のグラフの緑の破線が、この解き直し版です。
注意点2：Daniilidis は、回転の差が 180° 近い姿勢の組があると、まれに大きく外れる
原因：クォータニオンの符号の取り違え
Daniilidis の方法は、姿勢と姿勢の間の回転をクォータニオンで表します。クォータニオンは回転を4つの数で表す方法で、1つ目の数（スカラー部）は cos(回転角/2)、残り3つは回転軸の向きから決まります。同じ回転を、4つの数すべての符号を反転した2通りで書けるのが特徴です。
Daniilidis の方法では、ロボット側とカメラ側のクォータニオンを、同じ符号どうしで組み合わせる必要があります。OpenCV は、どちらもスカラー部が正になるように符号をそろえています。
ところが回転角が 180° に近いと、スカラー部 cos(回転角/2) はほぼ 0 になります。すると、カメラ側だけがわずかな計測誤差で符号が反転し、ロボット側と逆の符号で組まれてしまうことがあります（以下、符号の食い違い）。逆の符号で組まれた式が1つでも混ざると、解が大きく外れます。
図3は、回転角とスカラー部 w の関係です。回転角が小さいうちは w が 1 に近く、符号ははっきり決まります。180° に近づくと w はほぼ 0 になり、わずかな誤差で符号が変わってしまいます。
確かめ方
正しく組めていれば、ロボット側のクォータニオンと、カメラ側のクォータニオンを正解の X で回したものが一致します。この2つの内積（4つの数どうしを掛けて足したもの）が負なら、符号が食い違っていると判定できます。シミュレーションなので正解の X が分かっており、この判定ができます。
結果
外れた試行と、符号の食い違いの関係（コーナー位置誤差 0.1px、ロール ±90°、各 200 回）
| 姿勢の数 | 大きく外れた回数（並進 2mm 超） | 符号の食い違いがあった回数 | 外れた試行のうち、食い違いがあったもの |
|---|---|---|---|
| 15 | 0 | 0 | ― |
| 20 | 2 | 2 | 2 / 2 |
| 30 | 2 | 2 | 2 / 2 |
- 大きく外れた試行では、すべて、回転の差がほぼ 180° の姿勢の組で符号が食い違っていました。食い違いがなかった試行で外れたものはありません。
- 20姿勢と30姿勢で外れたのは、どちらも同じ2つの試行でした。同じ試行番号では最初の20姿勢が共通なので、実質的には 200 回中2回（1%）です。
- 15姿勢以下で起きなかったのは、姿勢が少ないと、回転の差が 180° に近い組ができにくいためと考えられます（30姿勢では、組の数は 435 通りあります）。
ロールの範囲を狭めたとき（30姿勢、0.1px、各 200 回）
| ロールの範囲 | 大きく外れた回数 | 符号の食い違いがあった回数 |
|---|---|---|
| ±90° | 2 | 2 |
| ±60° | 0 | 0 |
| ±30° | 0 | 0 |
ロールが ±90° だと、2つの姿勢のロールの差は最大 180° になります。ロールを ±60° 以下にすると、回転の差が 180° に届かなくなり、外れはなくなりました。
対策
- 2つの姿勢の回転の差がほぼ 180° になる組（例：光軸まわりにカメラを 180° 回した2つの姿勢）を作らない
- そうした姿勢を避けられないときは、Park か Horaud を使う
まとめ：OpenCV で解法を選ぶときの指針
| 解法 | 回転 | 並進 | 注意点 |
|---|---|---|---|
| Park・Horaud | 良い | 最も良い | 特になし。迷ったらこれ |
| Tsai | 良い | 1〜2割悪い | 特になし |
| Andreff | 良い | 偏りが出る | 並進は解き直して使う |
| Daniilidis | 良い | 良い | 回転の差が 180° 近い姿勢の組を避ける |
- OpenCV の Andreff は、並進を解き直していないため、並進に偏りが出る。偏りはコーナー位置誤差の2乗に比例し、姿勢を増やしても減らない。並進を解き直せば Park と同じ精度になる
- OpenCV の Daniilidis は、回転の差が 180° 近い姿勢の組があると、クォータニオンの符号を取り違えて、まれに大きく外れる
- どちらも、OpenCV が返す値だけを見ていると気づきにくい。正解の分かっているシミュレーションで確かめることの大切さが、ここでも表れた
次の第4章からは、実際の RealSense を使います。まず、シミュレーションで「理想的」と仮定してきたカメラの内部パラメータ（レンズの焦点距離、画像の中心、ゆがみなど、カメラ固有の性質を表す値）が信用できるかを確かめます。
参考
- N. Andreff, R. Horaud, and B. Espiau, “On-line hand-eye calibration,” Proc. 2nd International Conference on 3-D Digital Imaging and Modeling, 1999.
- K. Daniilidis, “Hand-eye calibration using dual quaternions,” The International Journal of Robotics Research, 1999.
- OpenCV のソースコード：modules/calib3d/src/calibration_handeye.cpp（4.x）
コメント