JP7111297B2 - 位置ずれ補正装置及びプログラム - Google Patents

Description

本発明は、位置ずれ補正装置及びプログラムに関する。

従来から、ＧＮＳＳ（全球測位衛星システム、例えばＧＰＳ）等を用いた車両測位装置と車両搭載の外界センサ（カメラやＬＩＤＡＲ）により検出した白線情報等を走行データとして収集し、走行環境の地図が生成されている。

複数の走行データを集約して地図の精度向上や生成範囲の拡大を行う際、ＧＰＳの測位誤差などによる走行軌跡の位置ずれが生じるため、異なる走行データの相対的な位置ずれを補正する必要がある。

相対的な位置ずれを補正するために、３次元環境をモデル化した点群Ｑに対して、取得した３次元点群Ｐを重ね合わせることにより物体の位置・姿勢や観測者の位置・姿勢を推定する技術において、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法（非特許文献１）がよく用いられている（特許文献１、特許文献２）。

ＩＣＰ法は、（１）点群Ｐと点群Ｑに含まれる点を対応付け、（２）対応点間の距離が小さくなるような座標変換行列（回転、並進）の算出を繰り返して最適な座標変換行列を推定する手法である（図１２）。

ＩＣＰ法は収束性が保証されているが、誤った対応が行われると局所解に陥る場合があるため、正しい対応付けを行うことが重要である。

特許文献１では、点群Ｐの各点に対し、点群Ｑ内で距離が最小となる点を探索し対応付ける。この時、距離が指定の値より大きい場合に誤対応として無効とする方法がとられている。

また、逆方向の対応付けを行い一致した場合のみ有効とするという手法も用いられている（特許文献３、非特許文献２）。

一般的には点群Ｑが密に得られており、ＩＣＰ法での繰り返し処理により徐々に正しい対応付けが行われることが期待できる。このように、点群が密な場合にはＩＣＰ法での繰り返し処理により、初期の誤った点への対応が徐々に解消し、多くの場合は正しい対応による誤差の小さい位置合わせが行われる。

"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>．

特開２０１３－０７２８５７号公報 特開２００８－２５０９０５号公報 特開２０１１－２０９１１６号公報

しかし、点群にノイズや欠落がある場合、初期の誤った対応が解消せず、誤った対応による誤差の大きい位置合わせ結果に収束する可能性が高くなる、という問題があった。

また、点群Ｑが疎な場合には初期状態の誤対応が解消されず、局所解に陥る可能性が高くなってしまう、という問題があった（図１３）。

また、特許文献３の手法では、点群Ｐと点群Ｑとのずれ方、配置によっては、誤対応が残存してしまう、という問題があった。

本発明は、上記問題点を解決するためになされたもので、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる位置ずれ補正装置及びプログラムを提供することを目的とする。

上記の目的を達成するために、本発明の位置ずれ補正装置は、位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付ける入力部と、位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得する取得部と、前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行うモデル点対応部と、同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定する重み設定部と、前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出する変換行列算出部と、前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換する座標変換部と、予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる判定部と、を含んで構成されている。

また、本発明のプログラムは、コンピュータを、位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付ける入力部、位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得する取得部、前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行うモデル点対応部、同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定する重み設定部、前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出する変換行列算出部、前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換する座標変換部、及び予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる判定部として機能させるためのプログラムである。

本発明の状態予測装置及びプログラムによれば、入力部が、位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付け、取得部が、位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得し、モデル点対応部が、前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行う。

重み設定部が、同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定し、変換行列算出部が、前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出し、座標変換部が、前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換し、判定部が、予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる。

このように、予め定めた条件を満たすと判定されるまで、データ点の座標及びモデル点の座標に基づいて、データ点群に含まれるデータ点の各々について、モデル点群に含まれるモデル点との対応付け、同一の当該モデル点に対し、データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の当該モデル点と複数のデータ点の各々との対応付けの各々に対して所定の重みを設定し、データ点群とモデル点群との対応付け、及び設定された所定の重みに基づいて、データ点群に含まれる当該データ点の座標を変換した場合に、変換後のデータ点群とモデル点群との位置ずれが最小となる座標変換行列を算出し、当該座標変換行列に基づいて、データ点群に含まれるデータ点の座標を変換することにより、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる。

また、本発明の位置ずれ補正装置の前記変換行列算出部は、対応付けられた前記データ点の変換後の座標と前記モデル点の座標との距離、及び前記データ点と前記モデル点との対応付けに対する重みを用いて求められる、重み付きの距離の二乗和が最小となるように、前記座標変換行列を算出することができる。

また、本発明の位置ずれ補正装置の前記重み設定部は、一つの前記モデル点に対し、一つの前記データ点のみが対応付けられた場合に、前記モデル点と前記データ点との対応付けに対して一定の重みを設定し、同一の前記モデル点に対し、複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して、前記一定の重みより低い重みを設定することができる。

また、本発明の位置ずれ補正装置は、前記モデル点の座標及び前記データ点の座標に基づいて、前記モデル点群に含まれるモデル点の各々について、前記データ点群に含まれるデータ点との対応付けるデータ点対応部を更に含み、前記重み設定部は、同一の前記データ点に対し、前記モデル点群に含まれる複数のモデル点が対応付けられた場合に、同一の前記データ点と前記複数のモデル点の各々との対応付けの各々に対して所定の重みを設定し、前記判定部は、予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記データ点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させることができる。

また、本発明の位置ずれ補正装置部は、前記モデル点対応部により対応付けられた前記データ点の座標と前記モデル点の座標との距離が予め定めた第１閾値以上である場合に、対応付けを無効化する第１有効距離判定部を更に含むことができる。

