JP2020186944A - 位置姿勢推定装置及び位置姿勢推定方法 - Google Patents

Abstract

【課題】高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる位置姿勢推定装置を提供する。【解決手段】位置姿勢推定装置１００は、測定器（トータルステーション３）により測定された位置を示す位置情報を取得する位置情報取得部１１と、撮像機（ステレオカメラ４）により撮像された画像を示す画像情報を取得する画像情報取得部１２と、位置情報及び画像情報を用いて反射材（プリズム２）の姿勢を算出することにより、反射材（プリズム２）の姿勢を示す姿勢情報を生成する姿勢情報生成部１３と、位置情報及び姿勢情報を用いて移動体（台車１）の位置及び移動体（台車１）の姿勢を算出する位置姿勢算出部１４と、を備える。【選択図】図３

Description

本発明は、位置姿勢推定装置及び位置姿勢推定方法に関する。

従来、移動体（例えば車両又は航空機）を用いて測量対象物（例えば地面又は構造物）の三次元形状を測定する測量装置が開発されている。

すなわち、測量対象物に沿うように移動体が移動しているとき、測量対象物撮像用の撮像機（例えばデジタルカメラ）により測量対象物が複数回撮像される。また、このとき、位置測定用の測定器（例えばトータルステーション）により移動体の位置が複数回測定される。また、このとき、姿勢検出用のセンサ（例えばジャイロセンサ）により移動体の姿勢が複数回検出される。当該撮像された画像、当該測定された位置、及び当該検出された姿勢に基づき、測量対象物の三次元形状が測定される。測量対象物撮像用の撮像機及び姿勢検出用のセンサは、移動体に設けられている（例えば、特許文献１参照。）。

または、測量対象物に沿うように移動体が移動しているとき、距離測定用の測定器（例えばレーザスキャナ）により移動体と測量対象物間の距離が複数回測定される。また、このとき、位置測定用の測定器により移動体の位置が複数回測定される。また、このとき、姿勢検出用のセンサにより移動体の姿勢が複数回検出される。当該測定された距離、当該測定された位置、及び当該検出された姿勢に基づき、測量対象物の三次元形状が測定される。距離測定用の測定器及び姿勢検出用のセンサは、移動体に設けられている（例えば、特許文献２参照。）。

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

高精度な測量を実現する観点から、移動体の姿勢を高精度に検出することが要求される。ここで、高精度なセンサを用いることにより、移動体の姿勢を高精度に検出することが考えられる。しかしながら、通常、高精度なセンサは高価である。

これに対して、特許文献２には、距離測定用の測定器（距離計測部２０）と異なる撮像機（画像取得部３０）により撮像された画像を用いて、姿勢検出用のセンサ（慣性計測部４０）による検出値を補正する技術が開示されている。これは、移動体の姿勢検出の精度向上を図るものである。しかしながら、かかる画像を用いた検出値の補正による精度向上には限界がある。

本発明は、上記のような課題を解決するためになされたものであり、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる位置姿勢推定装置を提供することを目的とする。

本発明の位置姿勢推定装置は、位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定装置であって、測定器により測定された位置を示す位置情報を取得する位置情報取得部と、撮像機により撮像された画像を示す画像情報を取得する画像情報取得部と、位置情報及び画像情報を用いて反射材の姿勢を算出することにより、反射材の姿勢を示す姿勢情報を生成する姿勢情報生成部と、位置情報及び姿勢情報を用いて移動体の位置及び移動体の姿勢を算出する位置姿勢算出部と、を備えるものである。

本発明によれば、上記のように構成したので、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる。

実施の形態１に係る位置姿勢推定装置を含む測量装置の要部を示す斜視図である。 実施の形態１に係る位置姿勢推定装置を含む測量装置の要部を示すブロック図である。 実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の要部を示すブロック図である。 実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機のハードウェア構成を示す説明図である。 実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の他のハードウェア構成を示す説明図である。 実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の動作を示すフローチャートである。 実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の詳細な動作を示すフローチャートである。 位置情報の例を示す説明図である。 画像情報の例を示す説明図である。 各時刻にて撮像された左眼画像における個々のタイポイントの画像座標の例を示す説明図である。 各時刻にて撮像された右眼画像における個々のタイポイントの画像座標の例を示す説明図である。 個々のタイポイントの三次元座標の初期値の例を示す説明図である。 各時刻におけるプリズムの姿勢角度の初期値の例を示す説明図である。 右眼カメラ及び右眼画像に関するパラメータの例を示す説明図である。 左眼カメラ及び左眼画像に関するパラメータの例を示す説明図である。 図１０に示す値を整形することにより得られた値を示す説明図である。 図１４に示す値を用いた場合における差分値の算出結果を示す説明図である。 図１４に示す値を用いた場合における勾配行列の算出結果を示す説明図である。 図１５に示す差分値の算出結果及び図１６に示す勾配行例の算出結果に基づく補正量の算出結果を示す説明図である。 第１最適化処理による最適化後の三次元座標の例を示す説明図である。 図１８に示す値及び図１１に示す値を整形することにより得られた値を示す説明図である。 図１９に示す値を用いた場合における差分値の算出結果を示す説明図である。 図１９に示す値を用いた場合における勾配行列の算出結果を示す説明図である。 図２０に示す差分値の算出結果及び図２１に示す勾配行例の算出結果に基づく補正量の算出結果を示す説明図である。 第２最適化処理による最適化後の三次元座標の例、及び第２最適化処理による最適化後の姿勢角度の例を示す説明図である。 各時刻における台車の三次元座標の例、及び各時刻における台車の姿勢角度の例を示す説明図である。 各時刻における台車の三次元座標の他の例、及び各時刻における台車の姿勢角度の他の例を示す説明図である。

実施の形態１．
図１は、実施の形態１に係る位置姿勢推定装置を含む測量装置の要部を示す斜視図である。図２は、実施の形態１に係る位置姿勢推定装置を含む測量装置の要部を示すブロック図である。図３は、実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の要部を示すブロック図である。図１〜図３を参照して、実施の形態１に係る位置姿勢推定装置を含む測量装置について説明する。

測量装置２００は、測量用の移動体を有している。移動体は、例えば、台車１により構成されている。

台車１に位置測定用の反射材が設けられている。反射材は、例えば、全周型のプリズム２により構成されている。また、測量装置２００は、位置測定用の測定器を有している。測定器は、例えば、自動追尾型のトータルステーション３により構成されている。すなわち、トータルステーション３は、プリズム２に位置測定用の光を照射する。プリズム２は、当該照射された光を反射する。トータルステーション３は、当該反射された光を受信する。これにより、プリズム２の位置が測定される。トータルステーション３は、プリズム２を追尾する機能を有している。

