JP2022087978A - 位置姿勢算出装置、位置姿勢算出方法及び測量装置 - 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.

位置姿勢算出部１４は、それぞれのタイポイントが、既知のマーカ又は未知のマーカに対応していれば、それぞれのデジタル画像に対するテンプレートマッチングを実行することによって、それぞれのデジタル画像に含まれているマーカを検出する。
位置姿勢算出部１４は、検出したマーカがタイポイントであるとして、検出したマーカの画像座標（ｕ，ｖ）を算出する。
また、それぞれのタイポイントが、既知のマーカ又は未知のマーカに対応している場合、測量装置２００のユーザが、それぞれのデジタル画像に含まれているマーカの位置を目視によって確認し、ユーザが、確認した位置を位置姿勢算出部１４に与えるようにしてもよい。位置姿勢算出部１４は、ユーザによって与えられた位置に対応する画像座標（ｕ，ｖ）を算出する。

次に、位置姿勢算出部１４は、世界座標系におけるそれぞれのタイポイントの三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の初期値を算出する（図８のステップＳＴ１２）。
以下、位置姿勢算出部１４による三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の初期値の算出処理を具体的に説明する。
位置姿勢算出部１４は、複数の情報対に含まれている位置情報が示す位置の全てのＸ座標（ｘ_ｐｗ）の平均値を算出する。
位置姿勢算出部１４は、Ｘ座標（ｘ_ｐｗ）の平均値を、それぞれのタイポイントのＸ座標（ｘ_Ｔ）の初期値に用いる。
位置姿勢算出部１４は、複数の情報対に含まれている位置情報が示す位置の全てのＹ座標（ｙ_ｐｗ）の平均値を算出する。
位置姿勢算出部１４は、Ｙ座標（ｙ_ｐｗ）の平均値を、それぞれのタイポイントのＹ座標（ｙ_Ｔ）の初期値に用いる。

位置姿勢算出部１４は、複数の情報対に含まれている位置情報が示す位置のＺ座標（ｚ_ｐｗ）を用いて、地面における任意の地点のＺ座標を算出する。地面における任意の地点のＺ座標は、地面の高さに近い位置のＺ座標である。
具体的には、例えば、位置姿勢算出部１４は、複数の情報対に含まれている位置情報が示す位置の全てのＺ座標（ｚ_ｐｗ）の平均値を算出する。そして、位置姿勢算出部１４は、Ｚ座標（ｚ_ｐｗ）の平均値から、移動体１に対して反射材３が設置されている位置の高さを示す既知のＺ座標を減算することによって、地面における任意の地点のＺ座標を算出する。
位置姿勢算出部１４は、地面における任意の地点のＺ座標を、それぞれのタイポイントのＺ座標（ｚ_Ｔ）の初期値に用いる。

次に、位置姿勢算出部１４は、右眼カメラ５の姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）の初期値を算出する（図８のステップＳＴ１３）。
以下、位置姿勢算出部１４による姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）の初期値の算出処理を具体的に説明する。
位置姿勢算出部１４では、時刻ｔにおける反射材３の三次元座標が（ｘ，ｙ，ｚ）であるものとする。時刻ｔに対応するＴＳ計測タイミングの次のＴＳ計測タイミングに対応する時刻がｔ’であるものとする。
位置姿勢算出部１４では、時刻ｔ’における反射材３の三次元座標が（ｘ’，ｙ’，ｚ’）であるものとする。
右眼カメラ座標系におけるＸ軸の正方向は、右眼カメラ５の投影面における上方向に対応しており、移動体１の正面方向を示すものとなる。図１に示す測量装置２００では、移動体１の進行方向が、移動体１の正面方向と同一又は略同一の方向に設定されている。
したがって、右眼カメラ５の姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）は、Ｘ座標（１，０，０）を反射材３の移動ベクトル（ｘ－ｘ’，ｙ－ｙ’，ｚ－ｚ’）と一致させる回転角によって近似される。

図１に示す測量装置２００では、右眼カメラ５が、世界座標系におけるＸＹ平面に沿って移動する。このため、ロール角（ω_ｒｗ）及びピッチ角（φ_ｒｗ）のそれぞれは、０に近似される。また、ヨー角（κ_ｒｗ）は、ＸＹ平面におけるベクトル（１，０）をベクトル（ｘ－ｘ’，ｙ－ｙ’）と一致させる回転角によって近似される。したがって、ヨー角（κ_ｒｗ）について、以下の式（３）が成立する。