また、本発明の位置ずれ補正装置部は、前記データ点対応部により対応付けられた前記モデル点の座標と前記データ点の座標との距離が予め定めた第２閾値以上である場合に、対応付けを無効化する第２有効距離判定部を更に含むことができる。

また、本発明の位置ずれ補正装置の前記重み設定部は、対応付けられた前記データ点群のデータ点と前記モデル点群のモデル点との位置ずれが予め定めた第３閾値以下となる場合、又は前記判定部による繰り返し回数が予め定めた第４閾値以上となる場合、重みの設定方法を変更することができる。

以上説明したように、本発明の位置ずれ補正装置及びプログラムによれば、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる、という効果が得られる。

本発明の第１の実施の形態における概略構成を示すブロック図である。 本発明の第１の実施の形態における点群の対応付けの例を示すイメージ図である。 本発明の第１の実施の形態における位置ずれ補正処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態における点群の対応付け処理ルーチンの内容を示すフローチャートである。 本発明の第２の実施の形態における概略構成を示すブロック図である。 本発明の第２の実施の形態における３次元点群の位置合わせの例を示すイメージ図である。 本発明の第２の実施の形態における点群の対応付け処理ルーチンの内容を示すフローチャートである。 本発明の第３の実施の形態における概略構成を示すブロック図である。 本発明の第３の実施の形態における点群の対応付け処理ルーチンの内容を示すフローチャートである。 本発明の第４の実施の形態における概略構成を示すブロック図である。 本発明の第４の実施の形態における点群の対応付け処理ルーチンの内容を示すフローチャートである。 従来の３次元点群の位置合わせの例を示すイメージ図である。 従来の３次元点群の位置合わせによる誤対応が残存する例を示すイメージ図である。 ３次元点群の位置合わせの基本構成を示すイメージ図である。 本発明の実施の形態における３次元点群の位置合わせの概要を示すイメージ図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。

＜本発明の実施の形態の概要＞
従来の点群間の位置合わせでは、３次元のデータ点群Ｐの点列で表現される図形（図１２（ａ））をモデル点群Ｑの点列で表現される図形に重ね合わせる（図１２（ｂ））。この図形は、線図形、面、立体など任意の図形である。

データ点群Ｐとモデル点群Ｑとの間で、対応するデータ点ｐ_ｉの座標とモデル点ｑ_ｊの座標の距離が最小となるようにデータ点群Ｐを座標変換する。例えばＩＣＰ法等を用いて、点群間の対応付けを行う。なお、データ点群Ｐは、モデル点群Ｑと同一の座標系にあるものとする。

ＩＣＰ法の基本構成を図１４（ａ）に示す。ＩＣＰ法の基本構成で行われる最近傍点の探索・対応付けでは、誤った対応付けが行われる可能性がある。

また、非特許文献２のように逆方向の対応付けによる構成を図１４（ｂ）に示す。この場合でも、ＩＣＰ法の繰り返し処理の初期段階において、データ点群Ｐとモデル点群Ｑとの位置ずれが大きい場合、点群の配置によっては誤対応が発生しやすい。

本発明の実施の形態は、このようなノイズや欠落がある点群の位置合わせに関するものである。

本実施の形態では、誤対応の発生を抑制するため、データ点群Ｐとモデル点群Ｑとの対応付けがあいまいな場合、その対応付けを無効化するか、又はその対応付けに対して小さな重みを設定する（図１５（ｃ）。あいまい性の判定基準として、従来技術のようなモデル点とデータ点の距離のほかに、対応付けの重複（複数のデータ点から同一モデル点への対応など）を用いる。

従来のＩＣＰ法の応用では、モデル点群Ｑはノイズなどを除去した状態を仮定しているが、本実施の形態では、位置合わせの基準となるモデル点群Ｑにもノイズや欠落が含まれる場合においても、誤った対応付けによる位置合わせを抑制することができる（図１５（ｄ））。

＜本発明の第１の実施の形態に係る位置ずれ補正装置１０の構成＞
図１を用いて、本発明の実施の形態にかかる位置ずれ補正装置１０について説明する。

位置ずれ補正装置１０は、ＣＰＵと、ＲＡＭと、後述する位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

図１に示すように、本発明の実施の形態に係る位置ずれ補正装置１０は、入力部１００と、モデル点群記憶部１１０と、取得部１２０と、モデル点対応部１３０と、重み設定部１４０と、変換行列算出部１５０と、判定部１６０と、座標変換部１７０と、出力部１８０とを備えて構成される。

入力部１００は、位置合わせの対象となるデータ点ｐ_ｉの座標の集合であるデータ点群Ｐの入力を受け付ける。データ点群Ｐは、ｎ個のデータ点ｐ_ｉ｛ｉ＝１，...，ｎ｝を含む。データ点ｐ_ｉは３次元の点であり、データ点ｐ_ｉの座標は（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）で表される。なお、入力部１００に入力されるデータ点群Ｐは、後述のモデル点群Ｑと同一の座標系であるか、又は同一の座標系に予め変換されたものである。

そして、入力部１００は、データ点群Ｐを、取得部１２０に渡す。

モデル点群記憶部１１０には、位置合わせの基準となるモデル点ｑ_ｊの座標の集合であるモデル点群Ｑが格納されている。モデル点群Ｑは、ｍ個のモデル点ｑ_ｊ｛ｊ＝１，...，ｍ｝を含む。モデル点ｑ_ｊは３次元の点であり、モデル点ｑ_ｊの座標は（ｘ_ｊ，ｙ_ｊ，ｚ_ｊ）で表される。