台車１に撮像機が設けられている。撮像機は、例えば、ステレオカメラ４により構成されている。すなわち、ステレオカメラ４は、右眼カメラ５及び左眼カメラ６を有している。右眼カメラ５による視野は、左眼カメラ６による視野と重複している。右眼カメラ５及び左眼カメラ６の各々は、デジタルカメラにより構成されている。ステレオカメラ４は、任意の所定の領域（以下「撮像対象領域」という。）を撮像するものである。撮像対象領域は、例えば、測量装置２００による測量対象物を含む領域である。測量対象物は、例えば、地面である。以下、測量対象物を含む領域が撮像対象領域に設定されており、かつ、測量対象物が地面である場合の例を中心に説明する。

台車１が走行しているとき、トータルステーション３がプリズム２の位置を複数回測定する。記憶装置７は、当該測定された位置を示す情報（以下「位置情報」という。）を記憶するものである。また、このとき、トータルステーション３による測定タイミングと同期された撮像タイミングにて、右眼カメラ５及び左眼カメラ６の各々が撮像対象領域を複数回撮像する。記憶装置７は、当該撮像された画像を示す情報（以下「画像情報」という。）を記憶するものである。記憶装置７は、例えば、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）により構成されている。

以下、トータルステーション３による測定タイミング及びステレオカメラ４による撮像タイミングを総称して単に「タイミング」ということがある。また、右眼カメラ５により撮像された画像を「右眼画像」という。また、左眼カメラ６により撮像された画像を「左眼画像」という。また、右眼画像及び左眼画像を総称して「ステレオ画像」又は「デジタル画像」ということがある。

位置情報取得部１１は、記憶装置７に記憶されている位置情報を取得するものである。画像情報取得部１２は、記憶装置７に記憶されている画像情報を取得するものである。

姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報及び画像情報取得部１２により取得された画像情報を用いて、各タイミングにおけるプリズム２の姿勢を算出するものである。姿勢情報生成部１３は、当該算出された姿勢を示す情報（以下「姿勢情報」という。）を生成するものである。

ここで、姿勢情報生成部１３は、デジタル画像における接合点（以下「タイポイント」という。）の位置に基づき、プリズム２の姿勢を算出するようになっている。

個々のタイポイントは、例えば、撮像対象領域における地点に対応するものである。この場合、個々の地点の位置は、測量装置２００において未知であっても良い。または、例えば、個々のタイポイントは、撮像対象領域に設けられたマーカに対応するものである。この場合、個々のマーカの位置は、測量装置２００において既知であっても良く、又は測量装置２００において未知であっても良い。

以下、個々のタイポイントが地点に対応するものである場合において、かかる地点の位置が未知であるとき、かかる地点を「未知の地点」ということがある。また、個々のタイポイントがマーカに対応するものである場合において、かかるマーカの位置が既知であるとき、かかるマーカを「既知のマーカ」ということがある。また、個々のタイポイントがマーカに対応するものである場合において、かかるマーカの位置が未知であるとき、かかるマーカを「未知のマーカ」ということがある。

位置姿勢算出部１４は、位置情報取得部１１により取得された位置情報及び姿勢情報生成部１３により生成された姿勢情報を用いて、各タイミングにおける台車１の位置及び台車１の姿勢を算出するものである。

三次元形状算出部１５は、画像情報取得部１２により取得された画像情報を用いて、位置姿勢算出部１４により算出された位置及び姿勢に基づき、測量対象物の三次元形状を算出するものである。具体的には、例えば、三次元形状算出部１５は、いわゆる「多視点ステレオ」により測量対象物の三次元形状を算出する。多視点ステレオには、公知の種々の技術を用いることができる。これらの技術についての詳細な説明は省略する。

位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５により、計算機８の要部が構成されている。また、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３及び位置姿勢算出部１４により、位置姿勢推定装置１００の要部が構成されている。

このようにして、測量装置２００の要部が構成されている。

次に、図４を参照して、計算機８の要部のハードウェア構成について説明する。

図４Ａに示す如く、計算機８は、プロセッサ２１及びメモリ２２を有している。メモリ２２には、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５の機能を実現するためのプログラムが記憶されている。当該記憶されているプログラムをプロセッサ２１が読み出して実行することにより、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５の機能が実現される。

または、図４Ｂに示す如く、計算機８は、処理回路２３を有している。この場合、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５の機能が専用の処理回路２３により実現される。

または、計算機８は、プロセッサ２１、メモリ２２及び処理回路２３を有している（不図示）。この場合、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５の機能のうちの一部の機能がプロセッサ２１及びメモリ２２により実現されて、残余の機能が専用の処理回路２３により実現される。

プロセッサ２１は、１個又は複数個のプロセッサにより構成されている。個々のプロセッサは、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、マイクロプロセッサ、マイクロコントローラ又はＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）を用いたものである。

メモリ２２は、１個又は複数個の不揮発性メモリにより構成されている。または、メモリ２２は、１個又は複数個の不揮発性メモリ及び１個又は複数個の揮発性メモリにより構成されている。個々の揮発性メモリは、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）を用いたものである。個々の不揮発性メモリは、例えば、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ−Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＳＳＤ又はＨＤＤを用いたものである。

処理回路２３は、１個又は複数個のデジタル回路により構成されている。または、処理回路２３は、１個又は複数個のデジタル回路及び１個又は複数個のアナログ回路により構成されている。すなわち、処理回路２３は、１個又は複数個の処理回路により構成されている。個々の処理回路は、例えば、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、ＳｏＣ（Ｓｙｓｔｅｍ−ｏｎ−ａ−Ｃｈｉｐ）又はシステムＬＳＩ（Ｌａｒｇｅ−Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ）を用いたものである。

次に、位置姿勢推定装置１００において使用される座標系について説明する。また、これらの座標系における位置座標について説明する。

世界座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。世界座標系におけるＸＹ平面は、地面に対して平行又は略平行である。世界座標系におけるＺ軸の正方向は、天頂方向を示している。

プリズム座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。プリズム座標系は、プリズム２を基準とする座標系である。プリズム座標系におけるＸＹ平面は、台車１が地面に載置されたとき、地面に対して平行又は略平行となる。プリズム座標系におけるＸ軸の正方向は、台車１が地面に載置されたとき、台車１の正面方向を示すものとなる。プリズム座標系におけるＺ軸の正方向は、台車１が地面に載置されたとき、天頂方向を示すものとなる。

右眼カメラ座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。右眼カメラ座標系の原点は、右眼カメラ５の投影中心に対応している。右眼カメラ座標系におけるＸＹ平面は、右眼カメラ５の投影面に対して平行である。右眼カメラ座標系におけるＸ軸の正方向は、右眼カメラ５の投影面における上方向に対応している。右眼カメラ座標系におけるＹ軸の正方向は、右眼カメラ５の投影面における左方向に対応している。