位置姿勢算出部１４は、式（３）によってヨー角（κ_ｒｗ）を算出する。ヨー角（κ_ｒｗ）は、逆三角関数により算出される。

なお、時刻ｔに対応するＴＳ計測タイミングの１つ前のＴＳ計測タイミングに対応する時刻がｔ”であるものとする。時刻ｔよりも後の時刻における反射材３の位置が未測定である場合、時刻ｔにおける姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）の初期値に、時刻ｔ”における姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）の初期値が用いられる。時刻ｔ”における姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）の初期値は、時刻ｔにおける反射材３の三次元座標と、時刻ｔ”における反射材３の三次元座標とを用いて算出される。

次に、位置姿勢算出部１４は、右眼カメラ５の位置の三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）の初期値を算出する（図８のステップＳＴ１４）。
以下、位置姿勢算出部１４による三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）の初期値の算出処理を具体的に説明する。
位置姿勢算出部１４では、世界座標系における反射材３の位置の三次元座標が（ｘ_ｐｗ，ｙ_ｐｗ，ｚ_ｐｗ）であるものとする。また、右眼カメラ座標系における反射材３の位置の三次元座標が（ｘ_ｐｒ，ｙ_ｐｒ，ｚ_ｐｒ）であるものとする。
位置姿勢算出部１４では、世界座標系における右眼カメラ５の姿勢角度が（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）であるものとする。

この場合、右眼カメラ５の位置の三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）は、以下の式（４）及び式（５）によって表される。

位置姿勢算出部１４は、ステップＳＴ１３で算出した右眼カメラ５の姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）の初期値を式（５）に代入し、式（４）に基づいて、右眼カメラ５の位置の三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）の初期値を算出する。

次に、位置姿勢算出部１４は、タイポイントの三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）の初期値を実際の値に近づけ、右眼カメラ５の姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）の初期値を実際の値に近づけ、右眼カメラ５の位置の三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）の初期値を実際の値に近づける最適化処理を実施する（図８のステップＳＴ１５）。
以下、位置姿勢算出部１４による最適化処理を具体的に説明する。

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

世界座標系における反射材３の位置の三次元座標（ｘ_ｐｗ，ｙ_ｐｗ，ｚ_ｐｗ）、右眼カメラ座標系における反射材３の位置の三次元座標（ｘ_ｐｒ，ｙ_ｐｒ，ｚ_ｐｒ）及び世界座標系における右眼カメラ５の姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）が与えられるものとする。この場合、任意の点Ｘについて、世界座標系における三次元座標Ｘ_ｗと、右眼カメラ座標系における三次元座標Ｘ_ｒとの変換式は、式（４）及び以下の式（６）によって表される。

右眼カメラ５の焦点距離がｆ_ｒであり、右眼カメラ５における撮像素子の幅がＣ_ｒであり、右眼画像の横幅がＷ_ｒであるものとする。このとき、三次元座標Ｘ_ｗを投影座標（^０ｕ_ｒ，^０ｖ_ｒ）に変換する変換式は、以下の式（７）及び式（８）によって表される。式（７）において、Ｔは、転置を表す数学記号である。

位置姿勢算出部１４は、世界座標系における三次元座標Ｘ_ｗに対応する三次元座標を有するタイポイントについて、式（７）及び式（８）に従って、右眼カメラ５における投影座標（^０ｕ_ｒ，^０ｖ_ｒ）を算出する。

位置姿勢算出部１４は、左眼カメラ６におけるそれぞれのタイポイントの投影座標（^０ｕ_ｌ，^０ｖ_ｌ）についても、右眼カメラ５におけるそれぞれのタイポイントの投影座標（^０ｕ_ｒ，^０ｖ_ｒ）と同様に算出する。
具体的には、世界座標系における左眼カメラ座標系の原点の三次元座標が（ｘ_ｌｗ，ｙ_ｌｗ，ｚ_ｌｗ）であるものとする。また、世界座標系の座標軸に対する左眼カメラ座標系の座標軸の回転角が（ω_ｌｗ，φ_ｌｗ，κ_ｌｗ）であるものとする。また、左眼カメラ６の焦点距離がｆ_ｌであり、左眼カメラ６における撮像素子の幅がＣ_ｌであり、左眼画像の横幅がＷ_ｌであるものとする。
位置姿勢算出部１４は、これらの値に基づき、三次元座標Ｘ_ｗを投影座標（^０ｕ_ｌ，^０ｖ_ｌ）に変換する変換式を用いて、左眼カメラ６における投影座標（^０ｕ_ｌ，^０ｖ_ｌ）を算出する。