取得部１２０は、位置合わせの基準となるモデル点ｑ_ｊの座標の集合であるモデル点群Ｑを取得する。

そして、取得部１２０は、データ点群Ｐ及びモデル点群Ｑを、モデル点対応部１３０に渡す。

モデル点対応部１３０は、データ点ｐ_ｉの座標及びモデル点ｑ_ｊの座標に基づいて、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、モデル点群Ｑに含まれるモデル点ｑ_ｊとの対応付けを行う。

具体的には、モデル点対応部１３０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、モデル点群Ｑから最近傍点となるモデル点ｑ_ｊを探索する（下記式（１））。

そして、モデル点対応部１３０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）を、重み設定部１４０に渡す。

重み設定部１４０は、同一のモデル点ｑ_ｊに対し、データ点群Ｐに含まれる複数のデータ点ｐ_ｉが対応付けられた場合に、同一のモデル点ｑ_ｊと当該複数のデータ点ｐ_ｉの各々との対応付け（ｐ_ｉ，ｑ_ｊ）の各々に対して所定の重みｗ_ｉを設定する。

具体的には、重み設定部１４０は、まず、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）に対して、所定の重みｗ_ｉ＝Ｗ１を付与し、重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）とする。ここで、Ｗ１は、予め定めた定数である。

次に、重み設定部１４０は、同一のモデル点ｑ_ｊに対し、複数のデータ点ｐ_ｉが対応付けられているか否かを判定する。

同一のモデル点ｑ_ｊに対し、複数のデータ点ｐ_ｉが対応付けられている場合、重み設定部１４０は、当該モデル点ｑ_ｊを含む重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）について、重みｗ_ｉ＝Ｗ２とする。Ｗ２は、予め定めた定数であり、Ｗ１よりも低い重みである。

そして、重み設定部１４０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々についての重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）を、変換行列算出部１５０に渡す。

変換行列算出部１５０は、データ点群Ｐとモデル点群Ｑとの対応付け、及び設定された重みｗ_ｉに基づいて、データ点群Ｐに含まれるデータ点ｐ_ｉの座標を変換した場合に、変換後のデータ点群とモデル点群Ｑとの位置ずれが最小となる座標変換行列Ｔｓを算出する。

具体的には、変換行列算出部１５０は、まず、予め座標変換行列ＴをＴ＝ｌ４に初期化する。ここで、ｌ４は、４×４の単位行列である。

次に、変換行列算出部１５０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々についての重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）に基づいて、位置ずれが最小となる座標変換行列Ｔｓを算出する。ここで、ｓは、位置合わせ回数を示すカウンタであり、初期値はｓ＝１である。

具体的には、変換行列算出部１５０は、対応付けられたデータ点ｐ_ｉの変換後の点ｐ_ｉ’の座標とモデル点ｑ_ｊの座標との距離、及びデータ点ｐ_ｉとモデル点ｑ_ｊとの対応付けに対する重みｗ_ｉを用いて求められる、重み付きの距離の二乗和（下記式（２））が最小となるように、座標変換行列Ｔｓを算出する。

ただし、変換後の点ｐ_ｉ’の座標Ｘ’は、以下の式（３）で表される。

ここで、Ｘは、変換前のデータ点ｐ_ｉの座標（ｘ，ｙ，ｚ）を用いたベクトルであり、

で表される。

座標変換行列Ｔｓの具体的な算出方法は、例えば参考文献１の手法を用いることができる。
［参考文献１］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>.

変換行列算出部１５０は、算出した座標変換行列Ｔｓを用いて、座標変換行列Ｔを下記式（４）により更新する。

また、変換行列算出部１５０は、ｓに１を加算する。

そして、変換行列算出部１５０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々についての重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）と、ｓと、座標変換行列Ｔとを判定部１６０に渡す。

判定部１６０は、予め定めた条件を満たすか否かを判定する。

条件としては、例えば、データ点群Ｐに含まれるデータ点ｐ_ｉの各々についての重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）の位置ずれの平均値が予め定めた第３閾値以下となる場合、カウンタｓが予め定めた第４閾値以上となる場合等である。

そして、予め定めた条件を満たさないと判定した場合、判定部１６０は、データ点群Ｐ及び座標変換行列Ｔを座標変換部１７０に渡す。

一方、予め定めた条件を満たすと判定した場合、判定部１６０は、座標変換行列Ｔを出力部１８０に渡す。

座標変換部１７０は、座標変換行列Ｔに基づいて、データ点群Ｐに含まれるデータ点ｐ_ｉの座標を変換する。

具体的には、座標変換部１７０は、下記式（４）を用いて、データ点群Ｐに含まれるデータ点ｐ_ｉの座標を変換する。

ここで、変換行列Ｔは、

であり、座標変換における回転行列Ｒは、

であり、並進ベクトルｔは、

である。

そして、座標変換部１７０は、座標変換したデータ点群Ｐ’を、モデル点対応部１３０に渡す。

判定部１６０により予め定めた条件を満たすと判定されるまで、座標変換部１７０による座標変換後のデータ点群Ｐ’に基づいて、モデル点対応部１３０により対応付け、重み設定部１４０による設定、及び変換行列算出部１５０による算出を繰り返すこととなる。

出力部１８０は、座標変換行列Ｔを出力する。

このように、重みｗ_ｉを付与することにより、あいまいな対応付けを除去し、又はあいまいな対応付けの影響小さくすることができるため、位置ずれを小さくすることができる。

例えば、重みｗ_ｉについて、Ｗ１＝１．０、Ｗ２＝０．０とすれば、重複判定がされた対応付け（ｐ_ｉ，ｑ_ｊ）については、無効なものとして処理される。