左眼カメラ座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。左眼カメラ座標系の原点は、左眼カメラ６の投影中心に対応している。左眼カメラ座標系におけるＸＹ平面は、左眼カメラ６の投影面に対して平行である。左眼カメラ座標系におけるＸ軸の正方向は、左眼カメラ６の投影面における上方向に対応している。左眼カメラ座標系におけるＹ軸の正方向は、左眼カメラ６の投影面における左方向に対応している。

台車座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する任意の三次元座標系である。台車座標系は、台車１を基準とする座標系である。台車座標系は、測量装置２００の用途等に応じて設定されるものである。具体的には、例えば、台車座標系は、右眼カメラ座標系と一致する座標系に設定される。または、例えば、台車座標系は、左眼カメラ座標系と一致する座標系に設定される。

ここで、プリズム座標系の原点は、プリズム２の位置に対応している。したがって、世界座標系におけるプリズム座標系の原点の位置を示す座標は、世界座標系におけるプリズム２の位置を示すものである。また、台車座標系の原点は、台車１の位置に対応している。したがって、世界座標系における台車座標系の原点の位置を示す座標は、世界座標系における台車１の位置を示すものである。

以下、世界座標系、プリズム座標系、右眼カメラ座標系、左眼カメラ座標系又は台車座標系における位置座標を「三次元座標」という。特に、世界座標系における個々のタイポイントの三次元座標を「タイポイント位置」ということがある。また、右眼カメラ５の投影面に沿う二次元座標系、又は左眼カメラ６の投影面に沿う二次元座標系における位置座標を「投影座標」という。

デジタル画像座標系は、互いに直交するＵ軸及びＶ軸を有する画像座標系である。デジタル画像座標系における原点は、個々のデジタル画像における左上の隅に対応している。デジタル画像座標系におけるＵ軸の正方向は、個々のデジタル画像における右方向に対応している。デジタル画像座標系におけるＶ軸の正方向は、個々のデジタル画像における下方向に対応している。

正規化画像座標系は、互いに直交するＵ軸及びＶ軸を有する画像座標系である。正規化画像座標系における原点は、個々のデジタル画像における中心に対応している。正規化画像座標系におけるＵ軸の正方向は、個々のデジタル画像における上方向に対応している。正規化画像座標系におけるＶ軸の正方向は、個々のデジタル画像における左方向に対応している。

以下、デジタル画像座標系又は正規化画像座標系における位置座標を「画像座標」という。特に、デジタル画像座標系又は正規化画像座標系における個々のタイポイントの画像座標を「タイポイント座標」ということがある。

次に、図５のフローチャートを参照して、計算機８の動作について説明する。

なお、ステップＳＴ１の処理が実行されるよりも先に、トータルステーション３がプリズム２の位置を複数回測定して、記憶装置７に位置情報が記憶されているものとする。また、ステレオカメラ４が撮像対象領域を複数回撮像して、記憶装置７に画像情報が記憶されているものとする。

まず、ステップＳＴ１にて、位置情報取得部１１は、記憶装置７に記憶されている位置情報を取得する。当該取得された位置情報は、各タイミングにおける世界座標系におけるプリズム２の三次元座標（ｘ_ｐ，ｙ_ｐ，ｚ_ｐ）を示している。

次いで、ステップＳＴ２にて、画像情報取得部１２は、記憶装置７に記憶されている画像情報を取得する。当該取得された画像情報は、各タイミングにて撮像された右眼画像及び左眼画像を示している。

次いで、ステップＳＴ３にて、姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報及び画像情報取得部１２により取得された画像情報を用いて、各タイミングにおけるプリズム２の姿勢を算出する。より具体的には、姿勢情報生成部１３は、各タイミングにおけるプリズム２の姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の値を算出する。これにより、当該算出された姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の値を示す姿勢情報が生成される。

ここで、姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）は、世界座標系の座標軸に対するプリズム座標系の座標軸の回転角に対応している。すなわち、姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）は、オイラー角を示している。より具体的には、ω_ｐがロール角を示しており、φ_ｐがピッチ角を示しており、κ_ｐがヨー角を示している。

次いで、ステップＳＴ４にて、位置姿勢算出部１４は、位置情報取得部１１により取得された位置情報及び姿勢情報生成部１３により生成された姿勢情報を用いて、各タイミングにおける台車１の位置及び台車１の姿勢を算出する。より具体的には、位置姿勢算出部１４は、各タイミングにおける世界座標系における台車１の三次元座標（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）の値を算出する。また、位置姿勢算出部１４は、各タイミングにおける台車１の姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）の値を算出する。

ここで、姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）は、世界座標系の座標軸に対する台車座標系の座標軸の回転角に対応している。すなわち、姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）は、オイラー角を示している。より具体的には、ω_ｔがロール角を示しており、φ_ｔがピッチ角を示しており、κ_ｔがヨー角を示している。

次いで、ステップＳＴ５にて、三次元形状算出部１５は、画像情報取得部１２により取得された画像情報を用いて、位置姿勢算出部１４により算出された位置及び姿勢に基づき、測量対象物の三次元形状を算出する。具体的には、例えば、三次元形状算出部１５は、多視点ステレオにより測量対象物の三次元形状を算出する。

次に、図６のフローチャートを参照して、姿勢情報生成部１３の詳細な動作について説明する。すなわち、ステップＳＴ３の詳細な処理内容について説明する。

まず、ステップＳＴ１１にて、姿勢情報生成部１３は、各タイミングにて撮像された右眼画像及び左眼画像の各々における、デジタル画像座標系における個々のタイポイントの画像座標（ｕ，ｖ）の値を算出する。ステップＳＴ１１の処理には、画像情報取得部１２により取得された画像情報が用いられる。

次いで、ステップＳＴ１２にて、姿勢情報生成部１３は、世界座標系における個々のタイポイントの三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の初期値を算出する。ステップＳＴ１２の処理には、位置情報取得部１１により取得された位置情報が用いられる。

次いで、ステップＳＴ１３にて、姿勢情報生成部１３は、各タイミングにおけるプリズム２の姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の初期値を算出する。ステップＳＴ１３の処理には、位置情報取得部１１により取得された位置情報が用いられる。

次いで、ステップＳＴ１４にて、姿勢情報生成部１３は、ステップＳＴ１２における初期値に比して最適化された三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の値を算出する処理（以下「第１最適化処理」という。）を実行する。

次いで、ステップＳＴ１５にて、姿勢情報生成部１３は、ステップＳＴ１４にて算出された三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）に比して更に最適化された三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の値を算出するとともに、ステップＳＴ１３における初期値に比して最適化された姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の値を算出する処理（以下「第２最適化処理」という。）を実行する。これにより、第２最適化処理による最適化後の姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の値を示す姿勢情報が生成される。