ここで、世界座標系における左眼カメラ座標系の原点の三次元座標（ｘ_ｌｗ，ｙ_ｌｗ，ｚ_ｌｗ）及び世界座標系の座標軸に対する左眼カメラ座標系の座標軸の回転角（ω_ｌｗ，φ_ｌｗ，κ_ｌｗ）は、以下の式（９）～式（１２）によって表される。即ち、三次元座標（ｘ_ｌｗ，ｙ_ｌｗ，ｚ_ｌｗ）及び回転角（ω_ｌｗ，φ_ｌｗ，κ_ｌｗ）は、世界座標系における右眼カメラ座標系の原点の三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）、世界座標系の座標軸に対する右眼カメラ座標系の座標軸の回転角（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）、右眼カメラ座標系における左眼カメラ座標系の原点の三次元座標（ｘ_ｌｒ，ｙ_ｌｒ，ｚ_ｌｒ）及び右眼カメラ座標系の座標軸に対する左眼カメラ座標系の座標軸の回転角（ω_ｌｒ，φ_ｌｒ，κ_ｌｒ）によって表される。

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

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

次に、位置姿勢算出部１４は、算出した右眼カメラ５における投影座標（^０ｕ_ｒ，^０ｖ_ｒ）と、算出したタイポイントの座標（^２ｕ_ｒ，^２ｖ_ｒ）との差分値を最小化する。
また、位置姿勢算出部１４は、算出した左眼カメラ６における投影座標（^０ｕ_ｌ，^０ｖ_ｌ）と、算出したタイポイントの座標（^２ｕ_ｌ，^２ｖ_ｌ）との差分値を最小化する。
最適化対象のパラメータは、世界座標系におけるそれぞれのタイポイントの三次元座標（ｘ_Ｔ，ｙ_Ｔ，ｚ_Ｔ）、右眼カメラ５の姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）及び右眼カメラ５の位置の三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）である。最適化対象のパラメータ以外のパラメータは、いずれも定数とする。または、最適化対象のパラメータ以外のパラメータは、最適化対象のパラメータを用いて算出する。
右眼カメラ５の位置の三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）は、上述したように、最適化対象のパラメータに含められているが、移動体１が静止しているときの右眼カメラ５の位置の三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）は、最適化対象のパラメータに含められない。即ち、位置姿勢算出部１４は、移動体１が静止しているときの右眼カメラ５の位置の三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）を最適化処理によって求めない。位置姿勢算出部１４は、式（４）に基づいて、世界座標系における反射材３の位置の三次元座標（ｘ_ｐｗ，ｙ_ｐｗ，ｚ_ｐｗ）、右眼カメラ座標系における反射材３の位置の三次元座標（ｘ_ｐｒ，ｙ_ｐｒ，ｚ_ｐｒ）及び世界座標系における右眼カメラ５の姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）を用いて、三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）を算出する。

一部のタイポイントが既知のマーカに対応するものである場合、一部のタイポイントの三次元座標も定数とすることができる。これにより、残余のタイポイントの三次元座標の算出精度の向上を図ることができる。ただし、上記のとおり、全てのタイポイントが未知の地点又は未知のマーカに対応するものであってもよい。
ステップＳＴ１５における最適化処理は、画像座標値の差分値の二乗和を最小化する非線形最小二乗問題であり、求解には、公知の種々の技術を用いることができる。これらの技術についての詳細な説明は省略する。具体的には、例えば、以下の参考文献２に記載された技術を用いることができる。
［参考文献2］
Levenberg, Kenneth. "A method for the solution of certain non-linear problems in least squares." Quarterly of applied mathematics 2.2 (1944): 164-168.

上記のように、ステップＳＴ１～ＳＴ４の処理が実行されることにより、右眼カメラ５の位置と右眼カメラ５の姿勢とを高精度に算出することができる。
通常の位置姿勢についての推定処理では、カメラの位置及び姿勢のそれぞれを未知数とする６自由度の最適化が行われる。これに対して、図１に示す測量装置２００では、移動体１が静止しているときの右眼カメラ５の姿勢のみを未知数とする３自由度の最適化を行うことにより、右眼カメラ５の位置と右眼カメラ５の姿勢とを高精度に算出することができる。