図２にＷ１＝１．０、Ｗ２＝０．０とした場合の例を示す。図２（ａ）は、データ点群Ｐ及びモデル点群Ｑが共に一部欠落がある場合を示す。

この場合、重複していると判定された対応付け（ｐ_ｉ，ｑ_ｊ）については、図２（ｂ）のように無効として取り扱う。そして、図２（ｃ）のように、位置合わせには、有効な対応付け（ｐ_ｉ，ｑ_ｊ）のみを用いる。

また、例えば、重みｗ_ｉについて、Ｗ１＝１．０、Ｗ２＝０．１とすれば、重複判定がされた対応付け（ｐ_ｉ，ｑ_ｊ）については、上記式（２）の計算における寄与が小さくなる。

そして、点群間の位置ずれが小さくなるにつれて、あいまい性が解消、対応付け可能な点対が増加し、精度の高い位置合わせが可能となる。

＜本発明の第１の実施の形態に係る位置ずれ補正装置１０の作用＞
次に、図３を参照して、本実施形態の位置ずれ補正装置１０の位置ずれ補正処理ルーチンについて説明する。

まず、ステップＳ１００において、入力部１００は、位置合わせの対象となるデータ点ｐ_ｉの座標の集合であるデータ点群Ｐの入力を受け付ける。

ステップＳ１１０において、取得部１２０は、位置合わせの基準となるモデル点ｑ_ｊの座標の集合であるモデル点群Ｑを取得する。

ステップＳ１２０において、変換行列算出部１５０は、予め座標変換行列ＴをＴ＝ｌ４に初期化する。

ステップＳ１３０において、変換行列算出部１５０は、ｓ＝１とする。なお、ｓはカウンタである。

ステップＳ１４０において、モデル点対応部１３０及び重み設定部１４０は、点群の対応付け処理を行う。

ステップＳ１５０において、変換行列算出部１５０は、データ点群Ｐとモデル点群Ｑとの対応付け、及び設定された所定の重みｗ_ｉに基づいて、データ点群Ｐに含まれるデータ点ｐ_ｉの座標を変換した場合に、座標変換後のデータ点群とモデル点群Ｑとの位置ずれが最小となる座標変換行列Ｔｓを算出し、座標変換行列Ｔｓを用いて、座標変換行列Ｔを上記式（３）により更新する。

ステップＳ１６０において、判定部１６０は、予め定めた条件を満たすか否かを判定する。

予め定めた条件を満たさない場合（ステップＳ１６０のＮＯ）、ステップＳ１７０において、座標変換部１７０は、座標変換行列Ｔに基づいて、データ点群Ｐに含まれるデータ点ｐ_ｉの座標を変換する。

ステップＳ１８０において、変換行列算出部１５０は、ｓに１を加算し、ステップＳ１４０へ戻る。

予め定めた条件を満たす場合（ステップＳ１６０のＹＥＳ）、ステップＳ１９０において、出力部１８０は、座標変換行列Ｔを出力し、処理を終了する。

次に、図４を参照して、上記ステップＳ１４０の点群の対応付け処理について説明する。

ステップＳ１４１において、モデル点対応部１３０は、データ点ｐ_ｉの座標及びモデル点ｑ_ｊの座標に基づいて、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、モデル点群Ｑに含まれるモデル点ｑ_ｊとの対応付けを行う。

ステップＳ１４２において、重み設定部１４０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）に対して、一定の重みｗ_ｉ＝Ｗ１を付与し、重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）とする。

ステップＳ１４３において、重み設定部１４０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々についての重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）において、同一のモデル点ｑ_ｊに対し、複数のデータ点ｐ_ｉが対応付けられているか否かを判定する。

同一のモデル点ｑ_ｊに対し、複数のデータ点ｐ_ｉが対応付けられている場合（ステップＳ１４３のＹＥＳ）、ステップＳ１４４において、重み設定部１４０は、当該モデル点ｑ_ｊを含む重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）について、重みｗ_ｉ＝Ｗ２とし、リターンする。

同一のモデル点ｑ_ｊに対し、複数のデータ点ｐ_ｉが対応付けられていない場合（ステップＳ１４３のＮＯ）、リターンする。

以上説明したように、本発明の第１の実施の形態に係る位置ずれ補正装置によれば、予め定めた条件を満たすと判定されるまで、データ点の座標及びモデル点の座標に基づいて、データ点群に含まれるデータ点の各々について、モデル点群に含まれるモデル点との対応付け、同一の当該モデル点に対し、データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の当該モデル点と複数のデータ点の各々との対応付けの各々に対して所定の重みを設定し、データ点群とモデル点群との対応付け、及び設定された所定の重みに基づいて、データ点群に含まれる当該データ点の座標を変換した場合に、変換後のデータ点群とモデル点群との位置ずれが最小となる座標変換行列を算出し、当該座標変換行列に基づいて、データ点群に含まれるデータ点の座標を変換することにより、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる。

＜本発明の第２の実施の形態に係る位置ずれ補正装置２０の構成＞
図５を用いて、本発明の第２の実施の形態に係る位置ずれ補正装置２０の構成について説明する。なお、第１の実施の形態に係る位置ずれ補正装置１０と同様の構成については、同一の符号を付して詳細な説明は省略する。

位置ずれ補正装置１０は、ＣＰＵと、ＲＡＭと、後述する位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

図５に示すように、本発明の実施の形態に係る位置ずれ補正装置２０は、入力部１００と、モデル点群記憶部１１０と、取得部２２０と、モデル点対応部１３０と、データ点対応部２３５と、重み設定部２４０と、変換行列算出部１５０と、判定部１６０と、座標変換部２７０と、出力部１８０とを備えて構成される。