次に、図７を参照して、ステップＳＴ１における、位置情報取得部１１により取得される位置情報の具体例について説明する。また、図８を参照して、ステップＳＴ２における、画像情報取得部１２により取得される画像情報の具体例について説明する。

上記のとおり、台車１が走行しているとき、トータルステーション３がプリズム２の位置を複数回測定するとともに、ステレオカメラ４が撮像対象領域を複数回撮像する。このとき、台車１の進行方向は、台車１の正面方向に対して同一又は略同一の方向に設定される。

図７は、プリズム２の位置が４回測定された場合における位置情報の例を示している。図８は、撮像対象領域が４回撮像された場合における画像情報の例を示している。図中、ｔ０〜ｔ３は、トータルステーション３による測定タイミングに対応する時刻、すなわちステレオカメラ４による撮像タイミングに対応する時刻を示している。

図７に示す位置情報は、各時刻ｔ０〜ｔ３における世界座標系におけるプリズム２の三次元座標（ｘ_ｐ，ｙ_ｐ，ｚ_ｐ）を示している。また、図８に示す画像情報は、各時刻ｔ０〜ｔ３にて撮像された右眼画像及び左眼画像を示している。

次に、図９を参照して、ステップＳＴ１１における、姿勢情報生成部１３による個々のタイポイントの画像座標（ｕ，ｖ）の値の算出方法の具体例について説明する。

上記のとおり、個々のタイポイントは、例えば、未知の地点、既知のマーカ又は未知のマーカに対応するものである。

個々のタイポイントが未知の地点に対応するものである場合、例えば、姿勢情報生成部１３は、個々のデジタル画像に対する画像処理を実行する。これにより、姿勢情報生成部１３は、複数枚のデジタル画像に共通して含まれる特徴点を抽出する。姿勢情報生成部１３は、当該抽出された特徴点をタイポイントに用いる。すなわち、姿勢情報生成部１３は、当該抽出された特徴点の画像座標（ｕ，ｖ）の値を算出する。

かかる特徴点の抽出には、公知の種々の技術を用いることができる。これらの技術についての詳細な説明は省略する。具体的には、例えば、以下の参考文献１に記載された技術を用いることができる。

［参考文献１］
David G. Lowe, "Distinctive Image Features from Scale-Invariant Keypoints," International Journal of Computer Vision, 2004, 60(2), pp. 91-110.

個々のタイポイントが既知のマーカ又は未知のマーカに対応するものである場合、例えば、姿勢情報生成部１３は、個々のデジタル画像に対して、いわゆる「テンプレートマッチング」の画像処理を実行する。これにより、姿勢情報生成部１３は、個々のデジタル画像に含まれるマーカを検出する。姿勢情報生成部１３は、当該検出されたマーカの画像座標（ｕ，ｖ）の値を算出する。

または、例えば、個々のタイポイントが既知のマーカ又は未知のマーカである場合、測量装置２００のユーザは、個々のデジタル画像におけるマーカの位置を目視により確認して、当該確認された位置を計算機８に入力する。姿勢情報生成部１３は、当該入力された位置に対応する画像座標（ｕ，ｖ）の値を算出する。

図９は、個々のタイポイントが未知のマーカに対応するものである場合における、個々のタイポイントの画像座標（ｕ，ｖ）の値の例を示している。図９に示す値は、図８に示す画像情報を用いて算出されたものである。

次に、図１０を参照して、ステップＳＴ１２における、姿勢情報生成部１３による個々のタイポイントの三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の初期値の算出方法の具体例について説明する。

姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報に含まれる全てのＸ座標（ｘ_ｐ）の平均値を算出する。姿勢情報生成部１３は、当該算出された平均値を各タイポイントのＸ座標（ｘ_Ｔ）の初期値に用いる。

また、姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報に含まれる全てのＹ座標（ｙ_ｐ）の平均値を算出する。姿勢情報生成部１３は、当該算出された平均値を各タイポイントのＹ座標（ｙ_Ｔ）の初期値に用いる。

また、姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報に含まれるＺ座標（ｚ_ｐ）の値を用いて、地面における任意の地点のＺ座標の値を算出する。姿勢情報生成部１３は、当該算出されたＺ座標の値を各タイポイントのＺ座標（ｚ_Ｔ）の初期値に用いる。

なお、地面における任意の地点のＺ座標の値を算出するために、全周型のプリズム２と異なる他のプリズム（不図示）が台車１に設けられているものであっても良い。姿勢情報生成部１３は、トータルステーション３により測定された当該他のプリズムの位置に基づき、地面における任意の地点のＺ座標の値を算出するものであっても良い。また、当該他のプリズムは、台車１に設けられているのに代えて、測量装置２００のユーザの手により保持されているものであっても良い。

このようにして、世界座標系における個々のタイポイントの三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の初期値が算出される。図１０は、上記他のプリズムを用いた場合における、個々のタイポイントの三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の初期値の例を示している。図１０に示す初期値は、図７に示す位置情報を用いて算出されたものである。

次に、図１１を参照して、ステップＳＴ１３における、姿勢情報生成部１３によるプリズム２の姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の初期値の算出方法の具体例について説明する。

時刻ｔにおけるプリズム２の三次元座標が（ｘ，ｙ，ｚ）であるものとする。時刻ｔに対応する測定タイミングに対する次回の測定タイミングに対応する時刻がｔ’であるものとする。時刻ｔ’におけるプリズム２の三次元座標が（ｘ’，ｙ’，ｚ’）であるものとする。

上記のとおり、プリズム座標系におけるＸ軸の正方向は、台車１が地面に載置されたとき、台車１の正面方向を示すものとなる。また、台車１の進行方向は、台車１の正面方向と同一又は略同一の方向に設定される。したがって、プリズム２の姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）は、Ｘ座標（１，０，０）を台車１の移動ベクトル（ｘ−ｘ’，ｙ−ｙ’，ｚ−ｚ’）と一致させる回転角により近似される。

ここで、台車１は、世界座標系におけるＸＹ平面に沿うように移動する。このため、ロール角（ω_ｐ）及びピッチ角（φ_ｐ）の各々は、０に近似される。また、ヨー角（κ_ｐ）は、ＸＹ平面におけるベクトル（１，０）をベクトル（ｘ−ｘ’，ｙ−ｙ’）と一致させる回転角により近似される。したがって、ヨー角（κ_ｐ）について、以下の式（１）が成立する。

ヨー角（κ_ｐ）は、上記式（１）により算出される。すなわち、ヨー角（κ_ｐ）は、逆三角関数により算出される。

