(57)【要約】【課題】位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができるようにする。【解決手段】判定部１６０が、予め定めた条件を満たすと判定されるまで、データ点の座標及びモデル点の座標に基づいて、データ点群に含まれるデータ点の各々について、モデル点群に含まれるモデル点との対応付け、同一の当該モデル点に対し、データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の当該モデル点と複数のデータ点の各々との対応付けの各々に対して所定の重みを設定し、データ点群とモデル点群との対応付け、及び設定された所定の重みに基づいて、データ点群に含まれる当該データ点の座標を変換した場合に、変換後のデータ点群とモデル点群との位置ずれが最小となる座標変換行列を算出し、当該座標変換行列に基づいて、データ点群に含まれるデータ点の座標を変換することを繰り返す。【選択図】図１
2020086941.tif 000002
【特許請求の範囲】
【請求項１】
位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付ける入力部と、
位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得する取得部と、
前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行うモデル点対応部と、
同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定する重み設定部と、
前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出する変換行列算出部と、
前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換する座標変換部と、
予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる判定部と、
を含む位置ずれ補正装置。
【請求項２】
前記変換行列算出部は、対応付けられた前記データ点の変換後の座標と前記モデル点の座標との距離、及び前記データ点と前記モデル点との対応付けに対する重みを用いて求められる、重み付きの距離の二乗和が最小となるように、前記座標変換行列を算出する
請求項１記載の位置ずれ補正装置。
【請求項３】
前記重み設定部は、一つの前記モデル点に対し、一つの前記データ点のみが対応付けられた場合に、前記モデル点と前記データ点との対応付けに対して一定の重みを設定し、同一の前記モデル点に対し、複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して、前記一定の重みより低い重みを設定する
請求項１又は２記載の位置ずれ補正装置。
【請求項４】
前記モデル点の座標及び前記データ点の座標に基づいて、前記モデル点群に含まれるモデル点の各々について、前記データ点群に含まれるデータ点との対応付けを行うデータ点対応部
を更に含み、
前記重み設定部は、同一の前記データ点に対し、前記モデル点群に含まれる複数のモデル点が対応付けられた場合に、同一の前記データ点と前記複数のモデル点の各々との対応付けの各々に対して所定の重みを設定し、
前記判定部は、予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部による対応付け、前記データ点対応部による対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる
請求項１乃至３の何れか１項記載の位置ずれ補正装置。
【請求項５】
前記モデル点対応部により対応付けられた前記データ点の座標と前記モデル点の座標との距離が予め定めた第１閾値以上である場合に、対応付けを無効化する第１有効距離判定部
請求項１乃至３の何れか１項記載の位置ずれ補正装置。
【請求項６】
前記データ点対応部により対応付けられた前記モデル点の座標と前記データ点の座標との距離が予め定めた第２閾値以上である場合に、対応付けを無効化する第２有効距離判定部
請求項４記載の位置ずれ補正装置。
【請求項７】
前記重み設定部は、対応付けられた前記データ点群のデータ点と前記モデル点群のモデル点との位置ずれが予め定めた第３閾値以下となる場合、又は前記判定部による繰り返し回数が予め定めた第４閾値以上となる場合、重みの設定方法を変更する
請求項１乃至４の何れか１項記載の位置ずれ補正装置。
【請求項８】
コンピュータを、
位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付ける入力部、
位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得する取得部、
前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行うモデル点対応部、
同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定する重み設定部、
前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出する変換行列算出部、
前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換する座標変換部、及び
予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる判定部
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置ずれ補正装置及びプログラムに関する。
【背景技術】
【０００２】
従来から、ＧＮＳＳ（全球測位衛星システム、例えばＧＰＳ）等を用いた車両測位装置と車両搭載の外界センサ（カメラやＬＩＤＡＲ）により検出した白線情報等を走行データとして収集し、走行環境の地図が生成されている。
【０００３】
複数の走行データを集約して地図の精度向上や生成範囲の拡大を行う際、ＧＰＳの測位誤差などによる走行軌跡の位置ずれが生じるため、異なる走行データの相対的な位置ずれを補正する必要がある。
【０００４】
相対的な位置ずれを補正するために、３次元環境をモデル化した点群Ｑに対して、取得した３次元点群Ｐを重ね合わせることにより物体の位置・姿勢や観測者の位置・姿勢を推定する技術において、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ  Ｃｌｏｓｅｓｔ  Ｐｏｉｎｔ）法（非特許文献１）がよく用いられている（特許文献１、特許文献２）。
【０００５】
ＩＣＰ法は、（１）点群Ｐと点群Ｑに含まれる点を対応付け、（２）対応点間の距離が小さくなるような座標変換行列（回転、並進）の算出を繰り返して最適な座標変換行列を推定する手法である（図１２）。
【０００６】
ＩＣＰ法は収束性が保証されているが、誤った対応が行われると局所解に陥る場合があるため、正しい対応付けを行うことが重要である。
【０００７】
特許文献１では、点群Ｐの各点に対し、点群Ｑ内で距離が最小となる点を探索し対応付ける。この時、距離が指定の値より大きい場合に誤対応として無効とする方法がとられている。
【０００８】
また、逆方向の対応付けを行い一致した場合のみ有効とするという手法も用いられている（特許文献３、非特許文献２）。
【０００９】
一般的には点群Ｑが密に得られており、ＩＣＰ法での繰り返し処理により徐々に正しい対応付けが行われることが期待できる。このように、点群が密な場合にはＩＣＰ法での繰り返し処理により、初期の誤った点への対応が徐々に解消し、多くの場合は正しい対応による誤差の小さい位置合わせが行われる。
【先行技術文献】
【非特許文献】
【００１０】
【非特許文献１】"Iterative closest point", [online], 2017, arXiv:1706.00227, [平成30年10月5日検索], インターネット<https://en.wikipedia.org/wiki/Iterative_closest_point>．
【非特許文献２】Congcong Jin, Jihua Zhu, Yaochen Li, Shaoyi Du, Zhongyu Li, Huimin Lu, "An Effective Approach for Point Clouds Registration Based on the Hard and Soft Assignments", [online], 2017, arXiv:1706.00227, [平成30年10月5日検索], インターネット<https://arxiv.org/abs/1706.00227>．
【特許文献】
【００１１】
【特許文献１】特開２０１３−０７２８５７号公報
【特許文献２】特開２００８−２５０９０５号公報
【特許文献３】特開２０１１−２０９１１６号公報
【発明の概要】
【発明が解決しようとする課題】
【００１２】
しかし、点群にノイズや欠落がある場合、初期の誤った対応が解消せず、誤った対応による誤差の大きい位置合わせ結果に収束する可能性が高くなる、という問題があった。
【００１３】
また、点群Ｑが疎な場合には初期状態の誤対応が解消されず、局所解に陥る可能性が高くなってしまう、という問題があった（図１３）。
【００１４】
また、特許文献３の手法では、点群Ｐと点群Ｑとのずれ方、配置によっては、誤対応が残存してしまう、という問題があった。
【００１５】
本発明は、上記問題点を解決するためになされたもので、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる位置ずれ補正装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１６】
上記の目的を達成するために、本発明の位置ずれ補正装置は、位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付ける入力部と、位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得する取得部と、前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行うモデル点対応部と、同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定する重み設定部と、前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出する変換行列算出部と、前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換する座標変換部と、予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる判定部と、を含んで構成されている。
【００１７】
また、本発明のプログラムは、コンピュータを、位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付ける入力部、位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得する取得部、前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行うモデル点対応部、同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定する重み設定部、前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出する変換行列算出部、前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換する座標変換部、及び予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる判定部として機能させるためのプログラムである。
【００１８】
本発明の状態予測装置及びプログラムによれば、入力部が、位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付け、取得部が、位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得し、モデル点対応部が、前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行う。
【００１９】
重み設定部が、同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定し、変換行列算出部が、前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出し、座標変換部が、前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換し、判定部が、予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる。
【００２０】
このように、予め定めた条件を満たすと判定されるまで、データ点の座標及びモデル点の座標に基づいて、データ点群に含まれるデータ点の各々について、モデル点群に含まれるモデル点との対応付け、同一の当該モデル点に対し、データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の当該モデル点と複数のデータ点の各々との対応付けの各々に対して所定の重みを設定し、データ点群とモデル点群との対応付け、及び設定された所定の重みに基づいて、データ点群に含まれる当該データ点の座標を変換した場合に、変換後のデータ点群とモデル点群との位置ずれが最小となる座標変換行列を算出し、当該座標変換行列に基づいて、データ点群に含まれるデータ点の座標を変換することにより、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる。
【００２１】
また、本発明の位置ずれ補正装置の前記変換行列算出部は、対応付けられた前記データ点の変換後の座標と前記モデル点の座標との距離、及び前記データ点と前記モデル点との対応付けに対する重みを用いて求められる、重み付きの距離の二乗和が最小となるように、前記座標変換行列を算出することができる。
【００２２】
また、本発明の位置ずれ補正装置の前記重み設定部は、一つの前記モデル点に対し、一つの前記データ点のみが対応付けられた場合に、前記モデル点と前記データ点との対応付けに対して一定の重みを設定し、同一の前記モデル点に対し、複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して、前記一定の重みより低い重みを設定することができる。
【００２３】
また、本発明の位置ずれ補正装置は、前記モデル点の座標及び前記データ点の座標に基づいて、前記モデル点群に含まれるモデル点の各々について、前記データ点群に含まれるデータ点との対応付けるデータ点対応部を更に含み、前記重み設定部は、同一の前記データ点に対し、前記モデル点群に含まれる複数のモデル点が対応付けられた場合に、同一の前記データ点と前記複数のモデル点の各々との対応付けの各々に対して所定の重みを設定し、前記判定部は、予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記データ点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させることができる。
【００２４】
また、本発明の位置ずれ補正装置部は、前記モデル点対応部により対応付けられた前記データ点の座標と前記モデル点の座標との距離が予め定めた第１閾値以上である場合に、対応付けを無効化する第１有効距離判定部を更に含むことができる。
【００２５】
また、本発明の位置ずれ補正装置部は、前記データ点対応部により対応付けられた前記モデル点の座標と前記データ点の座標との距離が予め定めた第２閾値以上である場合に、対応付けを無効化する第２有効距離判定部を更に含むことができる。
【００２６】
また、本発明の位置ずれ補正装置の前記重み設定部は、対応付けられた前記データ点群のデータ点と前記モデル点群のモデル点との位置ずれが予め定めた第３閾値以下となる場合、又は前記判定部による繰り返し回数が予め定めた第４閾値以上となる場合、重みの設定方法を変更することができる。
【発明の効果】
【００２７】
以上説明したように、本発明の位置ずれ補正装置及びプログラムによれば、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる、という効果が得られる。
【図面の簡単な説明】
【００２８】
【図１】本発明の第１の実施の形態における概略構成を示すブロック図である。
【図２】本発明の第１の実施の形態における点群の対応付けの例を示すイメージ図である。
【図３】本発明の第１の実施の形態における位置ずれ補正処理ルーチンの内容を示すフローチャートである。
【図４】本発明の第１の実施の形態における点群の対応付け処理ルーチンの内容を示すフローチャートである。
【図５】本発明の第２の実施の形態における概略構成を示すブロック図である。
【図６】本発明の第２の実施の形態における３次元点群の位置合わせの例を示すイメージ図である。
【図７】本発明の第２の実施の形態における点群の対応付け処理ルーチンの内容を示すフローチャートである。
【図８】本発明の第３の実施の形態における概略構成を示すブロック図である。
【図９】本発明の第３の実施の形態における点群の対応付け処理ルーチンの内容を示すフローチャートである。
【図１０】本発明の第４の実施の形態における概略構成を示すブロック図である。
【図１１】本発明の第４の実施の形態における点群の対応付け処理ルーチンの内容を示すフローチャートである。
【図１２】従来の３次元点群の位置合わせの例を示すイメージ図である。
【図１３】従来の３次元点群の位置合わせによる誤対応が残存する例を示すイメージ図である。
【図１４】３次元点群の位置合わせの基本構成を示すイメージ図である。
【図１５】本発明の実施の形態における３次元点群の位置合わせの概要を示すイメージ図である。
【発明を実施するための形態】
【００２９】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００３０】
＜本発明の実施の形態の概要＞
従来の点群間の位置合わせでは、３次元のデータ点群Ｐの点列で表現される図形（図１２（ａ））をモデル点群Ｑの点列で表現される図形に重ね合わせる（図１２（ｂ））。この図形は、線図形、面、立体など任意の図形である。
【００３１】
データ点群Ｐとモデル点群Ｑとの間で、対応するデータ点ｐｉの座標とモデル点ｑｊの座標の距離が最小となるようにデータ点群Ｐを座標変換する。例えばＩＣＰ法等を用いて、点群間の対応付けを行う。なお、データ点群Ｐは、モデル点群Ｑと同一の座標系にあるものとする。
【００３２】
ＩＣＰ法の基本構成を図１４（ａ）に示す。ＩＣＰ法の基本構成で行われる最近傍点の探索・対応付けでは、誤った対応付けが行われる可能性がある。
【００３３】
また、非特許文献２のように逆方向の対応付けによる構成を図１４（ｂ）に示す。この場合でも、ＩＣＰ法の繰り返し処理の初期段階において、データ点群Ｐとモデル点群Ｑとの位置ずれが大きい場合、点群の配置によっては誤対応が発生しやすい。
【００３４】
本発明の実施の形態は、このようなノイズや欠落がある点群の位置合わせに関するものである。
【００３５】
本実施の形態では、誤対応の発生を抑制するため、データ点群Ｐとモデル点群Ｑとの対応付けがあいまいな場合、その対応付けを無効化するか、又はその対応付けに対して小さな重みを設定する（図１５（ｃ）。あいまい性の判定基準として、従来技術のようなモデル点とデータ点の距離のほかに、対応付けの重複（複数のデータ点から同一モデル点への対応など）を用いる。
【００３６】
従来のＩＣＰ法の応用では、モデル点群Ｑはノイズなどを除去した状態を仮定しているが、本実施の形態では、位置合わせの基準となるモデル点群Ｑにもノイズや欠落が含まれる場合においても、誤った対応付けによる位置合わせを抑制することができる（図１５（ｄ））。
【００３７】
＜本発明の第１の実施の形態に係る位置ずれ補正装置１０の構成＞
図１を用いて、本発明の実施の形態にかかる位置ずれ補正装置１０について説明する。
【００３８】
位置ずれ補正装置１０は、ＣＰＵと、ＲＡＭと、後述する位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【００３９】
図１に示すように、本発明の実施の形態に係る位置ずれ補正装置１０は、入力部１００と、モデル点群記憶部１１０と、取得部１２０と、モデル点対応部１３０と、重み設定部１４０と、変換行列算出部１５０と、判定部１６０と、座標変換部１７０と、出力部１８０とを備えて構成される。
【００４０】
入力部１００は、位置合わせの対象となるデータ点ｐｉの座標の集合であるデータ点群Ｐの入力を受け付ける。データ点群Ｐは、ｎ個のデータ点ｐｉ｛ｉ＝１，...，ｎ｝を含む。データ点ｐｉは３次元の点であり、データ点ｐｉの座標は（ｘｉ，ｙｉ，ｚｉ）で表される。なお、入力部１００に入力されるデータ点群Ｐは、後述のモデル点群Ｑと同一の座標系であるか、又は同一の座標系に予め変換されたものである。
【００４１】
そして、入力部１００は、データ点群Ｐを、取得部１２０に渡す。
【００４２】
モデル点群記憶部１１０には、位置合わせの基準となるモデル点ｑｊの座標の集合であるモデル点群Ｑが格納されている。モデル点群Ｑは、ｍ個のモデル点ｑｊ｛ｊ＝１，...，ｍ｝を含む。モデル点ｑｊは３次元の点であり、モデル点ｑｊの座標は（ｘｊ，ｙｊ，ｚｊ）で表される。
【００４３】
取得部１２０は、位置合わせの基準となるモデル点ｑｊの座標の集合であるモデル点群Ｑを取得する。
【００４４】
そして、取得部１２０は、データ点群Ｐ及びモデル点群Ｑを、モデル点対応部１３０に渡す。
【００４５】
モデル点対応部１３０は、データ点ｐｉの座標及びモデル点ｑｊの座標に基づいて、データ点群Ｐに含まれるデータ点ｐｉの各々について、モデル点群Ｑに含まれるモデル点ｑｊとの対応付けを行う。
【００４６】
具体的には、モデル点対応部１３０は、データ点群Ｐに含まれるデータ点ｐｉの各々について、モデル点群Ｑから最近傍点となるモデル点ｑｊを探索する（下記式（１））。
【００４７】
【数１】
2020086941.tif 000003
【００４８】
そして、モデル点対応部１３０は、データ点群Ｐに含まれるデータ点ｐｉの各々について、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）を、重み設定部１４０に渡す。
【００４９】
重み設定部１４０は、同一のモデル点ｑｊに対し、データ点群Ｐに含まれる複数のデータ点ｐｉが対応付けられた場合に、同一のモデル点ｑｊと当該複数のデータ点ｐｉの各々との対応付け（ｐｉ，ｑｊ）の各々に対して所定の重みｗｉを設定する。
【００５０】
具体的には、重み設定部１４０は、まず、データ点群Ｐに含まれるデータ点ｐｉの各々について、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）に対して、所定の重みｗｉ＝Ｗ１を付与し、重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）とする。ここで、Ｗ１は、予め定めた定数である。
【００５１】
次に、重み設定部１４０は、同一のモデル点ｑｊに対し、複数のデータ点ｐｉが対応付けられているか否かを判定する。
【００５２】
同一のモデル点ｑｊに対し、複数のデータ点ｐｉが対応付けられている場合、重み設定部１４０は、当該モデル点ｑｊを含む重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）について、重みｗｉ＝Ｗ２とする。Ｗ２は、予め定めた定数であり、Ｗ１よりも低い重みである。
【００５３】
そして、重み設定部１４０は、データ点群Ｐに含まれるデータ点ｐｉの各々についての重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）を、変換行列算出部１５０に渡す。
【００５４】
変換行列算出部１５０は、データ点群Ｐとモデル点群Ｑとの対応付け、及び設定された重みｗｉに基づいて、データ点群Ｐに含まれるデータ点ｐｉの座標を変換した場合に、変換後のデータ点群とモデル点群Ｑとの位置ずれが最小となる座標変換行列Ｔｓを算出する。
【００５５】
具体的には、変換行列算出部１５０は、まず、予め座標変換行列ＴをＴ＝ｌ４に初期化する。ここで、ｌ４は、４×４の単位行列である。
【００５６】
次に、変換行列算出部１５０は、データ点群Ｐに含まれるデータ点ｐｉの各々についての重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）に基づいて、位置ずれが最小となる座標変換行列Ｔｓを算出する。ここで、ｓは、位置合わせ回数を示すカウンタであり、初期値はｓ＝１である。
【００５７】
具体的には、変換行列算出部１５０は、対応付けられたデータ点ｐｉの変換後の点ｐｉ’の座標とモデル点ｑｊの座標との距離、及びデータ点ｐｉとモデル点ｑｊとの対応付けに対する重みｗｉを用いて求められる、重み付きの距離の二乗和（下記式（２））が最小となるように、座標変換行列Ｔｓを算出する。
【００５８】
【数２】
2020086941.tif 000004
【００５９】
ただし、変換後の点ｐｉ’の座標Ｘ’は、以下の式（３）で表される。
【００６０】
【数３】
2020086941.tif 000005
【００６１】
ここで、Ｘは、変換前のデータ点ｐｉの座標（ｘ，ｙ，ｚ）を用いたベクトルであり、
2020086941.tif 000006
で表される。
【００６２】
座標変換行列Ｔｓの具体的な算出方法は、例えば参考文献１の手法を用いることができる。
［参考文献１］Olga Sorkine-Hornung, Michael Rabinovich, "Least-Squares Rigid Motion Using SVD", [online], Department of Computer Science, ETH Zurich, January 16, 2017, [平成30年10月5日検索], インターネット<https://igl.ethz.ch/projects/ARAP/svd_rot.pdf>.
【００６３】
変換行列算出部１５０は、算出した座標変換行列Ｔｓを用いて、座標変換行列Ｔを下記式（４）により更新する。
【００６４】
【数４】
2020086941.tif 000007
【００６５】
また、変換行列算出部１５０は、ｓに１を加算する。
【００６６】
そして、変換行列算出部１５０は、データ点群Ｐに含まれるデータ点ｐｉの各々についての重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）と、ｓと、座標変換行列Ｔとを判定部１６０に渡す。
【００６７】
判定部１６０は、予め定めた条件を満たすか否かを判定する。
【００６８】
条件としては、例えば、データ点群Ｐに含まれるデータ点ｐｉの各々についての重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）の位置ずれの平均値が予め定めた第３閾値以下となる場合、カウンタｓが予め定めた第４閾値以上となる場合等である。
【００６９】
そして、予め定めた条件を満たさないと判定した場合、判定部１６０は、データ点群Ｐ及び座標変換行列Ｔを座標変換部１７０に渡す。
【００７０】
一方、予め定めた条件を満たすと判定した場合、判定部１６０は、座標変換行列Ｔを出力部１８０に渡す。
【００７１】
座標変換部１７０は、座標変換行列Ｔに基づいて、データ点群Ｐに含まれるデータ点ｐｉの座標を変換する。
【００７２】
具体的には、座標変換部１７０は、下記式（４）を用いて、データ点群Ｐに含まれるデータ点ｐｉの座標を変換する。
【００７３】
【数５】
2020086941.tif 000008
【００７４】
ここで、変換行列Ｔは、
2020086941.tif 000009
であり、座標変換における回転行列Ｒは、
【００７５】
2020086941.tif 000010
であり、並進ベクトルｔは、
2020086941.tif 000011
である。
【００７６】
そして、座標変換部１７０は、座標変換したデータ点群Ｐ’を、モデル点対応部１３０に渡す。
【００７７】
判定部１６０により予め定めた条件を満たすと判定されるまで、座標変換部１７０による座標変換後のデータ点群Ｐ’に基づいて、モデル点対応部１３０により対応付け、重み設定部１４０による設定、及び変換行列算出部１５０による算出を繰り返すこととなる。
【００７８】
出力部１８０は、座標変換行列Ｔを出力する。
【００７９】
このように、重みｗｉを付与することにより、あいまいな対応付けを除去し、又はあいまいな対応付けの影響小さくすることができるため、位置ずれを小さくすることができる。
【００８０】
例えば、重みｗｉについて、Ｗ１＝１．０、Ｗ２＝０．０とすれば、重複判定がされた対応付け（ｐｉ，ｑｊ）については、無効なものとして処理される。
【００８１】
図２にＷ１＝１．０、Ｗ２＝０．０とした場合の例を示す。図２（ａ）は、データ点群Ｐ及びモデル点群Ｑが共に一部欠落がある場合を示す。
【００８２】
この場合、重複していると判定された対応付け（ｐｉ，ｑｊ）については、図２（ｂ）のように無効として取り扱う。そして、図２（ｃ）のように、位置合わせには、有効な対応付け（ｐｉ，ｑｊ）のみを用いる。
【００８３】
また、例えば、重みｗｉについて、Ｗ１＝１．０、Ｗ２＝０．１とすれば、重複判定がされた対応付け（ｐｉ，ｑｊ）については、上記式（２）の計算における寄与が小さくなる。
【００８４】
そして、点群間の位置ずれが小さくなるにつれて、あいまい性が解消、対応付け可能な点対が増加し、精度の高い位置合わせが可能となる。
【００８５】
＜本発明の第１の実施の形態に係る位置ずれ補正装置１０の作用＞
次に、図３を参照して、本実施形態の位置ずれ補正装置１０の位置ずれ補正処理ルーチンについて説明する。
【００８６】
まず、ステップＳ１００において、入力部１００は、位置合わせの対象となるデータ点ｐｉの座標の集合であるデータ点群Ｐの入力を受け付ける。
【００８７】
ステップＳ１１０において、取得部１２０は、位置合わせの基準となるモデル点ｑｊの座標の集合であるモデル点群Ｑを取得する。
【００８８】
ステップＳ１２０において、変換行列算出部１５０は、予め座標変換行列ＴをＴ＝ｌ４に初期化する。
【００８９】
ステップＳ１３０において、変換行列算出部１５０は、ｓ＝１とする。なお、ｓはカウンタである。
【００９０】
ステップＳ１４０において、モデル点対応部１３０及び重み設定部１４０は、点群の対応付け処理を行う。
【００９１】
ステップＳ１５０において、変換行列算出部１５０は、データ点群Ｐとモデル点群Ｑとの対応付け、及び設定された所定の重みｗｉに基づいて、データ点群Ｐに含まれるデータ点ｐｉの座標を変換した場合に、座標変換後のデータ点群とモデル点群Ｑとの位置ずれが最小となる座標変換行列Ｔｓを算出し、座標変換行列Ｔｓを用いて、座標変換行列Ｔを上記式（３）により更新する。
【００９２】
ステップＳ１６０において、判定部１６０は、予め定めた条件を満たすか否かを判定する。
【００９３】
予め定めた条件を満たさない場合（ステップＳ１６０のＮＯ）、ステップＳ１７０において、座標変換部１７０は、座標変換行列Ｔに基づいて、データ点群Ｐに含まれるデータ点ｐｉの座標を変換する。
【００９４】
ステップＳ１８０において、変換行列算出部１５０は、ｓに１を加算し、ステップＳ１４０へ戻る。
【００９５】
予め定めた条件を満たす場合（ステップＳ１６０のＹＥＳ）、ステップＳ１９０において、出力部１８０は、座標変換行列Ｔを出力し、処理を終了する。
【００９６】
次に、図４を参照して、上記ステップＳ１４０の点群の対応付け処理について説明する。
【００９７】
ステップＳ１４１において、モデル点対応部１３０は、データ点ｐｉの座標及びモデル点ｑｊの座標に基づいて、データ点群Ｐに含まれるデータ点ｐｉの各々について、モデル点群Ｑに含まれるモデル点ｑｊとの対応付けを行う。
【００９８】
ステップＳ１４２において、重み設定部１４０は、データ点群Ｐに含まれるデータ点ｐｉの各々について、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）に対して、一定の重みｗｉ＝Ｗ１を付与し、重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）とする。
【００９９】
ステップＳ１４３において、重み設定部１４０は、データ点群Ｐに含まれるデータ点ｐｉの各々についての重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）において、同一のモデル点ｑｊに対し、複数のデータ点ｐｉが対応付けられているか否かを判定する。
【０１００】
同一のモデル点ｑｊに対し、複数のデータ点ｐｉが対応付けられている場合（ステップＳ１４３のＹＥＳ）、ステップＳ１４４において、重み設定部１４０は、当該モデル点ｑｊを含む重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）について、重みｗｉ＝Ｗ２とし、リターンする。
【０１０１】
同一のモデル点ｑｊに対し、複数のデータ点ｐｉが対応付けられていない場合（ステップＳ１４３のＮＯ）、リターンする。
【０１０２】
以上説明したように、本発明の第１の実施の形態に係る位置ずれ補正装置によれば、予め定めた条件を満たすと判定されるまで、データ点の座標及びモデル点の座標に基づいて、データ点群に含まれるデータ点の各々について、モデル点群に含まれるモデル点との対応付け、同一の当該モデル点に対し、データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の当該モデル点と複数のデータ点の各々との対応付けの各々に対して所定の重みを設定し、データ点群とモデル点群との対応付け、及び設定された所定の重みに基づいて、データ点群に含まれる当該データ点の座標を変換した場合に、変換後のデータ点群とモデル点群との位置ずれが最小となる座標変換行列を算出し、当該座標変換行列に基づいて、データ点群に含まれるデータ点の座標を変換することにより、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる。
【０１０３】
＜本発明の第２の実施の形態に係る位置ずれ補正装置２０の構成＞
図５を用いて、本発明の第２の実施の形態に係る位置ずれ補正装置２０の構成について説明する。なお、第１の実施の形態に係る位置ずれ補正装置１０と同様の構成については、同一の符号を付して詳細な説明は省略する。
【０１０４】
位置ずれ補正装置１０は、ＣＰＵと、ＲＡＭと、後述する位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【０１０５】
図５に示すように、本発明の実施の形態に係る位置ずれ補正装置２０は、入力部１００と、モデル点群記憶部１１０と、取得部２２０と、モデル点対応部１３０と、データ点対応部２３５と、重み設定部２４０と、変換行列算出部１５０と、判定部１６０と、座標変換部２７０と、出力部１８０とを備えて構成される。
【０１０６】
取得部２２０は、位置合わせの基準となるモデル点ｑｊの座標の集合であるモデル点群Ｑを取得する。
【０１０７】
そして、取得部２２０は、データ点群Ｐ及びモデル点群Ｑを、モデル点対応部１３０及びデータ点対応部２３５に渡す。
【０１０８】
データ点対応部２３５は、モデル点ｑｊの座標及びデータ点ｐｉの座標に基づいて、モデル点群Ｑに含まれるモデル点ｑｊの各々について、データ点群Ｐに含まれるデータ点ｐｉとの対応付けを行う。
【０１０９】
具体的には、データ点対応部２３５は、モデル点群Ｑの各モデル点ｑｊの各々について、データ点群Ｐから最近傍点となるデータ点ｐｉを探索する。
【０１１０】
そして、データ点対応部２３５は、モデル点群Ｑに含まれるモデル点ｑｊの各々について、当該モデル点ｑｊと当該モデル点ｑｊの最近傍点であるデータ点ｐｉとの対応付け（ｐｉ，ｑｊ）を、重み設定部２４０に渡す。
【０１１１】
重み設定部２４０は、データ点群Ｐに含まれるデータ点ｐｉの各々について、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）に対して、所定の重みｗｉ＝Ｗ１を付与し、重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）とすると共に、モデル点群Ｑに含まれるモデル点ｑｊの各々について、当該モデル点ｑｊと当該モデル点ｑｊの最近傍点であるデータ点ｐｉとの対応付け（ｐｉ，ｑｊ）に対して、重みｗｊ＝Ｗ１を付与し、重み付き対応付け（ｑｊ，ｐｉ，ｗｊ）とする。
【０１１２】
また、重み設定部２４０は、同一のモデル点ｑｊに対し、複数のデータ点ｐｉが対応付けられている場合、当該モデル点ｑｊを含む重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）の各々について、重みｗｉ＝Ｗ２とする。重み設定部２４０は、更に、同一のデータ点ｐｉに対し、モデル点群Ｑに含まれる複数のモデル点ｑｊが対応付けられた場合に、当該データ点ｐｉを含む重み付き対応付け（ｑｊ，ｐｉ，ｗｊ）の各々について、重みｗｉ＝Ｗ２を付与する。
【０１１３】
そして、重み設定部２４０は、重み付き対応付け（ｐｉ，ｑｊ，ｗｊ）の各々を、変換行列算出部１５０に渡す。
【０１１４】
座標変換部２７０は、座標変換行列Ｔに基づいて、データ点群Ｐに含まれるデータ点ｐｉの座標を変換する。
【０１１５】
具体的には、座標変換部２７０は、上記式（４）を用いて、データ点群Ｐをデータ点群Ｐ’に座標変換する。
【０１１６】
そして、座標変換部２７０は、データ点群Ｐ’を、モデル点対応部１３０及びデータ点対応部２３５に渡す。
【０１１７】
このように、重みｗｊを付与することにより、あいまいな対応付けを除去し、又はあいまいな対応付けの影響小さくすることができるため、位置ずれを小さくすることができる。
【０１１８】
第１の実施の形態と同様に、図６にＷ１＝１．０、Ｗ２＝０．０とした場合の例を示す。図６（ａ）は、データ点群Ｐ及びモデル点群Ｑが共に一部欠落がある場合を示す。
【０１１９】
この場合、重複していると判定された対応付け（ｐｉ，ｑｊ）については、図６（ｂ）のように無効として取り扱う。そして、図６（ｃ）のように、位置合わせには、有効な対応付け（ｐｉ，ｑｊ）のみを用いる。
【０１２０】
よって、逆方向の対応付けを行う場合でも、点群間の位置ずれが小さくなるにつれて、あいまい性が解消、対応付け可能な点対が増加し、精度の高い位置合わせが可能となる。
【０１２１】
＜本発明の第２の実施の形態に係る位置ずれ補正装置２０の作用＞
図７は、本発明の第２の実施の形態に係る点群の対応付け処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る点群の対応付け処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０１２２】
ステップＳ３００において、データ点対応部２３５は、モデル点ｑｊの座標及びデータ点ｐｉの座標に基づいて、モデル点群Ｑに含まれるモデル点ｑｊの各々について、データ点群Ｐに含まれるデータ点ｐｉとの対応付けを行う。
【０１２３】
ステップＳ３０５において、重み設定部２４０は、データ点群Ｐに含まれるデータ点ｐｉの各々について、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）に対して、所定の重みｗｉ＝Ｗ１を付与し、重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）とすると共に、モデル点群Ｑに含まれるモデル点ｑｊの各々について、当該モデル点ｑｊと当該モデル点ｑｊの最近傍点であるデータ点ｐｉとの対応付け（ｐｉ，ｑｊ）に対して、重みｗｊ＝Ｗ１を付与し、重み付き対応付け（ｑｊ，ｐｉ，ｗｊ）とする。
【０１２４】
ステップＳ３１０において、重み設定部２４０は、重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）において、同一のモデル点ｑｊに対し、複数のデータ点ｐｉが対応付けられているか、又は同一のデータ点ｐｉに対し、複数のモデル点ｑｊが対応付けられているか否かを判定する。
【０１２５】
同一のモデル点ｑｊを含む複数の対応付けが無く、かつ、同一のデータ点ｐｉを含む複数の対応付けが無い場合（上記ステップＳ３１０のＮＯ）、リターンする。
【０１２６】
一方、同一のモデル点ｑｊを含む複数の対応付けが有るか、又は同一のデータ点ｐｉを含む複数の対応付けが有る場合（上記ステップＳ３１０のＹＥＳ）、ステップＳ２４４において、重み設定部２４０は、同一のモデル点ｑｊを含む複数の重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）、又は同一のデータ点ｐｉを含む複数の重み付き対応付け（ｐｉ，ｑｊ，ｗｊ）について、重みｗｉ＝Ｗ２、ｗｊ＝Ｗ２とし、リターンする。
【０１２７】
以上説明したように、本発明の第２の実施の形態に係る位置ずれ補正装置によれば、予め定めた条件を満たすと判定されるまで、
【０１２８】
データ点の座標及びモデル点の座標に基づいて、データ点群に含まれるデータ点の各々について、モデル点群に含まれるモデル点との対応付け、同一の当該モデル点に対し、データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の当該モデル点と複数のデータ点の各々との対応付けの各々に対して所定の重みを設定し、モデル点の座標及びデータ点の座標に基づいて、モデル点群に含まれるモデル点の各々について、データ点群に含まれるデータ点との対応付け、同一の当該データ点に対し、モデル点群に含まれる複数のモデル点が対応付けられた場合に、同一の当該データ点と複数のモデル点の各々との対応付けの各々に対して所定の重みを設定し、データ点群とモデル点群との対応付け、及び設定された所定の重みに基づいて、データ点群に含まれる当該データ点の座標を変換した場合に、変換後のデータ点群とモデル点群との位置ずれが最小となる座標変換行列を算出し、当該座標変換行列に基づいて、データ点群に含まれるデータ点の座標を変換することにより、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる。
【０１２９】
＜本発明の第３の実施の形態に係る位置ずれ補正装置３０の構成＞
図８を用いて、本発明の第２の実施の形態に係る位置ずれ補正装置３０の構成について説明する。なお、第１の実施の形態に係る位置ずれ補正装置１０と同様の構成については、同一の符号を付して詳細な説明は省略する。
【０１３０】
位置ずれ補正装置３０は、ＣＰＵと、ＲＡＭと、後述する位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【０１３１】
図８に示すように、本発明の実施の形態に係る位置ずれ補正装置３０は、入力部１００と、モデル点群記憶部１１０と、取得部１２０と、モデル点対応部１３０と、第１有効距離判定部３３２と、重み設定部１４０と、変換行列算出部１５０と、判定部１６０と、座標変換部１７０と、出力部１８０とを備えて構成される。
【０１３２】
本実施形態では、モデル点対応部１３０は、データ点群Ｐに含まれるデータ点ｐｉの各々について、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）を、第１有効距離判定部３３２に渡す。
【０１３３】
第１有効距離判定部３３２は、モデル点対応部１３０により対応付けられたデータ点ｐｉの座標とモデル点ｑｊの座標との距離が予め定めた第１閾値以上である場合に、対応付けを無効化する。
【０１３４】
具体的には、第１有効距離判定部３３２は、まず、データ点ｐｉとモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）の各々について、当該データ点ｐｉと当該モデル点ｑｊとの距離を算出する。
【０１３５】
次に、第１有効距離判定部３３２は、対応付け（ｐｉ，ｑｊ）の各々についての当該データ点ｐｉと当該モデル点ｑｊとの距離が、第１閾値以上であるか否かを判定する。
【０１３６】
距離が第１閾値以上である場合、第１有効距離判定部３３２は、当該対応付け（ｐｉ，ｑｊ）を無効なものとし、以降の処理では用いないこととする。
【０１３７】
一方、距離が第１閾値以上でない場合、第１有効距離判定部３３２は、当該対応付け（ｐｉ，ｑｊ）を有効なものとする。
【０１３８】
そして、第１有効距離判定部３３２は、データ点群Ｐに含まれるデータ点ｐｉの各々について、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）のうち有効な対応付け（ｐｉ，ｑｊ）を、重み設定部１４０に渡す。
【０１３９】
＜本発明の第３の実施の形態に係る位置ずれ補正装置３０の作用＞
図９は、本発明の第３の実施の形態に係る点群の対応付け処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る点群の対応付け処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０１４０】
ステップＳ４００において、第１有効距離判定部３３２は、データ点ｐｉとモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）の各々について、当該データ点ｐｉと当該モデル点ｑｊとの距離を算出する。
【０１４１】
ステップＳ４１０において、第１有効距離判定部３３２は、対応付け（ｐｉ，ｑｊ）の各々についての当該データ点ｐｉと当該モデル点ｑｊとの距離が、第１閾値以上であるか否かを判定する。
【０１４２】
距離が第１閾値以上である場合（上記ステップＳ４１０のＹＥＳ）ステップＳ４２０において、第１有効距離判定部３３２は、当該対応付け（ｐｉ，ｑｊ）を無効なものとし、ステップＳ１４２に進む。
【０１４３】
一方、距離が第１閾値以上でない場合（上記ステップＳ４１０のＹＥＳ）、ステップＳ１４２に進む。
【０１４４】
以上説明したように、本発明の第３の実施の形態に係る位置ずれ補正装置によれば、対応付けられたデータ点の座標とモデル点の座標との距離が予め定めた第１閾値以上である場合に、対応付けを無効化するため、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度をより高めることができる。
【０１４５】
＜本発明の第４の実施の形態に係る位置ずれ補正装置４０の構成＞
図１０を用いて、本発明の第４の実施の形態に係る位置ずれ補正装置４０の構成について説明する。なお、第１の実施の形態に係る位置ずれ補正装置１０、第２の実施の形態に係る位置ずれ補正装置２０及び第３の実施の形態に係る位置ずれ補正装置３０と同様の構成については、同一の符号を付して詳細な説明は省略する。
【０１４６】
位置ずれ補正装置４０は、ＣＰＵと、ＲＡＭと、後述する位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【０１４７】
図１０に示すように、本発明の実施の形態に係る位置ずれ補正装置４０は、入力部１００と、モデル点群記憶部１１０と、取得部２２０と、モデル点対応部１３０と、データ点対応部２３５と、第１有効距離判定部３３２と、第２有効距離判定部４３７と、重み設定部２４０と、変換行列算出部１５０と、判定部１６０と、座標変換部２７０と、出力部１８０とを備えて構成される。
【０１４８】
本実施形態では、モデル点対応部１３０は、データ点群Ｐに含まれるデータ点ｐｉの各々について、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）を、第１有効距離判定部３３２に渡す。
【０１４９】
また、データ点対応部２３５は、モデル点群Ｑの各モデル点ｑｊの各々について、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）を、第２有効距離判定部４３７に渡す。
【０１５０】
第２有効距離判定部４３７は、データ点対応部２３５により対応付けられたモデル点ｐｉの座標とデータ点ｐｉの座標との距離が予め定めた第２閾値以上である場合に、対応付けを無効化する。
【０１５１】
具体的には、第２有効距離判定部４３７は、まず、モデル点ｑｊとデータ点ｐｉとの対応付け（ｐｉ，ｑｊ）の各々について、当該モデル点ｑｊと当該データ点ｐｉとの距離を算出する。
【０１５２】
次に、第２有効距離判定部４３７は、対応付け（ｐｉ，ｑｊ）の各々についての当該モデル点ｑｊと当該データ点ｐｉとの距離が、第２閾値以上であるか否かを判定する。
【０１５３】
距離が第２閾値以上である場合、第２有効距離判定部４３７は、当該対応付け（ｐｉ，ｑｊ）を無効なものとし、以降の処理では用いないこととする。
【０１５４】
一方、距離が第２閾値以上でない場合、第２有効距離判定部４３７は、当該対応付け（ｐｉ，ｑｊ）を有効なものとする。
【０１５５】
そして、第２有効距離判定部４３７は、モデル点群Ｑに含まれるモデル点ｐｉの各々について、当該モデル点ｑｊと当該モデル点ｑｊの最近傍点であるデータ点ｐｉとの対応付け（ｐｉ，ｑｊ）のうち有効な対応付け（ｐｉ，ｑｊ）を、重み設定部２４０に渡す。
【０１５６】
＜本発明の第４の実施の形態に係る位置ずれ補正装置４０の作用＞
図１１は、本発明の第４の実施の形態に係る点群の対応付け処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る点群の対応付け処理ルーチン、第２の実施の形態に係る点群の対応付け処理ルーチン、及び第３の実施の形態に係る点群の対応付け処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０１５７】
ステップＳ５１０において、第２有効距離判定部４３７は、モデル点ｑｊとデータ点ｐｉとの対応付け（ｐｉ，ｑｊ）の各々について、当該モデル点ｑｊと当該データ点ｐｉとの距離を算出する。
【０１５８】
ステップＳ５２０において、第２有効距離判定部４３７は、対応付け（ｐｉ，ｑｊ）の各々についての当該モデル点ｑｊと当該データ点ｐｉとの距離が、第２閾値以上であるか否かを判定する。
【０１５９】
距離が第２閾値以上である場合（上記ステップＳ５２０のＹＥＳ）ステップＳ５３０において、第２有効距離判定部４３７は、当該対応付け（ｐｉ，ｑｊ）を無効なものとし、ステップＳ３０５に進む。
【０１６０】
一方、距離が第２閾値以上でない場合（上記ステップＳ５２０のＹＥＳ）、ステップＳ３０５に進む。
【０１６１】
以上説明したように、本発明の第４の実施の形態に係る位置ずれ補正装置によれば、モデル点群に含まれるモデル点の各々について、当該モデル点と、当該モデル点に対応付けられたデータ点との距離が予め定めた第２閾値以上か否かを判定し、当該距離が第２閾値以上である場合に、当該モデル点に対する対応付けを無効化するため、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度をより高めることができる。
【０１６２】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【０１６３】
例えば、上記実施の形態では、３次元の点群の場合について説明したが、これに限定されるものではない。２次元の点群を用いる構成としてもよい。
【０１６４】
また、重み設定部１４０（２４０）は、対応付けられたデータ点群Ｐのデータ点ｐｉとモデル点群Ｑのモデル点ｑｊとの位置ずれが予め定めた第３閾値以下となる場合、又は判定部１６０による繰り返し回数が予め定めた第４閾値以上となる場合、重みの設定方法を変更する。
【０１６５】
また、判定部１６０が、予め定めた条件を満たさないと判定した場合、判定結果に基づいて、重み設定部１４０（２４０）に対し、重みの設定方法を変更するように指示する構成としてもよい。
【０１６６】
具体的には、判定部１６０は、データ点群Ｐに含まれるデータ点ｐｉの各々についての重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）の位置ずれの平均値が予め定めた第３閾値以下となる場合、又はカウンタｓが予め定めた第４閾値以上となる場合のうち、予め定めた条件でない方の条件を満たした場合に、重み設定部１４０に対し、重みの設定方法（Ｗ１及びＷ２の値）を変更するように指示する。例えば、Ｗ２の値を大きくするように変更してもよい。
【０１６７】
点群に欠落がある場合、対応付けが無効化されたままとなってしまう場合があるため、重みの値を変更することで、位置合わせへの寄与を変更し、精度を向上させるためである。
【０１６８】
また、重み設定部１４０及び重み設定部２４０は、重みを予め定めたＷ２に変更する構成としたが、これに限定されるものではない。
【０１６９】
例えば、同一のモデル点ｑｊに対し、複数のデータ点ｐｉが対応している場合、又は同一のデータ点ｐｉに対し、複数のモデル点ｑｊが対応している場合には、ｗｉを１０で割った値とする、重複数に応じた数で割った値とする等の方法により変更する構成としてもよい。
【０１７０】
また、重み設定部２４０は、同一のモデル点ｑｊに対し、複数のデータ点ｐｉが対応している場合、又は同一のデータ点ｐｉに対し、複数のモデル点ｑｊが対応している場合のみ、重みの変更をする構成としたが、これに限定されるものではない。
【０１７１】
例えば、重み設定部２４０は、データ点群Ｐに含まれるデータ点ｐｉの各々について、モデル点ｑｊとの対応付けを行った結果、当該データ点ｐｉと当該データ点ｐｉの最近傍点であるモデル点ｑｊとの対応付け（ｐｉ，ｑｊ）がある場合に、モデル点群Ｑに含まれるモデル点ｑｊの各々について、データ点ｐｉとの対応付けを行った結果、当該モデル点ｑｊと対応付けられたデータ点がデータ点ｐｉと異なる場合に、対応付けに不一致があると判定し、重み設定部２４０は、当該不一致となる対応付け（ｐｉ，ｑｊ）について、重みｗｉ＝Ｗ３を付与し、重み付き対応付け（ｐｉ，ｑｊ，ｗｉ）としてもよい。ここで、Ｗ３は予め定めた定数である。
【０１７２】
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。
【符号の説明】
【０１７３】
１０   補正装置
２０   補正装置
３０   補正装置
４０   補正装置
１００ 入力部
１１０ モデル点群記憶部
１２０ 取得部
１３０ モデル点対応部
１４０ 重み設定部
１５０ 変換行列算出部
１６０ 判定部
１７０ 座標変換部
１８０ 出力部
２２０ 取得部
２３５ データ点対応部
２４０ 重み設定部
２７０ 座標変換部
３３２ 第１有効距離判定部
４３７ 第２有効距離判定部
【図１】
2020086941.tif 000012
【図２】
2020086941.tif 000013
【図３】
2020086941.tif 000014
【図４】
2020086941.tif 000015
【図５】
2020086941.tif 000016
【図６】
2020086941.tif 000017
【図７】
2020086941.tif 000018
【図８】
2020086941.tif 000019
【図９】
2020086941.tif 000020
【図１０】
2020086941.tif 000021
【図１１】
2020086941.tif 000022
【図１２】
2020086941.tif 000023
【図１３】
2020086941.tif 000024
【図１４】
2020086941.tif 000025
【図１５】
2020086941.tif 000026