取得部２２０は、位置合わせの基準となるモデル点ｑ_ｊの座標の集合であるモデル点群Ｑを取得する。

そして、取得部２２０は、データ点群Ｐ及びモデル点群Ｑを、モデル点対応部１３０及びデータ点対応部２３５に渡す。

データ点対応部２３５は、モデル点ｑ_ｊの座標及びデータ点ｐ_ｉの座標に基づいて、モデル点群Ｑに含まれるモデル点ｑ_ｊの各々について、データ点群Ｐに含まれるデータ点ｐ_ｉとの対応付けを行う。

具体的には、データ点対応部２３５は、モデル点群Ｑの各モデル点ｑ_ｊの各々について、データ点群Ｐから最近傍点となるデータ点ｐ_ｉを探索する。

そして、データ点対応部２３５は、モデル点群Ｑに含まれるモデル点ｑ_ｊの各々について、当該モデル点ｑ_ｊと当該モデル点ｑ_ｊの最近傍点であるデータ点ｐ_ｉとの対応付け（ｐ_ｉ，ｑ_ｊ）を、重み設定部２４０に渡す。

重み設定部２４０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）に対して、所定の重みｗ_ｉ＝Ｗ１を付与し、重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）とすると共に、モデル点群Ｑに含まれるモデル点ｑ_ｊの各々について、当該モデル点ｑ_ｊと当該モデル点ｑ_ｊの最近傍点であるデータ点ｐ_ｉとの対応付け（ｐ_ｉ，ｑ_ｊ）に対して、重みｗ_ｊ＝Ｗ１を付与し、重み付き対応付け（ｑ_ｊ，ｐ_ｉ，ｗ_ｊ）とする。

また、重み設定部２４０は、同一のモデル点ｑ_ｊに対し、複数のデータ点ｐ_ｉが対応付けられている場合、当該モデル点ｑ_ｊを含む重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）の各々について、重みｗ_ｉ＝Ｗ２とする。重み設定部２４０は、更に、同一のデータ点ｐ_ｉに対し、モデル点群Ｑに含まれる複数のモデル点ｑ_ｊが対応付けられた場合に、当該データ点ｐ_ｉを含む重み付き対応付け（ｑ_ｊ，ｐ_ｉ，ｗ_ｊ）の各々について、重みｗ_ｉ＝Ｗ２を付与する。

そして、重み設定部２４０は、重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｊ）の各々を、変換行列算出部１５０に渡す。

座標変換部２７０は、座標変換行列Ｔに基づいて、データ点群Ｐに含まれるデータ点ｐ_ｉの座標を変換する。

具体的には、座標変換部２７０は、上記式（４）を用いて、データ点群Ｐをデータ点群Ｐ’に座標変換する。

そして、座標変換部２７０は、データ点群Ｐ’を、モデル点対応部１３０及びデータ点対応部２３５に渡す。

このように、重みｗ_ｊを付与することにより、あいまいな対応付けを除去し、又はあいまいな対応付けの影響小さくすることができるため、位置ずれを小さくすることができる。

第１の実施の形態と同様に、図６にＷ１＝１．０、Ｗ２＝０．０とした場合の例を示す。図６（ａ）は、データ点群Ｐ及びモデル点群Ｑが共に一部欠落がある場合を示す。

この場合、重複していると判定された対応付け（ｐ_ｉ，ｑ_ｊ）については、図６（ｂ）のように無効として取り扱う。そして、図６（ｃ）のように、位置合わせには、有効な対応付け（ｐ_ｉ，ｑ_ｊ）のみを用いる。

よって、逆方向の対応付けを行う場合でも、点群間の位置ずれが小さくなるにつれて、あいまい性が解消、対応付け可能な点対が増加し、精度の高い位置合わせが可能となる。

＜本発明の第２の実施の形態に係る位置ずれ補正装置２０の作用＞
図７は、本発明の第２の実施の形態に係る点群の対応付け処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る点群の対応付け処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。

ステップＳ３００において、データ点対応部２３５は、モデル点ｑ_ｊの座標及びデータ点ｐ_ｉの座標に基づいて、モデル点群Ｑに含まれるモデル点ｑ_ｊの各々について、データ点群Ｐに含まれるデータ点ｐ_ｉとの対応付けを行う。

ステップＳ３０５において、重み設定部２４０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）に対して、所定の重みｗ_ｉ＝Ｗ１を付与し、重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）とすると共に、モデル点群Ｑに含まれるモデル点ｑ_ｊの各々について、当該モデル点ｑ_ｊと当該モデル点ｑ_ｊの最近傍点であるデータ点ｐ_ｉとの対応付け（ｐ_ｉ，ｑ_ｊ）に対して、重みｗ_ｊ＝Ｗ１を付与し、重み付き対応付け（ｑ_ｊ，ｐ_ｉ，ｗ_ｊ）とする。

ステップＳ３１０において、重み設定部２４０は、重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）において、同一のモデル点ｑ_ｊに対し、複数のデータ点ｐ_ｉが対応付けられているか、又は同一のデータ点ｐ_ｉに対し、複数のモデル点ｑ_ｊが対応付けられているか否かを判定する。

同一のモデル点ｑ_ｊを含む複数の対応付けが無く、かつ、同一のデータ点ｐ_ｉを含む複数の対応付けが無い場合（上記ステップＳ３１０のＮＯ）、リターンする。