三次元形状算出部１５は、情報抽出部１３により抽出された情報対に含まれている画像情報を取得する。
三次元形状算出部１５は、位置姿勢算出部１４から、移動体１の位置を示す位置情報及び移動体１の姿勢を示す姿勢情報のそれぞれを取得する。移動体１の位置は、上述したように、右眼カメラ５の位置に相当し、移動体１の姿勢は、右眼カメラ５の姿勢に相当する。右眼カメラ５の位置は、三次元座標（ｘ_ｒｗ，ｙ_ｒｗ，ｚ_ｒｗ）であり、右眼カメラ５の姿勢は、姿勢角度（ω_ｒｗ，φ_ｒｗ，κ_ｒｗ）である。
三次元形状算出部１５は、画像情報が示す撮像画像と、位置情報が示す移動体１の位置と、姿勢情報が示す移動体１の姿勢とを用いて、測量対象物の三次元形状を算出する（図７のステップＳＴ５）。
三次元形状算出部１５は、例えば、いわゆる「多視点ステレオ」の技術を用いることによって、測量対象物の三次元形状を算出する。
右眼カメラ５の位置及び右眼カメラ５の姿勢のそれぞれが、位置姿勢算出部１４によって高精度に求められているため、三次元形状算出部１５による三次元形状の算出精度が向上する。
図１に示す測量装置２００では、姿勢検出用のセンサを用いることなく、測量対象物の三次元形状を算出することができる。

以上の実施の形態１では、測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器４と、反射材３とを備えている移動体１の位置及び移動体１の姿勢のそれぞれを算出する位置姿勢算出装置１００であって、反射材３の位置を複数回測定する測定器２による複数の測定結果のそれぞれを示す位置情報を取得する位置情報取得部１１と、撮像器４により生成されたそれぞれの撮像画像を示す画像情報を取得する画像情報取得部１２と、位置情報取得部１１により取得された複数の位置情報と画像情報取得部１２により取得された複数の画像情報との中から、測定器２による位置の測定タイミングと撮像器４による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する情報抽出部１３と、情報抽出部１３により抽出された情報対から、移動体１の位置及び移動体１の姿勢のそれぞれを算出する位置姿勢算出部１４とを備えるように、位置姿勢算出装置１００を構成した。したがって、位置姿勢算出装置１００は、姿勢検出器を実装することなく、移動体１の姿勢を算出することができる。

図１に示す測量装置２００では、撮像器４が、右眼カメラ５及び左眼カメラ６の双方を有するステレオカメラによって実現されている。しかし、撮像器４は、１つ以上のデジタルカメラを実装しているものであればよい。
撮像器４が実装しているデジタルカメラの個数を増やすことにより、移動体１の位置及び姿勢におけるそれぞれの推定精度を更に向上させることができる。撮像器４が実装しているデジタルカメラの個数を減らすことにより、測量装置２００を安価に実現することができる。

撮像対象領域は、測量対象物を含む領域に限定されるものではない。また、測量対象物は、地面に限定されるものではない。例えば、測量対象物は、橋梁の表面又はトンネルの内面であってもよい。即ち、測量対象物は、水平面を含むものであってもよく、鉛直面を含むものであってもよく、傾斜面を含むものであってもよい。また、測量対象物は、平面状であってもよく、曲面状であってもよく、凹凸面状であってもよい。
なお、測量対象物を含む領域と異なる領域が撮像対象領域に設定されている場合、移動体１に、距離測定用の測定器として、例えば、レーザスキャナが設けられているものであってもよい。
三次元形状算出部１５は、距離測定用の測定器により測定された距離と、位置姿勢算出部１４によりそれぞれ算出された位置及び姿勢とを用いて、測量対象物の三次元形状を算出するものであってもよい。

なお、本開示は、実施の形態の任意の構成要素の変形、もしくは実施の形態の任意の構成要素の省略が可能である。

１ 移動体、２ 測定器、３ 反射材、４ 撮像器、５ 右眼カメラ、６ 左眼カメラ、７ 記憶装置、８ 演算装置、１１ 位置情報取得部、１２ 画像情報取得部、１３ 情報抽出部、１４ 位置姿勢算出部、１５ 三次元形状算出部、２１ 位置情報取得回路、２２ 画像情報取得回路、２３ 情報抽出回路、２４ 位置姿勢算出回路、２５ 三次元形状算出回路、３１ メモリ、３２ プロセッサ、１００ 位置姿勢算出装置、２００ 測量装置。

Claims (10)