なお、時刻ｔに対応する測定タイミングに対する前回の測定タイミングに対応する時刻がｔ”であるものとする。時刻ｔよりも後の時刻におけるプリズム２の位置が未測定である場合、時刻ｔにおける姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の初期値に、時刻ｔ”における姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の初期値が用いられる。時刻ｔ”における姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の初期値は、時刻ｔにおけるプリズム２の三次元座標と、時刻ｔ”におけるプリズム２の三次元座標とを用いて算出される。

図１１は、各時刻ｔ０〜ｔ３におけるプリズム２の姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の初期値の例を示している。図１１に示す初期値は、図７に示す位置情報を用いて算出されたものである。

次に、図１２〜図１８を参照して、ステップＳＴ１４における、姿勢情報生成部１３による第１最適化処理の具体例について説明する。

まず、姿勢情報生成部１３は、以下のようにして、右眼カメラ５における個々のタイポイントの投影座標（^０ｕ_ｒ，^０ｖ_ｒ）を算出する。また、姿勢情報生成部１３は、左眼カメラ６における個々のタイポイントの投影座標（^０ｕ_ｌ，^０ｖ_ｌ）を算出する。これらの投影座標（^０ｕ_ｒ，^０ｖ_ｒ），（^０ｕ_ｌ，^０ｖ_ｌ）は、正規化画像座標系における位置座標である。

世界座標系におけるプリズム座標系の原点の三次元座標が（ｘ_ｐ，ｙ_ｐ，ｚ_ｐ）であるものとする。また、世界座標系の座標軸に対するプリズム座標系の座標軸の回転角が（ω_ｐ，φ_ｐ，κ_ｐ）であるものとする。この場合、任意の点Ｘについて、世界座標系における三次元座標Ｘ_ｗとプリズム座標系における三次元座標Ｘ_ｐとの変換式は、以下の式（２）により表される。

ただし、以下の式（３）及び式（４）が成立している。

また、世界座標系における右眼カメラ座標系の原点の三次元座標が（ｘ_ｒ，ｙ_ｒ，ｚ_ｒ）であるものとする。また、世界座標系の座標軸に対する右眼カメラ座標系の座標軸の回転角が（ω_ｒ，φ_ｒ，κ_ｒ）であるものとする。この場合、任意の点Ｘについて、世界座標系における三次元座標Ｘ_ｐと右眼カメラ座標系における三次元座標Ｘ_ｒとの変換式は、以下の式（５）により表される。

ただし、以下の式（６）及び式（７）が成立している。

上記式（２）及び上記式（５）に基づき、任意の点Ｘについて、世界座標系における三次元座標Ｘ_ｗと右眼カメラ座標系における三次元座標Ｘ_ｒとの変換式は、以下の式（８）により表される。

ただし、以下の式（９）及び式（１０）が成立している。

上記式（８）に基づき、Ｘ_ｗに対応する三次元座標を有するタイポイントについて、右眼カメラ５における投影座標（^０ｕ_ｒ，^０ｖ_ｒ）が算出される。すなわち、右眼カメラ５の焦点距離がｆ_ｒであり、右眼カメラ５における撮像素子の幅がＣ_ｒであり、右眼画像の横幅がＷ_ｒであるものとする。このとき、三次元座標Ｘ_ｗを投影座標（^０ｕ_ｒ，^０ｖ_ｒ）に変換する変換式は、以下の式（１１）により表される。

ただし、以下の式（１２）が成立している。

上記式（１１）を用いて三次元座標Ｘ_ｗが投影座標（^０ｕ_ｒ，^０ｖ_ｒ）に変換されることにより、右眼カメラ５における投影座標（^０ｕ_ｒ，^０ｖ_ｒ）が算出される。これと同様にして、左眼カメラ６における投影座標（^０ｕ_ｌ，^０ｖ_ｌ）が算出される。

すなわち、世界座標系における左眼カメラ座標系の原点の三次元座標が（ｘ_ｌ，ｙ_ｌ，ｚ_ｌ）であるものとする。また、世界座標系の座標軸に対する左眼カメラ座標系の座標軸の回転角が（ω_ｌ，φ_ｌ，κ_ｌ）であるものとする。また、左眼カメラ６の焦点距離がｆ_ｌであり、左眼カメラ６における撮像素子の幅がＣ_ｌであり、左眼画像の横幅がＷ_ｌであるものとする。これらの値に基づき、三次元座標Ｘ_ｗを投影座標（^０ｕ_ｌ，^０ｖ_ｌ）に変換する変換式を用いて、左眼カメラ６における投影座標（^０ｕ_ｌ，^０ｖ_ｌ）が算出される。

次いで、姿勢情報生成部１３は、以下のようにして、ステップＳＴ１１にて算出された画像座標（ｕ，ｖ）における歪みを補正する。

右眼画像において、デジタル画像座標系におけるタイポイントの画像座標が（^１ｕ_ｒ，^１ｖ_ｒ）であるものとする。右眼カメラ５の主点位置を示す位置座標がＸ_ｐｒ及びＹ_ｐｒであり、右眼カメラ５における歪み係数がｋ_１ｒ、ｋ_２ｒ、ｋ_３ｒ、ｐ_１ｒ、ｐ_２ｒ、ｂ_１ｒ及びｂ_２ｒであり、右眼カメラ５における撮像素子の幅がＣ_ｒであり、右眼画像の横幅がＷ_ｒであり、右眼画像の縦幅がＨ_ｒであるものとする。このとき、デジタル画像座標系における歪みを有するタイポイント座標（^１ｕ_ｒ，^１ｖ_ｒ）を、正規化画像座標系における歪み補正後のタイポイント座標（^２ｕ_ｒ，^２ｖ_ｒ）に変換する変換式は、以下の式（１３）により表される。

ただし、以下の式（１４）〜式（１９）が成立している。

上記式（１３）を用いて、デジタル画像座標系における歪みを有するタイポイント座標（^１ｕ_ｒ，^１ｖ_ｒ）が、正規化画像座標系における歪み補正後のタイポイント座標（^２ｕ_ｒ，^２ｖ_ｒ）に変換される。これと同様にして、デジタル画像座標系における歪みを有するタイポイント座標（^１ｕ_ｌ，^１ｖ_ｌ）が、正規化画像座標系における歪み補正後のタイポイント座標（^２ｕ_ｌ，^２ｖ_ｌ）に変換される。