一方、同一のモデル点ｑ_ｊを含む複数の対応付けが有るか、又は同一のデータ点ｐ_ｉを含む複数の対応付けが有る場合（上記ステップＳ３１０のＹＥＳ）、ステップＳ２４４において、重み設定部２４０は、同一のモデル点ｑ_ｊを含む複数の重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）、又は同一のデータ点ｐ_ｉを含む複数の重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｊ）について、重みｗ_ｉ＝Ｗ２、ｗ_ｊ＝Ｗ２とし、リターンする。

以上説明したように、本発明の第２の実施の形態に係る位置ずれ補正装置によれば、予め定めた条件を満たすと判定されるまで、

データ点の座標及びモデル点の座標に基づいて、データ点群に含まれるデータ点の各々について、モデル点群に含まれるモデル点との対応付け、同一の当該モデル点に対し、データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の当該モデル点と複数のデータ点の各々との対応付けの各々に対して所定の重みを設定し、モデル点の座標及びデータ点の座標に基づいて、モデル点群に含まれるモデル点の各々について、データ点群に含まれるデータ点との対応付け、同一の当該データ点に対し、モデル点群に含まれる複数のモデル点が対応付けられた場合に、同一の当該データ点と複数のモデル点の各々との対応付けの各々に対して所定の重みを設定し、データ点群とモデル点群との対応付け、及び設定された所定の重みに基づいて、データ点群に含まれる当該データ点の座標を変換した場合に、変換後のデータ点群とモデル点群との位置ずれが最小となる座標変換行列を算出し、当該座標変換行列に基づいて、データ点群に含まれるデータ点の座標を変換することにより、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度を高めることができる。

＜本発明の第３の実施の形態に係る位置ずれ補正装置３０の構成＞
図８を用いて、本発明の第２の実施の形態に係る位置ずれ補正装置３０の構成について説明する。なお、第１の実施の形態に係る位置ずれ補正装置１０と同様の構成については、同一の符号を付して詳細な説明は省略する。

位置ずれ補正装置３０は、ＣＰＵと、ＲＡＭと、後述する位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

図８に示すように、本発明の実施の形態に係る位置ずれ補正装置３０は、入力部１００と、モデル点群記憶部１１０と、取得部１２０と、モデル点対応部１３０と、第１有効距離判定部３３２と、重み設定部１４０と、変換行列算出部１５０と、判定部１６０と、座標変換部１７０と、出力部１８０とを備えて構成される。

本実施形態では、モデル点対応部１３０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）を、第１有効距離判定部３３２に渡す。

第１有効距離判定部３３２は、モデル点対応部１３０により対応付けられたデータ点ｐ_ｉの座標とモデル点ｑ_ｊの座標との距離が予め定めた第１閾値以上である場合に、対応付けを無効化する。

具体的には、第１有効距離判定部３３２は、まず、データ点ｐ_ｉとモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）の各々について、当該データ点ｐ_ｉと当該モデル点ｑ_ｊとの距離を算出する。

次に、第１有効距離判定部３３２は、対応付け（ｐ_ｉ，ｑ_ｊ）の各々についての当該データ点ｐ_ｉと当該モデル点ｑ_ｊとの距離が、第１閾値以上であるか否かを判定する。

距離が第１閾値以上である場合、第１有効距離判定部３３２は、当該対応付け（ｐ_ｉ，ｑ_ｊ）を無効なものとし、以降の処理では用いないこととする。

一方、距離が第１閾値以上でない場合、第１有効距離判定部３３２は、当該対応付け（ｐ_ｉ，ｑ_ｊ）を有効なものとする。

そして、第１有効距離判定部３３２は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）のうち有効な対応付け（ｐ_ｉ，ｑ_ｊ）を、重み設定部１４０に渡す。

＜本発明の第３の実施の形態に係る位置ずれ補正装置３０の作用＞
図９は、本発明の第３の実施の形態に係る点群の対応付け処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る点群の対応付け処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。

ステップＳ４００において、第１有効距離判定部３３２は、データ点ｐ_ｉとモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）の各々について、当該データ点ｐ_ｉと当該モデル点ｑ_ｊとの距離を算出する。

ステップＳ４１０において、第１有効距離判定部３３２は、対応付け（ｐ_ｉ，ｑ_ｊ）の各々についての当該データ点ｐ_ｉと当該モデル点ｑ_ｊとの距離が、第１閾値以上であるか否かを判定する。

距離が第１閾値以上である場合（上記ステップＳ４１０のＹＥＳ）ステップＳ４２０において、第１有効距離判定部３３２は、当該対応付け（ｐ_ｉ，ｑ_ｊ）を無効なものとし、ステップＳ１４２に進む。

一方、距離が第１閾値以上でない場合（上記ステップＳ４１０のＹＥＳ）、ステップＳ１４２に進む。

以上説明したように、本発明の第３の実施の形態に係る位置ずれ補正装置によれば、対応付けられたデータ点の座標とモデル点の座標との距離が予め定めた第１閾値以上である場合に、対応付けを無効化するため、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度をより高めることができる。

＜本発明の第４の実施の形態に係る位置ずれ補正装置４０の構成＞
図１０を用いて、本発明の第４の実施の形態に係る位置ずれ補正装置４０の構成について説明する。なお、第１の実施の形態に係る位置ずれ補正装置１０、第２の実施の形態に係る位置ずれ補正装置２０及び第３の実施の形態に係る位置ずれ補正装置３０と同様の構成については、同一の符号を付して詳細な説明は省略する。

位置ずれ補正装置４０は、ＣＰＵと、ＲＡＭと、後述する位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