1. 測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器と、反射材とを備えている移動体の位置及び前記移動体の姿勢のそれぞれを算出する位置姿勢算出装置であって、
   前記反射材の位置を複数回測定する測定器による複数の測定結果のそれぞれを示す位置情報を取得する位置情報取得部と、
   前記撮像器により生成されたそれぞれの撮像画像を示す画像情報を取得する画像情報取得部と、
   前記位置情報取得部により取得された複数の位置情報と前記画像情報取得部により取得された複数の画像情報との中から、前記測定器による位置の測定タイミングと前記撮像器による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する情報抽出部と、
   前記情報抽出部により抽出された情報対から、前記移動体の位置及び前記移動体の姿勢のそれぞれを算出する位置姿勢算出部と
   を備えた位置姿勢算出装置。
2. 前記位置姿勢算出部は、前記情報抽出部により抽出された情報対から、前記移動体の位置の初期値及び前記移動体の姿勢の初期値のそれぞれを算出し、前記位置の初期値を前記位置の実際の値に近づけ、前記姿勢の初期値を前記姿勢の実際の値に近づける最適化処理として、前記移動体の姿勢のみを未知数とする３自由度の最適化処理を実施することを特徴とする請求項１記載の位置姿勢算出装置。
3. 前記情報抽出部は、前記位置情報取得部により取得された複数の位置情報の中から、前記移動体が静止しているときの測定結果を示す位置情報を抽出し、前記画像情報取得部により取得された複数の画像情報の中から、前記移動体が静止しているときの撮像画像を示す画像情報を抽出し、抽出した位置情報と抽出した画像情報との組を情報対として前記位置姿勢算出部に出力することを特徴とする請求項１記載の位置姿勢算出装置。
4. 前記情報抽出部は、前記画像情報取得部により取得された複数の画像情報が示す撮像画像を互いに比較し、撮像画像の比較結果に基づいて、前記複数の撮像画像の中で、前記移動体が静止しているときの撮像画像を特定することを特徴とする請求項３記載の位置姿勢算出装置。
5. 前記情報抽出部は、前記位置情報取得部により取得された複数の位置情報が示す位置を互いに比較し、位置の比較結果に基づいて、前記複数の測定結果の中で、前記移動体が静止しているときの測定結果を特定することを特徴とする請求項３記載の位置姿勢算出装置。
6. 測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器と、反射材とを備えている移動体の位置及び前記移動体の姿勢のそれぞれを算出する位置姿勢算出方法であって、
   位置情報取得部が、前記反射材の位置を複数回測定する測定器による複数の測定結果のそれぞれを示す位置情報を取得し、
   画像情報取得部が、前記撮像器により生成された複数の撮像画像のそれぞれを示す画像情報を取得し、
   情報抽出部が、前記位置情報取得部により取得された複数の位置情報と前記画像情報取得部により取得された複数の画像情報との中から、前記測定器による位置の測定タイミングと前記撮像器による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出し、
   位置姿勢算出部が、前記情報抽出部により抽出された情報対から、前記移動体の位置及び前記移動体の姿勢のそれぞれを算出する
   位置姿勢算出方法。
7. 移動体に設けられている反射材の位置を複数回測定する測定器と、
   前記移動体に搭載されており、測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器と、
   前記測定器による複数の測定結果のそれぞれを示す位置情報を取得する位置情報取得部と、
   前記撮像器により生成されたそれぞれの撮像画像を示す画像情報を取得する画像情報取得部と、
   前記位置情報取得部により取得された複数の位置情報と前記画像情報取得部により取得された複数の画像情報との中から、前記測定器による位置の測定タイミングと前記撮像器による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する情報抽出部と、
   前記情報抽出部により抽出された情報対から、前記移動体の位置及び前記移動体の姿勢のそれぞれを算出する位置姿勢算出部と
   前記情報抽出部により抽出された情報対に含まれている画像情報が示す撮像画像と、前記位置姿勢算出部によりそれぞれ算出された位置及び姿勢とを用いて、前記測量対象物の三次元形状を算出する三次元形状算出部と
   を備えた測量装置。
8. 前記測定器は、前記反射材を追尾する機能を有していることを特徴とする請求項７記載の測量装置。
9. 前記撮像器は、複数のカメラを有しており、
   前記複数のカメラの視野が互いに重複していることを特徴とする請求項７記載の測量装置。
10. 前記位置姿勢算出部は、前記情報抽出部により抽出された情報対に含まれている画像情報が示す撮像画像に映っているマーカの位置から、前記移動体の姿勢を算出することを特徴とする請求項７記載の測量装置。

Images