すなわち、左眼画像において、デジタル画像座標系におけるタイポイントの画像座標が（^１ｕ_ｌ，^１ｖ_ｌ）であるものとする。左眼カメラ６の主点位置を示す位置座標がＸ_ｐｌ及びＹ_ｐｌであり、左眼カメラ６における歪み係数がｋ_１ｌ、ｋ_２ｌ、ｋ_３ｌ、ｐ_１ｌ、ｐ_２ｌ、ｂ_１ｌ及びｂ_２ｌであり、左眼カメラ６における撮像素子の幅がＣ_ｌであり、右眼画像の横幅がＷ_ｌであり、右眼画像の縦幅がＨ_ｌであるものとする。このとき、上記式（１３）と同様の変換式を用いて、デジタル画像座標系における歪みを有するタイポイント座標（^１ｕ_ｌ，^１ｖ_ｌ）が、正規化画像座標系における歪み補正後のタイポイント座標（^２ｕ_ｌ，^２ｖ_ｌ）に変換される。

次いで、姿勢情報生成部１３は、以下のようにして、上記算出された投影座標（^０ｕ_ｒ，^０ｖ_ｒ）と、上記算出された画像座標（^２ｕ_ｒ，^２ｖ_ｒ）との差分値を最小化する。また、姿勢情報生成部１３は、上記算出された投影座標（^０ｕ_ｌ，^０ｖ_ｌ）と、上記算出された画像座標（^２ｕ_ｌ，^２ｖ_ｌ）との差分値を最小化する。これにより、タイポイントの三次元座標が最適化される。

ステップＳＴ２にて取得された画像情報に、ｍ枚の右眼画像及びｍ枚の左眼画像が含まれているものとする。ｍ枚の右眼画像のうちのｋ番目の右眼画像に含まれているタイポイントの個数がｐ_ｋ個であり、ｍ枚の左眼画像のうちのｋ番目の左眼画像に含まれているタイポイントの個数がｑ_ｋ個であるものとする。また、ステップＳＴ１１にてｎ個のタイポイントＴ_１，Ｔ_２，……，Ｔ_ｎが検出されており、ｎ個のタイポイントＴ_１，Ｔ_２，……，Ｔ_ｎの三次元座標がそれぞれ（ｘ_Ｔ１，ｙ_Ｔ１，ｚ_Ｔ１），（ｘ_Ｔ２，ｙ_Ｔ２，ｚ_Ｔ２），……，（ｘ_Ｔｎ，ｙ_Ｔｎ，ｚ_Ｔｎ）であるものとする。

上記式（１１）及び上記式（１３）に基づき、以下の式（２０）に示す関数が定義される。当該関数における引数は、タイポイントの三次元座標である。当該関数における返り値は、タイポイントの投影座標とタイポイントの画像座標との差分値である。

上記式（２０）において、^０ｕ_{ｒ，ａ，ｂ}及び^０ｖ_{ｒ，ａ，ｂ}は、ａ番目の右眼画像におけるｂ番目のタイポイントについて上記式（１１）を用いて算出された投影座標の値を示している。また、^０ｕ_{ｌ，ａ，ｂ}及び^０ｖ_{ｌ，ａ，ｂ}は、ａ番目の左眼画像におけるｂ番目のタイポイントについて上記式（１１）と同様の式を用いて算出された投影座標の値を示している。また、^２ｕ_{ｒ，ａ，ｂ}及び^２ｖ_{ｒ，ａ，ｂ}は、ａ番目の右眼画像におけるｂ番目のタイポイントについて上記式（１３）を用いて算出された歪み補正後の画像座標の値を示している。また、^２ｕ_{ｌ，ａ，ｂ}及び^２ｖ_{ｌ，ａ，ｂ}は、ａ番目の左眼画像におけるｂ番目のタイポイントについて上記式（１３）と同様の式を用いて算出された歪み補正後の画像座標の値を示している。

ここで、図１２は、右眼カメラ５及び右眼画像に関するパラメータの値の具体例を示している。図１３は、左眼カメラ６及び左眼画像に関するパラメータの値の具体例を示している。これらのパラメータは、上記式（１１）及び上記式（１３）に基づく上記式（２０）の計算に用いられるものである。

上記式（１１）及び上記（１３）に基づく上記式（２０）の計算において、タイポイントの三次元座標を除くパラメータは、いずれも定数とすることができる（図１２及び図１３参照）。また、一部のタイポイントが既知のマーカに対応するものである場合、当該一部のタイポイントの三次元座標も定数とすることができる。これにより、残余のタイポイントの三次元座標の算出精度の向上を図ることができる。ただし、上記のとおり、全てのタイポイントが未知の地点又は未知のマーカに対応するものであっても良い。

姿勢情報生成部１３は、上記式（２０）に示す関数において、返り値に対応するベクトルの大きさを最小にする最適化計算を実行する。これにより、姿勢情報生成部１３は、投影座標と画像座標との差分値が最小となるタイポイント位置を求める。以下、かかる最適化計算について説明する。

まず、タイポイント位置の初期値^０ｘ_Ｔが以下の式（２１）により表されるものとする。図１４は、図１０に示す初期値（すなわちステップＳＴ１２にて算出された初期値）を整形することにより得られた^０ｘ_Ｔの値を示している。

姿勢情報生成部１３は、タイポイント位置の初期値^０ｘ_Ｔを用いて、以下の式（２２）により、投影座標と画像座標との差分値^０ｙ_Ｔを算出する。式（２２）の計算には、上記式（２０）に示す関数が用いられる。図１５は、図１４に示す値を用いた場合における差分値^０ｙ_Ｔの算出結果を示している。

ここで、タイポイント位置の初期値^０ｘ_Ｔに補正量^１ｘ_Ｔを加えることにより、上記式（２０）に示す関数における返り値を最小化することを考える。補正量^１ｘ_Ｔの算出は、以下の式（２３）により算出される勾配行列を用いて、以下の式（２４）の右辺における残差ベクトルＶを最小化する問題として定式化される。図１６は、図１４に示す値を用いた場合における勾配行列の算出結果を示している。

上記式（２４）の右辺における残差ベクトルＶを最小化する補正量^１ｘ_Ｔは、以下の式（２５）に基づき、最小二乗法により算出される。図１７は、図１５に示す差分値^０ｙ_Ｔの算出結果及び図１６に示す勾配行列の算出結果に基づく補正量^１ｘ_Ｔの算出結果を示している。

ただし、以下の式（２６）が成立している。

このようにして、以下の式（２７）を満たす補正量^１ｘ_Ｔが算出される。

得られたタイポイント位置に対して最小二乗法を繰り返し適用することにより、次段落の（２８）に示す如く、投影座標と画像座標との差分値が最小となるタイポイント位置が得られる。ここで、Ｎ_１は繰り返し回数を示している。

図１８は、最小二乗法の計算を３回繰り返すことにより得られたタイポイント位置の値を示している。すなわち、図１８は、第１最適化処理による最適化後の三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の値の具体例を示している。

次に、図１９〜図２３を参照して、ステップＳＴ１５における、姿勢情報生成部１３による第２最適化処理の具体例について説明する。