図１０に示すように、本発明の実施の形態に係る位置ずれ補正装置４０は、入力部１００と、モデル点群記憶部１１０と、取得部２２０と、モデル点対応部１３０と、データ点対応部２３５と、第１有効距離判定部３３２と、第２有効距離判定部４３７と、重み設定部２４０と、変換行列算出部１５０と、判定部１６０と、座標変換部２７０と、出力部１８０とを備えて構成される。

本実施形態では、モデル点対応部１３０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）を、第１有効距離判定部３３２に渡す。

また、データ点対応部２３５は、モデル点群Ｑの各モデル点ｑ_ｊの各々について、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）を、第２有効距離判定部４３７に渡す。

第２有効距離判定部４３７は、データ点対応部２３５により対応付けられたモデル点ｐ_ｉの座標とデータ点ｐ_ｉの座標との距離が予め定めた第２閾値以上である場合に、対応付けを無効化する。

具体的には、第２有効距離判定部４３７は、まず、モデル点ｑ_ｊとデータ点ｐ_ｉとの対応付け（ｐ_ｉ，ｑ_ｊ）の各々について、当該モデル点ｑ_ｊと当該データ点ｐ_ｉとの距離を算出する。

次に、第２有効距離判定部４３７は、対応付け（ｐ_ｉ，ｑ_ｊ）の各々についての当該モデル点ｑ_ｊと当該データ点ｐ_ｉとの距離が、第２閾値以上であるか否かを判定する。

距離が第２閾値以上である場合、第２有効距離判定部４３７は、当該対応付け（ｐ_ｉ，ｑ_ｊ）を無効なものとし、以降の処理では用いないこととする。

一方、距離が第２閾値以上でない場合、第２有効距離判定部４３７は、当該対応付け（ｐ_ｉ，ｑ_ｊ）を有効なものとする。

そして、第２有効距離判定部４３７は、モデル点群Ｑに含まれるモデル点ｐ_ｉの各々について、当該モデル点ｑ_ｊと当該モデル点ｑ_ｊの最近傍点であるデータ点ｐ_ｉとの対応付け（ｐ_ｉ，ｑ_ｊ）のうち有効な対応付け（ｐ_ｉ，ｑ_ｊ）を、重み設定部２４０に渡す。

＜本発明の第４の実施の形態に係る位置ずれ補正装置４０の作用＞
図１１は、本発明の第４の実施の形態に係る点群の対応付け処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る点群の対応付け処理ルーチン、第２の実施の形態に係る点群の対応付け処理ルーチン、及び第３の実施の形態に係る点群の対応付け処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。

ステップＳ５１０において、第２有効距離判定部４３７は、モデル点ｑ_ｊとデータ点ｐ_ｉとの対応付け（ｐ_ｉ，ｑ_ｊ）の各々について、当該モデル点ｑ_ｊと当該データ点ｐ_ｉとの距離を算出する。

ステップＳ５２０において、第２有効距離判定部４３７は、対応付け（ｐ_ｉ，ｑ_ｊ）の各々についての当該モデル点ｑ_ｊと当該データ点ｐ_ｉとの距離が、第２閾値以上であるか否かを判定する。

距離が第２閾値以上である場合（上記ステップＳ５２０のＹＥＳ）ステップＳ５３０において、第２有効距離判定部４３７は、当該対応付け（ｐ_ｉ，ｑ_ｊ）を無効なものとし、ステップＳ３０５に進む。

一方、距離が第２閾値以上でない場合（上記ステップＳ５２０のＹＥＳ）、ステップＳ３０５に進む。

以上説明したように、本発明の第４の実施の形態に係る位置ずれ補正装置によれば、モデル点群に含まれるモデル点の各々について、当該モデル点と、当該モデル点に対応付けられたデータ点との距離が予め定めた第２閾値以上か否かを判定し、当該距離が第２閾値以上である場合に、当該モデル点に対する対応付けを無効化するため、位置合わせの初期段階での誤った対応付けを抑制し、位置合わせの精度をより高めることができる。

なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

例えば、上記実施の形態では、３次元の点群の場合について説明したが、これに限定されるものではない。２次元の点群を用いる構成としてもよい。

また、重み設定部１４０（２４０）は、対応付けられたデータ点群Ｐのデータ点ｐ_ｉとモデル点群Ｑのモデル点ｑ_ｊとの位置ずれが予め定めた第３閾値以下となる場合、又は判定部１６０による繰り返し回数が予め定めた第４閾値以上となる場合、重みの設定方法を変更する。

また、判定部１６０が、予め定めた条件を満たさないと判定した場合、判定結果に基づいて、重み設定部１４０（２４０）に対し、重みの設定方法を変更するように指示する構成としてもよい。

具体的には、判定部１６０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々についての重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）の位置ずれの平均値が予め定めた第３閾値以下となる場合、又はカウンタｓが予め定めた第４閾値以上となる場合のうち、予め定めた条件でない方の条件を満たした場合に、重み設定部１４０に対し、重みの設定方法（Ｗ１及びＷ２の値）を変更するように指示する。例えば、Ｗ２の値を大きくするように変更してもよい。

点群に欠落がある場合、対応付けが無効化されたままとなってしまう場合があるため、重みの値を変更することで、位置合わせへの寄与を変更し、精度を向上させるためである。

また、重み設定部１４０及び重み設定部２４０は、重みを予め定めたＷ２に変更する構成としたが、これに限定されるものではない。

例えば、同一のモデル点ｑ_ｊに対し、複数のデータ点ｐ_ｉが対応している場合、又は同一のデータ点ｐ_ｉに対し、複数のモデル点ｑ_ｊが対応している場合には、ｗ_ｉを１０で割った値とする、重複数に応じた数で割った値とする等の方法により変更する構成としてもよい。