まず、姿勢情報生成部１３は、第１最適化処理における算出方法と同様の算出方法により、右眼カメラ５における個々のタイポイントの投影座標（^０ｕ_ｒ，^０ｖ_ｒ）を算出する。また、姿勢情報生成部１３は、左眼カメラ６における個々のタイポイントの投影座標（^０ｕ_ｌ，^０ｖ_ｌ）を算出する。

次いで、姿勢情報生成部１３は、第１最適化処理における補正方法と同様の補正方法により、ステップＳＴ１１にて算出された画像座標（ｕ，ｖ）における歪みを補正する。

次いで、姿勢情報生成部１３は、以下のようにして、上記算出された投影座標（^０ｕ_ｒ，^０ｖ_ｒ）と、上記算出された画像座標（^２ｕ_ｒ，^２ｖ_ｒ）との差分値を最小化する。また、姿勢情報生成部１３は、上記算出された投影座標（^０ｕ_ｌ，^０ｖ_ｌ）と、上記算出された画像座標（^２ｕ_ｌ，^２ｖ_ｌ）との差分値を最小化する。これにより、タイポイントの三次元座標が最適化されるとともに、プリズム２の姿勢角度が最適化される。

上記式（１１）及び上記式（１３）に基づき、以下の式（２９）に示す関数が定義される。当該関数における引数は、プリズム２の姿勢角度及びタイポイントの三次元座標である。当該関数における返り値は、タイポイントの投影座標とタイポイントの画像座標との差分値である。

上記式（２９）において、上記式（２０）に示す符号と同様の符号は、上記（２０）におけるパラメータと同様のパラメータを示している。また、（ω_ｐｋ，φ_ｐｋ，κ_ｐｋ）は、ｋ番目の右眼画像の撮像タイミングに対応する測定タイミングにおけるプリズム２の姿勢角度を示している。

上記式（１１）及び上記（１３）に基づく上記式（２９）の計算において、プリズム２の姿勢角度及びタイポイントの三次元座標を除くパラメータは、いずれも定数とすることができる（図１２及び図１３参照）。また、一部のタイポイントが既知のマーカに対応するものである場合、当該一部のタイポイントの三次元座標も定数とすることができる。これにより、プリズム２の姿勢角度の算出精度の向上を図ることができる。ただし、上記のとおり、全てのタイポイントが未知の地点又は未知のマーカに対応するものであっても良い。

姿勢情報生成部１３は、上記式（２９）に示す関数において、返り値に対応するベクトルの大きさを最小にする最適化計算を実行する。これにより、姿勢情報生成部１３は、投影座標と画像座標との差分値が最小となるタイポイント位置を求める。以下、かかる最適化計算について説明する。

まず、プリズム２の姿勢角度及びタイポイント位置の初期値^０ｘ_ｐＴが以下の式（３０）により表されるものとする。図１９は、図１８に示す値（すなわちステップＳＴ１４にて算出された値）及び図１１に示す初期値（すなわちステップＳＴ１３にて算出された初期値）を整形することにより得られた^０ｘ_ｐＴの値を示している。

姿勢情報生成部１３は、プリズム２の姿勢角度及びタイポイント位置の初期値^０ｘ_Ｔを用いて、以下の式（３１）により、投影座標と画像座標との差分値^０ｙ_ｐＴを算出する。式（３１）の計算には、上記式（２９）に示す関数が用いられる。図２０は、図１９に示す値を用いた場合における差分値^０ｙ_ｐＴの算出結果を示している。

ここで、プリズム２の姿勢角度及びタイポイント位置の初期値^０ｘ_ｐＴに補正量^１ｘ_ｐＴを加えることにより、上記式（２９）に示す関数における返り値を最小化することを考える。補正量^１ｘ_ｐＴの算出は、以下の式（３２）により算出される勾配行列を用いて、以下の式（３３）の右辺における残差ベクトルＶを最小化する問題として定式化される。図２１は、図１９に示す値を用いた場合における勾配行列の算出結果を示している。

上記式（３２）における残差ベクトルＶを最小化する補正量^１ｘ_ｐＴは、以下の式（３４）に基づき、最小二乗法により算出される。図２２は、図２０に示す差分値^０ｙ_ｐＴの算出結果及び図２１に示す勾配行列の算出結果に基づく補正量^１ｘ_ｐＴの算出結果を示している。

ただし、以下の式（３５）が成立している。

このようにして、以下の式（３６）を満たす補正量^１ｘ_ｐＴが算出される。

得られたプリズム２の姿勢角度及びタイポイント位置に対して最小二乗法を繰り返し適用することにより、次段落の（３７）に示す如く、投影座標と画像座標との差分値が最小となるプリズム２の姿勢角度及びタイポイント位置が得られる。ここで、Ｎ_２は繰り返し回数を示している。

図２３は、最小二乗法の計算を６回繰り返すことにより得られたプリズム２の姿勢角度の値及びタイポイント位置の値を示している。すなわち、図２３は、第２最適化処理による最適化後の三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の値の具体例、及び第２最適化処理による最適化後の姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の値の具体例を示している。

次に、図２４及び図２５を参照して、ステップＳＴ４における、位置姿勢算出部１４による台車１の三次元座標（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）の値の算出方法の具体例について説明する。また、台車１の姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）の値の算出方法の具体例について説明する。

台車座標系が右眼カメラ座標系と一致する座標系に設定される場合、台車１の三次元座標（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）及び台車１の姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）は、上記式（８）に基づき、以下の式（３８）により表される。

ただし、以下の式（３９）及び式（４０）が成立している。

この場合、位置姿勢算出部１４は、上記式（３８）を用いて、台車１の三次元座標（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）の値を算出するとともに、台車１の姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）の値を算出する。図２４は、この場合における、台車１の三次元座標（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）の値の具体例、及び台車１の姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）の値の具体例を示している。なお、図２４に示す値は、図７に示す位置情報、図１２に示すパラメータ、及び図１３に示すパラメータに基づくものである。

他方、台車座標系が左眼カメラ座標系と一致する座標系に設定される場合、位置姿勢算出部１４は、上記式（３８）と同様の式を用いて、台車１の姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）の値を算出するとともに、台車１の姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）の値を算出する。図２５は、この場合における、台車１の三次元座標（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）の値の具体例、及び台車１の姿勢角度（ω_ｔ，φ_ｔ，κ_ｔ）の値の具体例を示している。なお、図２５に示す値は、図７に示す位置情報、図１２に示すパラメータ、及び図１３に示すパラメータに基づくものである。

このように、ステップＳＴ１〜ＳＴ３の処理が実行されることにより、プリズム２の姿勢を高精度に算出することができる。すなわち、画像情報に加えて位置情報を用いることにより、プリズム２の姿勢を高精度に算出することができる。この結果、ステップＳＴ４の処理が実行されることにより、台車１の姿勢を高精度に推定することができる。また、台車１の位置を高精度に推定することができる。したがって、ステップＳＴ５の処理が実行されることにより、測量対象物の三次元形状を高精度に測定することができる。

また、姿勢検出用のセンサを不要とすることができる。この結果、測量装置２００を安価に実現することができる。

次に、測量装置２００の変形例について説明する。

測量用の移動体は、台車１に限定されるものではない。例えば、移動体は、車両、航空機、船舶又は背負子であっても良い。

位置測定用の反射材は、全周型のプリズム２に限定されるものではない。例えば、トータルステーション３に対する移動体の相対方向の変動量が小さい場合、反射材は、通常のプリズムを用いたものであっても良い。

位置測定用の測定器は、自動追尾型のトータルステーション３に限定されるものではない。例えば、各タイミングにて移動体が静止するものである場合、測定器は、自動追尾機能を有しない通常のトータルステーションを用いたものであっても良い。

撮像機は、２個のデジタルカメラを用いたステレオカメラ４に限定されるものではない。撮像機は、１個以上のデジタルカメラを用いたものであれば良い。ただし、デジタルカメラの個数を増やすことにより、台車１の位置及び姿勢の推定精度を更に向上することができる。他方、デジタルカメラの個数を減らすことにより、測量装置２００を更に安価に実現することができる。

撮像対象領域は、測量対象物を含む領域に限定されるものではない。また、測量対象物は、地面に限定されるものではない。例えば、測量対象物は、橋梁の表面又はトンネルの内面であっても良い。すなわち、測量対象物は、水平面を含むものであっても良く、鉛直面を含むものであっても良く、傾斜面を含むものであっても良い。また、測量対象物は、平面状であっても良く、曲面状であっても良く、凹凸面状であっても良い。

なお、測量対象物を含む領域と異なる領域が撮像対象領域に設定されている場合、移動体に距離測定用の測定器（例えばレーザスキャナ）が設けられているものであっても良い。三次元形状算出部１５は、当該測定器により測定された距離、位置姿勢算出部１４により算出された位置、及び位置姿勢算出部１４により算出された姿勢に基づき、測量対象物の三次元形状を算出するものであっても良い。

姿勢情報生成部１３は、第１最適化処理を実行することなく、第２最適化処理を実行するものであっても良い。すなわち、姿勢情報生成部１３は、ステップＳＴ１３に次いで、ステップＳＴ１５の処理を実行するものであっても良い。この場合における第２最適化処理は、ステップＳＴ１２における初期値に比して最適化された三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の値を算出するとともに、ステップＳＴ１３における初期値に比して最適化された姿勢角度（ω_ｐ，φ_ｐ，κ_ｐ）の値を算出する処理であっても良い。

ただし、第１最適化処理が実行されることにより、ステップＳＴ１２における初期値の真値に対する差分値が大きいときであっても、プリズム２の姿勢を正確に算出することができる。このため、第１最適化処理を実行するのがより好適である。

以上のように、実施の形態１に係る位置姿勢推定装置１００は、位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定装置１００であって、測定器により測定された位置を示す位置情報を取得する位置情報取得部１１と、撮像機により撮像された画像を示す画像情報を取得する画像情報取得部１２と、位置情報及び画像情報を用いて反射材の姿勢を算出することにより、反射材の姿勢を示す姿勢情報を生成する姿勢情報生成部１３と、位置情報及び姿勢情報を用いて移動体の位置及び移動体の姿勢を算出する位置姿勢算出部１４と、を備える。これにより、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる。

また、実施の形態１に係る位置姿勢推定方法は、位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定方法であって、位置情報取得部１１が、測定器により測定された位置を示す位置情報を取得するステップＳＴ１と、画像情報取得部１２が、撮像機により撮像された画像を示す画像情報を取得するステップＳＴ２と、姿勢情報生成部１３が、位置情報及び画像情報を用いて反射材の姿勢を算出することにより、反射材の姿勢を示す姿勢情報を生成するステップＳＴ３と、位置姿勢算出部１４が、位置情報及び姿勢情報を用いて移動体の位置及び移動体の姿勢を算出するステップＳＴ４と、を備える。これにより、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる。

なお、本願発明はその発明の範囲内において、実施の形態の任意の構成要素の変形、もしくは実施の形態の任意の構成要素の省略が可能である。

１ 台車（移動体）、２ プリズム（反射材）、３ トータルステーション（測定器）、４ ステレオカメラ（撮像機）、５ 右眼カメラ、６ 左眼カメラ、７ 記憶装置、８ 計算機、１１ 位置情報取得部、１２ 画像情報取得部、１３ 姿勢情報生成部、１４ 位置姿勢算出部、１５ 三次元形状算出部、２１ プロセッサ、２２ メモリ、２３ 処理回路、１００ 位置姿勢推定装置、２００ 測量装置。

Claims (5)

1. 位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定装置であって、
   前記測定器により測定された位置を示す位置情報を取得する位置情報取得部と、
   前記撮像機により撮像された画像を示す画像情報を取得する画像情報取得部と、
   前記位置情報及び前記画像情報を用いて前記反射材の姿勢を算出することにより、前記反射材の姿勢を示す姿勢情報を生成する姿勢情報生成部と、
   前記位置情報及び前記姿勢情報を用いて前記移動体の位置及び前記移動体の姿勢を算出する位置姿勢算出部と、
   を備えることを特徴とする位置姿勢推定装置。
2. 前記測定器は、前記反射材を追尾する機能を有することを特徴とする請求項１記載の位置姿勢推定装置。
3. 前記撮像機は、複数個のカメラにより構成されており、
   前記複数個のカメラによる視野が互いに重複している
   ことを特徴とする請求項１記載の位置姿勢推定装置。
4. 前記姿勢情報生成部は、前記画像に含まれるマーカの位置に基づき前記反射材の姿勢を算出するものであり、
   前記マーカの位置が既知である
   ことを特徴とする請求項１記載の位置姿勢推定装置。
5. 位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定方法であって、
   位置情報取得部が、前記測定器により測定された位置を示す位置情報を取得するステップと、
   画像情報取得部が、前記撮像機により撮像された画像を示す画像情報を取得するステップと、
   姿勢情報生成部が、前記位置情報及び前記画像情報を用いて前記反射材の姿勢を算出することにより、前記反射材の姿勢を示す姿勢情報を生成するステップと、
   位置姿勢算出部が、前記位置情報及び前記姿勢情報を用いて前記移動体の位置及び前記移動体の姿勢を算出するステップと、
   を備えることを特徴とする位置姿勢推定方法。

Images