また、重み設定部２４０は、同一のモデル点ｑ_ｊに対し、複数のデータ点ｐ_ｉが対応している場合、又は同一のデータ点ｐ_ｉに対し、複数のモデル点ｑ_ｊが対応している場合のみ、重みの変更をする構成としたが、これに限定されるものではない。

例えば、重み設定部２４０は、データ点群Ｐに含まれるデータ点ｐ_ｉの各々について、モデル点ｑ_ｊとの対応付けを行った結果、当該データ点ｐ_ｉと当該データ点ｐ_ｉの最近傍点であるモデル点ｑ_ｊとの対応付け（ｐ_ｉ，ｑ_ｊ）がある場合に、モデル点群Ｑに含まれるモデル点ｑ_ｊの各々について、データ点ｐ_ｉとの対応付けを行った結果、当該モデル点ｑ_ｊと対応付けられたデータ点がデータ点ｐ_ｉと異なる場合に、対応付けに不一致があると判定し、重み設定部２４０は、当該不一致となる対応付け（ｐ_ｉ，ｑ_ｊ）について、重みｗ_ｉ＝Ｗ３を付与し、重み付き対応付け（ｐ_ｉ，ｑ_ｊ，ｗ_ｉ）としてもよい。ここで、Ｗ３は予め定めた定数である。

また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。

１０ 補正装置
２０ 補正装置
３０ 補正装置
４０ 補正装置
１００ 入力部
１１０ モデル点群記憶部
１２０ 取得部
１３０ モデル点対応部
１４０ 重み設定部
１５０ 変換行列算出部
１６０ 判定部
１７０ 座標変換部
１８０ 出力部
２２０ 取得部
２３５ データ点対応部
２４０ 重み設定部
２７０ 座標変換部
３３２ 第１有効距離判定部
４３７ 第２有効距離判定部

Claims (8)

1. 位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付ける入力部と、
   位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得する取得部と、
   前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行うモデル点対応部と、
   同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定する重み設定部と、
   前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出する変換行列算出部と、
   前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換する座標変換部と、
   予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる判定部と、
   を含む位置ずれ補正装置。
2. 前記変換行列算出部は、対応付けられた前記データ点の変換後の座標と前記モデル点の座標との距離、及び前記データ点と前記モデル点との対応付けに対する重みを用いて求められる、重み付きの距離の二乗和が最小となるように、前記座標変換行列を算出する
   請求項１記載の位置ずれ補正装置。
3. 前記重み設定部は、一つの前記モデル点に対し、一つの前記データ点のみが対応付けられた場合に、前記モデル点と前記データ点との対応付けに対して一定の重みを設定し、同一の前記モデル点に対し、複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して、前記一定の重みより低い重みを設定する
   請求項１又は２記載の位置ずれ補正装置。
4. 前記モデル点の座標及び前記データ点の座標に基づいて、前記モデル点群に含まれるモデル点の各々について、前記データ点群に含まれるデータ点との対応付けを行うデータ点対応部
   を更に含み、
   前記重み設定部は、同一の前記データ点に対し、前記モデル点群に含まれる複数のモデル点が対応付けられた場合に、同一の前記データ点と前記複数のモデル点の各々との対応付けの各々に対して所定の重みを設定し、
   前記判定部は、予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部による対応付け、前記データ点対応部による対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる
   請求項１乃至３の何れか１項記載の位置ずれ補正装置。
5. 前記モデル点対応部により対応付けられた前記データ点の座標と前記モデル点の座標との距離が予め定めた第１閾値以上である場合に、対応付けを無効化する第１有効距離判定部
   請求項１乃至３の何れか１項記載の位置ずれ補正装置。
6. 前記データ点対応部により対応付けられた前記モデル点の座標と前記データ点の座標との距離が予め定めた第２閾値以上である場合に、対応付けを無効化する第２有効距離判定部
   請求項４記載の位置ずれ補正装置。
7. 前記重み設定部は、対応付けられた前記データ点群のデータ点と前記モデル点群のモデル点との位置ずれが予め定めた第３閾値以下となる場合、又は前記判定部による繰り返し回数が予め定めた第４閾値以上となる場合、重みの設定方法を変更する
   請求項１乃至４の何れか１項記載の位置ずれ補正装置。
8. コンピュータを、
   位置合わせの対象となるデータ点の座標の集合であるデータ点群の入力を受け付ける入力部、
   位置合わせの基準となるモデル点の座標の集合であるモデル点群を取得する取得部、
   前記データ点の座標及び前記モデル点の座標に基づいて、前記データ点群に含まれるデータ点の各々について、前記モデル点群に含まれるモデル点との対応付けを行うモデル点対応部、
   同一の前記モデル点に対し、前記データ点群に含まれる複数のデータ点が対応付けられた場合に、同一の前記モデル点と前記複数のデータ点の各々との対応付けの各々に対して所定の重みを設定する重み設定部、
   前記データ点群と前記モデル点群との対応付け、及び前記設定された前記所定の重みに基づいて、前記データ点群に含まれる前記データ点の座標を変換した場合に、変換後の前記データ点群と前記モデル点群との位置ずれが最小となる座標変換行列を算出する変換行列算出部、
   前記座標変換行列に基づいて、前記データ点群に含まれる前記データ点の座標を変換する座標変換部、及び
   予め定めた条件を満たすと判定されるまで、前記座標変換部による変換、前記モデル点対応部により対応付け、前記重み設定部による設定、及び前記変換行列算出部による算出を繰り返させる判定部
   として機能させるためのプログラム。

Images