(57)【要約】【課題】高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる位置姿勢推定装置を提供する。【解決手段】位置姿勢推定装置１００は、測定器（トータルステーション３）により測定された位置を示す位置情報を取得する位置情報取得部１１と、撮像機（ステレオカメラ４）により撮像された画像を示す画像情報を取得する画像情報取得部１２と、位置情報及び画像情報を用いて反射材（プリズム２）の姿勢を算出することにより、反射材（プリズム２）の姿勢を示す姿勢情報を生成する姿勢情報生成部１３と、位置情報及び姿勢情報を用いて移動体（台車１）の位置及び移動体（台車１）の姿勢を算出する位置姿勢算出部１４と、を備える。【選択図】図３
2020186944.tif 000002
【特許請求の範囲】
【請求項１】
位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定装置であって、
前記測定器により測定された位置を示す位置情報を取得する位置情報取得部と、
前記撮像機により撮像された画像を示す画像情報を取得する画像情報取得部と、
前記位置情報及び前記画像情報を用いて前記反射材の姿勢を算出することにより、前記反射材の姿勢を示す姿勢情報を生成する姿勢情報生成部と、
前記位置情報及び前記姿勢情報を用いて前記移動体の位置及び前記移動体の姿勢を算出する位置姿勢算出部と、
を備えることを特徴とする位置姿勢推定装置。
【請求項２】
前記測定器は、前記反射材を追尾する機能を有することを特徴とする請求項１記載の位置姿勢推定装置。
【請求項３】
前記撮像機は、複数個のカメラにより構成されており、
前記複数個のカメラによる視野が互いに重複している
ことを特徴とする請求項１記載の位置姿勢推定装置。
【請求項４】
前記姿勢情報生成部は、前記画像に含まれるマーカの位置に基づき前記反射材の姿勢を算出するものであり、
前記マーカの位置が既知である
ことを特徴とする請求項１記載の位置姿勢推定装置。
【請求項５】
位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定方法であって、
位置情報取得部が、前記測定器により測定された位置を示す位置情報を取得するステップと、
画像情報取得部が、前記撮像機により撮像された画像を示す画像情報を取得するステップと、
姿勢情報生成部が、前記位置情報及び前記画像情報を用いて前記反射材の姿勢を算出することにより、前記反射材の姿勢を示す姿勢情報を生成するステップと、
位置姿勢算出部が、前記位置情報及び前記姿勢情報を用いて前記移動体の位置及び前記移動体の姿勢を算出するステップと、
を備えることを特徴とする位置姿勢推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置姿勢推定装置及び位置姿勢推定方法に関する。
【背景技術】
【０００２】
従来、移動体（例えば車両又は航空機）を用いて測量対象物（例えば地面又は構造物）の三次元形状を測定する測量装置が開発されている。
【０００３】
すなわち、測量対象物に沿うように移動体が移動しているとき、測量対象物撮像用の撮像機（例えばデジタルカメラ）により測量対象物が複数回撮像される。また、このとき、位置測定用の測定器（例えばトータルステーション）により移動体の位置が複数回測定される。また、このとき、姿勢検出用のセンサ（例えばジャイロセンサ）により移動体の姿勢が複数回検出される。当該撮像された画像、当該測定された位置、及び当該検出された姿勢に基づき、測量対象物の三次元形状が測定される。測量対象物撮像用の撮像機及び姿勢検出用のセンサは、移動体に設けられている（例えば、特許文献１参照。）。
【０００４】
または、測量対象物に沿うように移動体が移動しているとき、距離測定用の測定器（例えばレーザスキャナ）により移動体と測量対象物間の距離が複数回測定される。また、このとき、位置測定用の測定器により移動体の位置が複数回測定される。また、このとき、姿勢検出用のセンサにより移動体の姿勢が複数回検出される。当該測定された距離、当該測定された位置、及び当該検出された姿勢に基づき、測量対象物の三次元形状が測定される。距離測定用の測定器及び姿勢検出用のセンサは、移動体に設けられている（例えば、特許文献２参照。）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１５−８７３０７号公報
【特許文献２】特開２０１７−２０９７２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
高精度な測量を実現する観点から、移動体の姿勢を高精度に検出することが要求される。ここで、高精度なセンサを用いることにより、移動体の姿勢を高精度に検出することが考えられる。しかしながら、通常、高精度なセンサは高価である。
【０００７】
これに対して、特許文献２には、距離測定用の測定器（距離計測部２０）と異なる撮像機（画像取得部３０）により撮像された画像を用いて、姿勢検出用のセンサ（慣性計測部４０）による検出値を補正する技術が開示されている。これは、移動体の姿勢検出の精度向上を図るものである。しかしながら、かかる画像を用いた検出値の補正による精度向上には限界がある。
【０００８】
本発明は、上記のような課題を解決するためになされたものであり、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる位置姿勢推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明の位置姿勢推定装置は、位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定装置であって、測定器により測定された位置を示す位置情報を取得する位置情報取得部と、撮像機により撮像された画像を示す画像情報を取得する画像情報取得部と、位置情報及び画像情報を用いて反射材の姿勢を算出することにより、反射材の姿勢を示す姿勢情報を生成する姿勢情報生成部と、位置情報及び姿勢情報を用いて移動体の位置及び移動体の姿勢を算出する位置姿勢算出部と、を備えるものである。
【発明の効果】
【００１０】
本発明によれば、上記のように構成したので、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる。
【図面の簡単な説明】
【００１１】
【図１】実施の形態１に係る位置姿勢推定装置を含む測量装置の要部を示す斜視図である。
【図２】実施の形態１に係る位置姿勢推定装置を含む測量装置の要部を示すブロック図である。
【図３】実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の要部を示すブロック図である。
【図４Ａ】実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機のハードウェア構成を示す説明図である。
【図４Ｂ】実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の他のハードウェア構成を示す説明図である。
【図５】実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の動作を示すフローチャートである。
【図６】実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の詳細な動作を示すフローチャートである。
【図７】位置情報の例を示す説明図である。
【図８】画像情報の例を示す説明図である。
【図９Ａ】各時刻にて撮像された左眼画像における個々のタイポイントの画像座標の例を示す説明図である。
【図９Ｂ】各時刻にて撮像された右眼画像における個々のタイポイントの画像座標の例を示す説明図である。
【図１０】個々のタイポイントの三次元座標の初期値の例を示す説明図である。
【図１１】各時刻におけるプリズムの姿勢角度の初期値の例を示す説明図である。
【図１２】右眼カメラ及び右眼画像に関するパラメータの例を示す説明図である。
【図１３】左眼カメラ及び左眼画像に関するパラメータの例を示す説明図である。
【図１４】図１０に示す値を整形することにより得られた値を示す説明図である。
【図１５】図１４に示す値を用いた場合における差分値の算出結果を示す説明図である。
【図１６】図１４に示す値を用いた場合における勾配行列の算出結果を示す説明図である。
【図１７】図１５に示す差分値の算出結果及び図１６に示す勾配行例の算出結果に基づく補正量の算出結果を示す説明図である。
【図１８】第１最適化処理による最適化後の三次元座標の例を示す説明図である。
【図１９】図１８に示す値及び図１１に示す値を整形することにより得られた値を示す説明図である。
【図２０】図１９に示す値を用いた場合における差分値の算出結果を示す説明図である。
【図２１】図１９に示す値を用いた場合における勾配行列の算出結果を示す説明図である。
【図２２】図２０に示す差分値の算出結果及び図２１に示す勾配行例の算出結果に基づく補正量の算出結果を示す説明図である。
【図２３】第２最適化処理による最適化後の三次元座標の例、及び第２最適化処理による最適化後の姿勢角度の例を示す説明図である。
【図２４】各時刻における台車の三次元座標の例、及び各時刻における台車の姿勢角度の例を示す説明図である。
【図２５】各時刻における台車の三次元座標の他の例、及び各時刻における台車の姿勢角度の他の例を示す説明図である。
【発明を実施するための形態】
【００１２】
実施の形態１．
図１は、実施の形態１に係る位置姿勢推定装置を含む測量装置の要部を示す斜視図である。図２は、実施の形態１に係る位置姿勢推定装置を含む測量装置の要部を示すブロック図である。図３は、実施の形態１に係る位置姿勢推定装置を含む測量装置における計算機の要部を示すブロック図である。図１〜図３を参照して、実施の形態１に係る位置姿勢推定装置を含む測量装置について説明する。
【００１３】
測量装置２００は、測量用の移動体を有している。移動体は、例えば、台車１により構成されている。
【００１４】
台車１に位置測定用の反射材が設けられている。反射材は、例えば、全周型のプリズム２により構成されている。また、測量装置２００は、位置測定用の測定器を有している。測定器は、例えば、自動追尾型のトータルステーション３により構成されている。すなわち、トータルステーション３は、プリズム２に位置測定用の光を照射する。プリズム２は、当該照射された光を反射する。トータルステーション３は、当該反射された光を受信する。これにより、プリズム２の位置が測定される。トータルステーション３は、プリズム２を追尾する機能を有している。
【００１５】
台車１に撮像機が設けられている。撮像機は、例えば、ステレオカメラ４により構成されている。すなわち、ステレオカメラ４は、右眼カメラ５及び左眼カメラ６を有している。右眼カメラ５による視野は、左眼カメラ６による視野と重複している。右眼カメラ５及び左眼カメラ６の各々は、デジタルカメラにより構成されている。ステレオカメラ４は、任意の所定の領域（以下「撮像対象領域」という。）を撮像するものである。撮像対象領域は、例えば、測量装置２００による測量対象物を含む領域である。測量対象物は、例えば、地面である。以下、測量対象物を含む領域が撮像対象領域に設定されており、かつ、測量対象物が地面である場合の例を中心に説明する。
【００１６】
台車１が走行しているとき、トータルステーション３がプリズム２の位置を複数回測定する。記憶装置７は、当該測定された位置を示す情報（以下「位置情報」という。）を記憶するものである。また、このとき、トータルステーション３による測定タイミングと同期された撮像タイミングにて、右眼カメラ５及び左眼カメラ６の各々が撮像対象領域を複数回撮像する。記憶装置７は、当該撮像された画像を示す情報（以下「画像情報」という。）を記憶するものである。記憶装置７は、例えば、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ  Ｓｔａｔｅ  Ｄｒｉｖｅ）により構成されている。
【００１７】
以下、トータルステーション３による測定タイミング及びステレオカメラ４による撮像タイミングを総称して単に「タイミング」ということがある。また、右眼カメラ５により撮像された画像を「右眼画像」という。また、左眼カメラ６により撮像された画像を「左眼画像」という。また、右眼画像及び左眼画像を総称して「ステレオ画像」又は「デジタル画像」ということがある。
【００１８】
位置情報取得部１１は、記憶装置７に記憶されている位置情報を取得するものである。画像情報取得部１２は、記憶装置７に記憶されている画像情報を取得するものである。
【００１９】
姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報及び画像情報取得部１２により取得された画像情報を用いて、各タイミングにおけるプリズム２の姿勢を算出するものである。姿勢情報生成部１３は、当該算出された姿勢を示す情報（以下「姿勢情報」という。）を生成するものである。
【００２０】
ここで、姿勢情報生成部１３は、デジタル画像における接合点（以下「タイポイント」という。）の位置に基づき、プリズム２の姿勢を算出するようになっている。
【００２１】
個々のタイポイントは、例えば、撮像対象領域における地点に対応するものである。この場合、個々の地点の位置は、測量装置２００において未知であっても良い。または、例えば、個々のタイポイントは、撮像対象領域に設けられたマーカに対応するものである。この場合、個々のマーカの位置は、測量装置２００において既知であっても良く、又は測量装置２００において未知であっても良い。
【００２２】
以下、個々のタイポイントが地点に対応するものである場合において、かかる地点の位置が未知であるとき、かかる地点を「未知の地点」ということがある。また、個々のタイポイントがマーカに対応するものである場合において、かかるマーカの位置が既知であるとき、かかるマーカを「既知のマーカ」ということがある。また、個々のタイポイントがマーカに対応するものである場合において、かかるマーカの位置が未知であるとき、かかるマーカを「未知のマーカ」ということがある。
【００２３】
位置姿勢算出部１４は、位置情報取得部１１により取得された位置情報及び姿勢情報生成部１３により生成された姿勢情報を用いて、各タイミングにおける台車１の位置及び台車１の姿勢を算出するものである。
【００２４】
三次元形状算出部１５は、画像情報取得部１２により取得された画像情報を用いて、位置姿勢算出部１４により算出された位置及び姿勢に基づき、測量対象物の三次元形状を算出するものである。具体的には、例えば、三次元形状算出部１５は、いわゆる「多視点ステレオ」により測量対象物の三次元形状を算出する。多視点ステレオには、公知の種々の技術を用いることができる。これらの技術についての詳細な説明は省略する。
【００２５】
位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５により、計算機８の要部が構成されている。また、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３及び位置姿勢算出部１４により、位置姿勢推定装置１００の要部が構成されている。
【００２６】
このようにして、測量装置２００の要部が構成されている。
【００２７】
次に、図４を参照して、計算機８の要部のハードウェア構成について説明する。
【００２８】
図４Ａに示す如く、計算機８は、プロセッサ２１及びメモリ２２を有している。メモリ２２には、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５の機能を実現するためのプログラムが記憶されている。当該記憶されているプログラムをプロセッサ２１が読み出して実行することにより、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５の機能が実現される。
【００２９】
または、図４Ｂに示す如く、計算機８は、処理回路２３を有している。この場合、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５の機能が専用の処理回路２３により実現される。
【００３０】
または、計算機８は、プロセッサ２１、メモリ２２及び処理回路２３を有している（不図示）。この場合、位置情報取得部１１、画像情報取得部１２、姿勢情報生成部１３、位置姿勢算出部１４及び三次元形状算出部１５の機能のうちの一部の機能がプロセッサ２１及びメモリ２２により実現されて、残余の機能が専用の処理回路２３により実現される。
【００３１】
プロセッサ２１は、１個又は複数個のプロセッサにより構成されている。個々のプロセッサは、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、マイクロプロセッサ、マイクロコントローラ又はＤＳＰ（Ｄｉｇｉｔａｌ  Ｓｉｇｎａｌ  Ｐｒｏｃｅｓｓｏｒ）を用いたものである。
【００３２】
メモリ２２は、１個又は複数個の不揮発性メモリにより構成されている。または、メモリ２２は、１個又は複数個の不揮発性メモリ及び１個又は複数個の揮発性メモリにより構成されている。個々の揮発性メモリは、例えば、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）を用いたものである。個々の不揮発性メモリは、例えば、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ  Ｅｒａｓａｂｌｅ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｒｅａｄ−Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＳＳＤ又はＨＤＤを用いたものである。
【００３３】
処理回路２３は、１個又は複数個のデジタル回路により構成されている。または、処理回路２３は、１個又は複数個のデジタル回路及び１個又は複数個のアナログ回路により構成されている。すなわち、処理回路２３は、１個又は複数個の処理回路により構成されている。個々の処理回路は、例えば、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ  Ｓｐｅｃｉｆｉｃ  Ｉｎｔｅｇｒａｔｅｄ  Ｃｉｒｃｕｉｔ）、ＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ  Ｌｏｇｉｃ  Ｄｅｖｉｃｅ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ  Ｇａｔｅ  Ａｒｒａｙ）、ＳｏＣ（Ｓｙｓｔｅｍ−ｏｎ−ａ−Ｃｈｉｐ）又はシステムＬＳＩ（Ｌａｒｇｅ−Ｓｃａｌｅ  Ｉｎｔｅｇｒａｔｉｏｎ）を用いたものである。
【００３４】
次に、位置姿勢推定装置１００において使用される座標系について説明する。また、これらの座標系における位置座標について説明する。
【００３５】
世界座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。世界座標系におけるＸＹ平面は、地面に対して平行又は略平行である。世界座標系におけるＺ軸の正方向は、天頂方向を示している。
【００３６】
プリズム座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。プリズム座標系は、プリズム２を基準とする座標系である。プリズム座標系におけるＸＹ平面は、台車１が地面に載置されたとき、地面に対して平行又は略平行となる。プリズム座標系におけるＸ軸の正方向は、台車１が地面に載置されたとき、台車１の正面方向を示すものとなる。プリズム座標系におけるＺ軸の正方向は、台車１が地面に載置されたとき、天頂方向を示すものとなる。
【００３７】
右眼カメラ座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。右眼カメラ座標系の原点は、右眼カメラ５の投影中心に対応している。右眼カメラ座標系におけるＸＹ平面は、右眼カメラ５の投影面に対して平行である。右眼カメラ座標系におけるＸ軸の正方向は、右眼カメラ５の投影面における上方向に対応している。右眼カメラ座標系におけるＹ軸の正方向は、右眼カメラ５の投影面における左方向に対応している。
【００３８】
左眼カメラ座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。左眼カメラ座標系の原点は、左眼カメラ６の投影中心に対応している。左眼カメラ座標系におけるＸＹ平面は、左眼カメラ６の投影面に対して平行である。左眼カメラ座標系におけるＸ軸の正方向は、左眼カメラ６の投影面における上方向に対応している。左眼カメラ座標系におけるＹ軸の正方向は、左眼カメラ６の投影面における左方向に対応している。
【００３９】
台車座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する任意の三次元座標系である。台車座標系は、台車１を基準とする座標系である。台車座標系は、測量装置２００の用途等に応じて設定されるものである。具体的には、例えば、台車座標系は、右眼カメラ座標系と一致する座標系に設定される。または、例えば、台車座標系は、左眼カメラ座標系と一致する座標系に設定される。
【００４０】
ここで、プリズム座標系の原点は、プリズム２の位置に対応している。したがって、世界座標系におけるプリズム座標系の原点の位置を示す座標は、世界座標系におけるプリズム２の位置を示すものである。また、台車座標系の原点は、台車１の位置に対応している。したがって、世界座標系における台車座標系の原点の位置を示す座標は、世界座標系における台車１の位置を示すものである。
【００４１】
以下、世界座標系、プリズム座標系、右眼カメラ座標系、左眼カメラ座標系又は台車座標系における位置座標を「三次元座標」という。特に、世界座標系における個々のタイポイントの三次元座標を「タイポイント位置」ということがある。また、右眼カメラ５の投影面に沿う二次元座標系、又は左眼カメラ６の投影面に沿う二次元座標系における位置座標を「投影座標」という。
【００４２】
デジタル画像座標系は、互いに直交するＵ軸及びＶ軸を有する画像座標系である。デジタル画像座標系における原点は、個々のデジタル画像における左上の隅に対応している。デジタル画像座標系におけるＵ軸の正方向は、個々のデジタル画像における右方向に対応している。デジタル画像座標系におけるＶ軸の正方向は、個々のデジタル画像における下方向に対応している。
【００４３】
正規化画像座標系は、互いに直交するＵ軸及びＶ軸を有する画像座標系である。正規化画像座標系における原点は、個々のデジタル画像における中心に対応している。正規化画像座標系におけるＵ軸の正方向は、個々のデジタル画像における上方向に対応している。正規化画像座標系におけるＶ軸の正方向は、個々のデジタル画像における左方向に対応している。
【００４４】
以下、デジタル画像座標系又は正規化画像座標系における位置座標を「画像座標」という。特に、デジタル画像座標系又は正規化画像座標系における個々のタイポイントの画像座標を「タイポイント座標」ということがある。
【００４５】
次に、図５のフローチャートを参照して、計算機８の動作について説明する。
【００４６】
なお、ステップＳＴ１の処理が実行されるよりも先に、トータルステーション３がプリズム２の位置を複数回測定して、記憶装置７に位置情報が記憶されているものとする。また、ステレオカメラ４が撮像対象領域を複数回撮像して、記憶装置７に画像情報が記憶されているものとする。
【００４７】
まず、ステップＳＴ１にて、位置情報取得部１１は、記憶装置７に記憶されている位置情報を取得する。当該取得された位置情報は、各タイミングにおける世界座標系におけるプリズム２の三次元座標（ｘｐ，ｙｐ，ｚｐ）を示している。
【００４８】
次いで、ステップＳＴ２にて、画像情報取得部１２は、記憶装置７に記憶されている画像情報を取得する。当該取得された画像情報は、各タイミングにて撮像された右眼画像及び左眼画像を示している。
【００４９】
次いで、ステップＳＴ３にて、姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報及び画像情報取得部１２により取得された画像情報を用いて、各タイミングにおけるプリズム２の姿勢を算出する。より具体的には、姿勢情報生成部１３は、各タイミングにおけるプリズム２の姿勢角度（ωｐ，φｐ，κｐ）の値を算出する。これにより、当該算出された姿勢角度（ωｐ，φｐ，κｐ）の値を示す姿勢情報が生成される。
【００５０】
ここで、姿勢角度（ωｐ，φｐ，κｐ）は、世界座標系の座標軸に対するプリズム座標系の座標軸の回転角に対応している。すなわち、姿勢角度（ωｐ，φｐ，κｐ）は、オイラー角を示している。より具体的には、ωｐがロール角を示しており、φｐがピッチ角を示しており、κｐがヨー角を示している。
【００５１】
次いで、ステップＳＴ４にて、位置姿勢算出部１４は、位置情報取得部１１により取得された位置情報及び姿勢情報生成部１３により生成された姿勢情報を用いて、各タイミングにおける台車１の位置及び台車１の姿勢を算出する。より具体的には、位置姿勢算出部１４は、各タイミングにおける世界座標系における台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）の値を算出する。また、位置姿勢算出部１４は、各タイミングにおける台車１の姿勢角度（ωｔ，φｔ，κｔ）の値を算出する。
【００５２】
ここで、姿勢角度（ωｔ，φｔ，κｔ）は、世界座標系の座標軸に対する台車座標系の座標軸の回転角に対応している。すなわち、姿勢角度（ωｔ，φｔ，κｔ）は、オイラー角を示している。より具体的には、ωｔがロール角を示しており、φｔがピッチ角を示しており、κｔがヨー角を示している。
【００５３】
次いで、ステップＳＴ５にて、三次元形状算出部１５は、画像情報取得部１２により取得された画像情報を用いて、位置姿勢算出部１４により算出された位置及び姿勢に基づき、測量対象物の三次元形状を算出する。具体的には、例えば、三次元形状算出部１５は、多視点ステレオにより測量対象物の三次元形状を算出する。
【００５４】
次に、図６のフローチャートを参照して、姿勢情報生成部１３の詳細な動作について説明する。すなわち、ステップＳＴ３の詳細な処理内容について説明する。
【００５５】
まず、ステップＳＴ１１にて、姿勢情報生成部１３は、各タイミングにて撮像された右眼画像及び左眼画像の各々における、デジタル画像座標系における個々のタイポイントの画像座標（ｕ，ｖ）の値を算出する。ステップＳＴ１１の処理には、画像情報取得部１２により取得された画像情報が用いられる。
【００５６】
次いで、ステップＳＴ１２にて、姿勢情報生成部１３は、世界座標系における個々のタイポイントの三次元座標（ｘＴ，ｙＴ，ｚＴ）の初期値を算出する。ステップＳＴ１２の処理には、位置情報取得部１１により取得された位置情報が用いられる。
【００５７】
次いで、ステップＳＴ１３にて、姿勢情報生成部１３は、各タイミングにおけるプリズム２の姿勢角度（ωｐ，φｐ，κｐ）の初期値を算出する。ステップＳＴ１３の処理には、位置情報取得部１１により取得された位置情報が用いられる。
【００５８】
次いで、ステップＳＴ１４にて、姿勢情報生成部１３は、ステップＳＴ１２における初期値に比して最適化された三次元座標（ｘＴ，ｙＴ，ｚＴ）の値を算出する処理（以下「第１最適化処理」という。）を実行する。
【００５９】
次いで、ステップＳＴ１５にて、姿勢情報生成部１３は、ステップＳＴ１４にて算出された三次元座標（ｘＴ，ｙＴ，ｚＴ）に比して更に最適化された三次元座標（ｘＴ，ｙＴ，ｚＴ）の値を算出するとともに、ステップＳＴ１３における初期値に比して最適化された姿勢角度（ωｐ，φｐ，κｐ）の値を算出する処理（以下「第２最適化処理」という。）を実行する。これにより、第２最適化処理による最適化後の姿勢角度（ωｐ，φｐ，κｐ）の値を示す姿勢情報が生成される。
【００６０】
次に、図７を参照して、ステップＳＴ１における、位置情報取得部１１により取得される位置情報の具体例について説明する。また、図８を参照して、ステップＳＴ２における、画像情報取得部１２により取得される画像情報の具体例について説明する。
【００６１】
上記のとおり、台車１が走行しているとき、トータルステーション３がプリズム２の位置を複数回測定するとともに、ステレオカメラ４が撮像対象領域を複数回撮像する。このとき、台車１の進行方向は、台車１の正面方向に対して同一又は略同一の方向に設定される。
【００６２】
図７は、プリズム２の位置が４回測定された場合における位置情報の例を示している。図８は、撮像対象領域が４回撮像された場合における画像情報の例を示している。図中、ｔ０〜ｔ３は、トータルステーション３による測定タイミングに対応する時刻、すなわちステレオカメラ４による撮像タイミングに対応する時刻を示している。
【００６３】
図７に示す位置情報は、各時刻ｔ０〜ｔ３における世界座標系におけるプリズム２の三次元座標（ｘｐ，ｙｐ，ｚｐ）を示している。また、図８に示す画像情報は、各時刻ｔ０〜ｔ３にて撮像された右眼画像及び左眼画像を示している。
【００６４】
次に、図９を参照して、ステップＳＴ１１における、姿勢情報生成部１３による個々のタイポイントの画像座標（ｕ，ｖ）の値の算出方法の具体例について説明する。
【００６５】
上記のとおり、個々のタイポイントは、例えば、未知の地点、既知のマーカ又は未知のマーカに対応するものである。
【００６６】
個々のタイポイントが未知の地点に対応するものである場合、例えば、姿勢情報生成部１３は、個々のデジタル画像に対する画像処理を実行する。これにより、姿勢情報生成部１３は、複数枚のデジタル画像に共通して含まれる特徴点を抽出する。姿勢情報生成部１３は、当該抽出された特徴点をタイポイントに用いる。すなわち、姿勢情報生成部１３は、当該抽出された特徴点の画像座標（ｕ，ｖ）の値を算出する。
【００６７】
かかる特徴点の抽出には、公知の種々の技術を用いることができる。これらの技術についての詳細な説明は省略する。具体的には、例えば、以下の参考文献１に記載された技術を用いることができる。
【００６８】
［参考文献１］
David G. Lowe, "Distinctive Image Features from Scale-Invariant Keypoints," International Journal of Computer Vision, 2004, 60(2), pp. 91-110.
【００６９】
個々のタイポイントが既知のマーカ又は未知のマーカに対応するものである場合、例えば、姿勢情報生成部１３は、個々のデジタル画像に対して、いわゆる「テンプレートマッチング」の画像処理を実行する。これにより、姿勢情報生成部１３は、個々のデジタル画像に含まれるマーカを検出する。姿勢情報生成部１３は、当該検出されたマーカの画像座標（ｕ，ｖ）の値を算出する。
【００７０】
または、例えば、個々のタイポイントが既知のマーカ又は未知のマーカである場合、測量装置２００のユーザは、個々のデジタル画像におけるマーカの位置を目視により確認して、当該確認された位置を計算機８に入力する。姿勢情報生成部１３は、当該入力された位置に対応する画像座標（ｕ，ｖ）の値を算出する。
【００７１】
図９は、個々のタイポイントが未知のマーカに対応するものである場合における、個々のタイポイントの画像座標（ｕ，ｖ）の値の例を示している。図９に示す値は、図８に示す画像情報を用いて算出されたものである。
【００７２】
次に、図１０を参照して、ステップＳＴ１２における、姿勢情報生成部１３による個々のタイポイントの三次元座標（ｘＴ，ｙＴ，ｚＴ）の初期値の算出方法の具体例について説明する。
【００７３】
姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報に含まれる全てのＸ座標（ｘｐ）の平均値を算出する。姿勢情報生成部１３は、当該算出された平均値を各タイポイントのＸ座標（ｘＴ）の初期値に用いる。
【００７４】
また、姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報に含まれる全てのＹ座標（ｙｐ）の平均値を算出する。姿勢情報生成部１３は、当該算出された平均値を各タイポイントのＹ座標（ｙＴ）の初期値に用いる。
【００７５】
また、姿勢情報生成部１３は、位置情報取得部１１により取得された位置情報に含まれるＺ座標（ｚｐ）の値を用いて、地面における任意の地点のＺ座標の値を算出する。姿勢情報生成部１３は、当該算出されたＺ座標の値を各タイポイントのＺ座標（ｚＴ）の初期値に用いる。
【００７６】
なお、地面における任意の地点のＺ座標の値を算出するために、全周型のプリズム２と異なる他のプリズム（不図示）が台車１に設けられているものであっても良い。姿勢情報生成部１３は、トータルステーション３により測定された当該他のプリズムの位置に基づき、地面における任意の地点のＺ座標の値を算出するものであっても良い。また、当該他のプリズムは、台車１に設けられているのに代えて、測量装置２００のユーザの手により保持されているものであっても良い。
【００７７】
このようにして、世界座標系における個々のタイポイントの三次元座標（ｘＴ，ｙＴ，ｚＴ）の初期値が算出される。図１０は、上記他のプリズムを用いた場合における、個々のタイポイントの三次元座標（ｘＴ，ｙＴ，ｚＴ）の初期値の例を示している。図１０に示す初期値は、図７に示す位置情報を用いて算出されたものである。
【００７８】
次に、図１１を参照して、ステップＳＴ１３における、姿勢情報生成部１３によるプリズム２の姿勢角度（ωｐ，φｐ，κｐ）の初期値の算出方法の具体例について説明する。
【００７９】
時刻ｔにおけるプリズム２の三次元座標が（ｘ，ｙ，ｚ）であるものとする。時刻ｔに対応する測定タイミングに対する次回の測定タイミングに対応する時刻がｔ’であるものとする。時刻ｔ’におけるプリズム２の三次元座標が（ｘ’，ｙ’，ｚ’）であるものとする。
【００８０】
上記のとおり、プリズム座標系におけるＸ軸の正方向は、台車１が地面に載置されたとき、台車１の正面方向を示すものとなる。また、台車１の進行方向は、台車１の正面方向と同一又は略同一の方向に設定される。したがって、プリズム２の姿勢角度（ωｐ，φｐ，κｐ）は、Ｘ座標（１，０，０）を台車１の移動ベクトル（ｘ−ｘ’，ｙ−ｙ’，ｚ−ｚ’）と一致させる回転角により近似される。
【００８１】
ここで、台車１は、世界座標系におけるＸＹ平面に沿うように移動する。このため、ロール角（ωｐ）及びピッチ角（φｐ）の各々は、０に近似される。また、ヨー角（κｐ）は、ＸＹ平面におけるベクトル（１，０）をベクトル（ｘ−ｘ’，ｙ−ｙ’）と一致させる回転角により近似される。したがって、ヨー角（κｐ）について、以下の式（１）が成立する。
【００８２】
2020186944.tif 000003
【００８３】
ヨー角（κｐ）は、上記式（１）により算出される。すなわち、ヨー角（κｐ）は、逆三角関数により算出される。
【００８４】
なお、時刻ｔに対応する測定タイミングに対する前回の測定タイミングに対応する時刻がｔ”であるものとする。時刻ｔよりも後の時刻におけるプリズム２の位置が未測定である場合、時刻ｔにおける姿勢角度（ωｐ，φｐ，κｐ）の初期値に、時刻ｔ”における姿勢角度（ωｐ，φｐ，κｐ）の初期値が用いられる。時刻ｔ”における姿勢角度（ωｐ，φｐ，κｐ）の初期値は、時刻ｔにおけるプリズム２の三次元座標と、時刻ｔ”におけるプリズム２の三次元座標とを用いて算出される。
【００８５】
図１１は、各時刻ｔ０〜ｔ３におけるプリズム２の姿勢角度（ωｐ，φｐ，κｐ）の初期値の例を示している。図１１に示す初期値は、図７に示す位置情報を用いて算出されたものである。
【００８６】
次に、図１２〜図１８を参照して、ステップＳＴ１４における、姿勢情報生成部１３による第１最適化処理の具体例について説明する。
【００８７】
まず、姿勢情報生成部１３は、以下のようにして、右眼カメラ５における個々のタイポイントの投影座標（０ｕｒ，０ｖｒ）を算出する。また、姿勢情報生成部１３は、左眼カメラ６における個々のタイポイントの投影座標（０ｕｌ，０ｖｌ）を算出する。これらの投影座標（０ｕｒ，０ｖｒ），（０ｕｌ，０ｖｌ）は、正規化画像座標系における位置座標である。
【００８８】
世界座標系におけるプリズム座標系の原点の三次元座標が（ｘｐ，ｙｐ，ｚｐ）であるものとする。また、世界座標系の座標軸に対するプリズム座標系の座標軸の回転角が（ωｐ，φｐ，κｐ）であるものとする。この場合、任意の点Ｘについて、世界座標系における三次元座標Ｘｗとプリズム座標系における三次元座標Ｘｐとの変換式は、以下の式（２）により表される。
【００８９】
2020186944.tif 000004
【００９０】
ただし、以下の式（３）及び式（４）が成立している。
【００９１】
2020186944.tif 000005
【００９２】
2020186944.tif 000006
【００９３】
また、世界座標系における右眼カメラ座標系の原点の三次元座標が（ｘｒ，ｙｒ，ｚｒ）であるものとする。また、世界座標系の座標軸に対する右眼カメラ座標系の座標軸の回転角が（ωｒ，φｒ，κｒ）であるものとする。この場合、任意の点Ｘについて、世界座標系における三次元座標Ｘｐと右眼カメラ座標系における三次元座標Ｘｒとの変換式は、以下の式（５）により表される。
【００９４】
2020186944.tif 000007
【００９５】
ただし、以下の式（６）及び式（７）が成立している。
【００９６】
2020186944.tif 000008
【００９７】
2020186944.tif 000009
【００９８】
上記式（２）及び上記式（５）に基づき、任意の点Ｘについて、世界座標系における三次元座標Ｘｗと右眼カメラ座標系における三次元座標Ｘｒとの変換式は、以下の式（８）により表される。
【００９９】
2020186944.tif 000010
【０１００】
ただし、以下の式（９）及び式（１０）が成立している。
【０１０１】
2020186944.tif 000011
【０１０２】
2020186944.tif 000012
【０１０３】
上記式（８）に基づき、Ｘｗに対応する三次元座標を有するタイポイントについて、右眼カメラ５における投影座標（０ｕｒ，０ｖｒ）が算出される。すなわち、右眼カメラ５の焦点距離がｆｒであり、右眼カメラ５における撮像素子の幅がＣｒであり、右眼画像の横幅がＷｒであるものとする。このとき、三次元座標Ｘｗを投影座標（０ｕｒ，０ｖｒ）に変換する変換式は、以下の式（１１）により表される。
【０１０４】
2020186944.tif 000013
【０１０５】
ただし、以下の式（１２）が成立している。
【０１０６】
2020186944.tif 000014
【０１０７】
上記式（１１）を用いて三次元座標Ｘｗが投影座標（０ｕｒ，０ｖｒ）に変換されることにより、右眼カメラ５における投影座標（０ｕｒ，０ｖｒ）が算出される。これと同様にして、左眼カメラ６における投影座標（０ｕｌ，０ｖｌ）が算出される。
【０１０８】
すなわち、世界座標系における左眼カメラ座標系の原点の三次元座標が（ｘｌ，ｙｌ，ｚｌ）であるものとする。また、世界座標系の座標軸に対する左眼カメラ座標系の座標軸の回転角が（ωｌ，φｌ，κｌ）であるものとする。また、左眼カメラ６の焦点距離がｆｌであり、左眼カメラ６における撮像素子の幅がＣｌであり、左眼画像の横幅がＷｌであるものとする。これらの値に基づき、三次元座標Ｘｗを投影座標（０ｕｌ，０ｖｌ）に変換する変換式を用いて、左眼カメラ６における投影座標（０ｕｌ，０ｖｌ）が算出される。
【０１０９】
次いで、姿勢情報生成部１３は、以下のようにして、ステップＳＴ１１にて算出された画像座標（ｕ，ｖ）における歪みを補正する。
【０１１０】
右眼画像において、デジタル画像座標系におけるタイポイントの画像座標が（１ｕｒ，１ｖｒ）であるものとする。右眼カメラ５の主点位置を示す位置座標がＸｐｒ及びＹｐｒであり、右眼カメラ５における歪み係数がｋ１ｒ、ｋ２ｒ、ｋ３ｒ、ｐ１ｒ、ｐ２ｒ、ｂ１ｒ及びｂ２ｒであり、右眼カメラ５における撮像素子の幅がＣｒであり、右眼画像の横幅がＷｒであり、右眼画像の縦幅がＨｒであるものとする。このとき、デジタル画像座標系における歪みを有するタイポイント座標（１ｕｒ，１ｖｒ）を、正規化画像座標系における歪み補正後のタイポイント座標（２ｕｒ，２ｖｒ）に変換する変換式は、以下の式（１３）により表される。
【０１１１】
2020186944.tif 000015
【０１１２】
ただし、以下の式（１４）〜式（１９）が成立している。
【０１１３】
2020186944.tif 000016
【０１１４】
2020186944.tif 000017
【０１１５】
2020186944.tif 000018
【０１１６】
2020186944.tif 000019
【０１１７】
2020186944.tif 000020
【０１１８】
2020186944.tif 000021
【０１１９】
上記式（１３）を用いて、デジタル画像座標系における歪みを有するタイポイント座標（１ｕｒ，１ｖｒ）が、正規化画像座標系における歪み補正後のタイポイント座標（２ｕｒ，２ｖｒ）に変換される。これと同様にして、デジタル画像座標系における歪みを有するタイポイント座標（１ｕｌ，１ｖｌ）が、正規化画像座標系における歪み補正後のタイポイント座標（２ｕｌ，２ｖｌ）に変換される。
【０１２０】
すなわち、左眼画像において、デジタル画像座標系におけるタイポイントの画像座標が（１ｕｌ，１ｖｌ）であるものとする。左眼カメラ６の主点位置を示す位置座標がＸｐｌ及びＹｐｌであり、左眼カメラ６における歪み係数がｋ１ｌ、ｋ２ｌ、ｋ３ｌ、ｐ１ｌ、ｐ２ｌ、ｂ１ｌ及びｂ２ｌであり、左眼カメラ６における撮像素子の幅がＣｌであり、右眼画像の横幅がＷｌであり、右眼画像の縦幅がＨｌであるものとする。このとき、上記式（１３）と同様の変換式を用いて、デジタル画像座標系における歪みを有するタイポイント座標（１ｕｌ，１ｖｌ）が、正規化画像座標系における歪み補正後のタイポイント座標（２ｕｌ，２ｖｌ）に変換される。
【０１２１】
次いで、姿勢情報生成部１３は、以下のようにして、上記算出された投影座標（０ｕｒ，０ｖｒ）と、上記算出された画像座標（２ｕｒ，２ｖｒ）との差分値を最小化する。また、姿勢情報生成部１３は、上記算出された投影座標（０ｕｌ，０ｖｌ）と、上記算出された画像座標（２ｕｌ，２ｖｌ）との差分値を最小化する。これにより、タイポイントの三次元座標が最適化される。
【０１２２】
ステップＳＴ２にて取得された画像情報に、ｍ枚の右眼画像及びｍ枚の左眼画像が含まれているものとする。ｍ枚の右眼画像のうちのｋ番目の右眼画像に含まれているタイポイントの個数がｐｋ個であり、ｍ枚の左眼画像のうちのｋ番目の左眼画像に含まれているタイポイントの個数がｑｋ個であるものとする。また、ステップＳＴ１１にてｎ個のタイポイントＴ１，Ｔ２，……，Ｔｎが検出されており、ｎ個のタイポイントＴ１，Ｔ２，……，Ｔｎの三次元座標がそれぞれ（ｘＴ１，ｙＴ１，ｚＴ１），（ｘＴ２，ｙＴ２，ｚＴ２），……，（ｘＴｎ，ｙＴｎ，ｚＴｎ）であるものとする。
【０１２３】
上記式（１１）及び上記式（１３）に基づき、以下の式（２０）に示す関数が定義される。当該関数における引数は、タイポイントの三次元座標である。当該関数における返り値は、タイポイントの投影座標とタイポイントの画像座標との差分値である。
【０１２４】
2020186944.tif 000022
【０１２５】
上記式（２０）において、０ｕｒ，ａ，ｂ及び０ｖｒ，ａ，ｂは、ａ番目の右眼画像におけるｂ番目のタイポイントについて上記式（１１）を用いて算出された投影座標の値を示している。また、０ｕｌ，ａ，ｂ及び０ｖｌ，ａ，ｂは、ａ番目の左眼画像におけるｂ番目のタイポイントについて上記式（１１）と同様の式を用いて算出された投影座標の値を示している。また、２ｕｒ，ａ，ｂ及び２ｖｒ，ａ，ｂは、ａ番目の右眼画像におけるｂ番目のタイポイントについて上記式（１３）を用いて算出された歪み補正後の画像座標の値を示している。また、２ｕｌ，ａ，ｂ及び２ｖｌ，ａ，ｂは、ａ番目の左眼画像におけるｂ番目のタイポイントについて上記式（１３）と同様の式を用いて算出された歪み補正後の画像座標の値を示している。
【０１２６】
ここで、図１２は、右眼カメラ５及び右眼画像に関するパラメータの値の具体例を示している。図１３は、左眼カメラ６及び左眼画像に関するパラメータの値の具体例を示している。これらのパラメータは、上記式（１１）及び上記式（１３）に基づく上記式（２０）の計算に用いられるものである。
【０１２７】
上記式（１１）及び上記（１３）に基づく上記式（２０）の計算において、タイポイントの三次元座標を除くパラメータは、いずれも定数とすることができる（図１２及び図１３参照）。また、一部のタイポイントが既知のマーカに対応するものである場合、当該一部のタイポイントの三次元座標も定数とすることができる。これにより、残余のタイポイントの三次元座標の算出精度の向上を図ることができる。ただし、上記のとおり、全てのタイポイントが未知の地点又は未知のマーカに対応するものであっても良い。
【０１２８】
姿勢情報生成部１３は、上記式（２０）に示す関数において、返り値に対応するベクトルの大きさを最小にする最適化計算を実行する。これにより、姿勢情報生成部１３は、投影座標と画像座標との差分値が最小となるタイポイント位置を求める。以下、かかる最適化計算について説明する。
【０１２９】
まず、タイポイント位置の初期値０ｘＴが以下の式（２１）により表されるものとする。図１４は、図１０に示す初期値（すなわちステップＳＴ１２にて算出された初期値）を整形することにより得られた０ｘＴの値を示している。
【０１３０】
2020186944.tif 000023
【０１３１】
姿勢情報生成部１３は、タイポイント位置の初期値０ｘＴを用いて、以下の式（２２）により、投影座標と画像座標との差分値０ｙＴを算出する。式（２２）の計算には、上記式（２０）に示す関数が用いられる。図１５は、図１４に示す値を用いた場合における差分値０ｙＴの算出結果を示している。
【０１３２】
2020186944.tif 000024
【０１３３】
ここで、タイポイント位置の初期値０ｘＴに補正量１ｘＴを加えることにより、上記式（２０）に示す関数における返り値を最小化することを考える。補正量１ｘＴの算出は、以下の式（２３）により算出される勾配行列を用いて、以下の式（２４）の右辺における残差ベクトルＶを最小化する問題として定式化される。図１６は、図１４に示す値を用いた場合における勾配行列の算出結果を示している。
【０１３４】
2020186944.tif 000025
【０１３５】
2020186944.tif 000026
【０１３６】
上記式（２４）の右辺における残差ベクトルＶを最小化する補正量１ｘＴは、以下の式（２５）に基づき、最小二乗法により算出される。図１７は、図１５に示す差分値０ｙＴの算出結果及び図１６に示す勾配行列の算出結果に基づく補正量１ｘＴの算出結果を示している。
【０１３７】
2020186944.tif 000027
【０１３８】
ただし、以下の式（２６）が成立している。
【０１３９】
2020186944.tif 000028
【０１４０】
このようにして、以下の式（２７）を満たす補正量１ｘＴが算出される。
【０１４１】
2020186944.tif 000029
【０１４２】
得られたタイポイント位置に対して最小二乗法を繰り返し適用することにより、次段落の（２８）に示す如く、投影座標と画像座標との差分値が最小となるタイポイント位置が得られる。ここで、Ｎ１は繰り返し回数を示している。
【０１４３】
2020186944.tif 000030
【０１４４】
図１８は、最小二乗法の計算を３回繰り返すことにより得られたタイポイント位置の値を示している。すなわち、図１８は、第１最適化処理による最適化後の三次元座標（ｘＴ，ｙＴ，ｚＴ）の値の具体例を示している。
【０１４５】
次に、図１９〜図２３を参照して、ステップＳＴ１５における、姿勢情報生成部１３による第２最適化処理の具体例について説明する。
【０１４６】
まず、姿勢情報生成部１３は、第１最適化処理における算出方法と同様の算出方法により、右眼カメラ５における個々のタイポイントの投影座標（０ｕｒ，０ｖｒ）を算出する。また、姿勢情報生成部１３は、左眼カメラ６における個々のタイポイントの投影座標（０ｕｌ，０ｖｌ）を算出する。
【０１４７】
次いで、姿勢情報生成部１３は、第１最適化処理における補正方法と同様の補正方法により、ステップＳＴ１１にて算出された画像座標（ｕ，ｖ）における歪みを補正する。
【０１４８】
次いで、姿勢情報生成部１３は、以下のようにして、上記算出された投影座標（０ｕｒ，０ｖｒ）と、上記算出された画像座標（２ｕｒ，２ｖｒ）との差分値を最小化する。また、姿勢情報生成部１３は、上記算出された投影座標（０ｕｌ，０ｖｌ）と、上記算出された画像座標（２ｕｌ，２ｖｌ）との差分値を最小化する。これにより、タイポイントの三次元座標が最適化されるとともに、プリズム２の姿勢角度が最適化される。
【０１４９】
上記式（１１）及び上記式（１３）に基づき、以下の式（２９）に示す関数が定義される。当該関数における引数は、プリズム２の姿勢角度及びタイポイントの三次元座標である。当該関数における返り値は、タイポイントの投影座標とタイポイントの画像座標との差分値である。
【０１５０】
2020186944.tif 000031
【０１５１】
上記式（２９）において、上記式（２０）に示す符号と同様の符号は、上記（２０）におけるパラメータと同様のパラメータを示している。また、（ωｐｋ，φｐｋ，κｐｋ）は、ｋ番目の右眼画像の撮像タイミングに対応する測定タイミングにおけるプリズム２の姿勢角度を示している。
【０１５２】
上記式（１１）及び上記（１３）に基づく上記式（２９）の計算において、プリズム２の姿勢角度及びタイポイントの三次元座標を除くパラメータは、いずれも定数とすることができる（図１２及び図１３参照）。また、一部のタイポイントが既知のマーカに対応するものである場合、当該一部のタイポイントの三次元座標も定数とすることができる。これにより、プリズム２の姿勢角度の算出精度の向上を図ることができる。ただし、上記のとおり、全てのタイポイントが未知の地点又は未知のマーカに対応するものであっても良い。
【０１５３】
姿勢情報生成部１３は、上記式（２９）に示す関数において、返り値に対応するベクトルの大きさを最小にする最適化計算を実行する。これにより、姿勢情報生成部１３は、投影座標と画像座標との差分値が最小となるタイポイント位置を求める。以下、かかる最適化計算について説明する。
【０１５４】
まず、プリズム２の姿勢角度及びタイポイント位置の初期値０ｘｐＴが以下の式（３０）により表されるものとする。図１９は、図１８に示す値（すなわちステップＳＴ１４にて算出された値）及び図１１に示す初期値（すなわちステップＳＴ１３にて算出された初期値）を整形することにより得られた０ｘｐＴの値を示している。
【０１５５】
2020186944.tif 000032
【０１５６】
姿勢情報生成部１３は、プリズム２の姿勢角度及びタイポイント位置の初期値０ｘＴを用いて、以下の式（３１）により、投影座標と画像座標との差分値０ｙｐＴを算出する。式（３１）の計算には、上記式（２９）に示す関数が用いられる。図２０は、図１９に示す値を用いた場合における差分値０ｙｐＴの算出結果を示している。
【０１５７】
2020186944.tif 000033
【０１５８】
ここで、プリズム２の姿勢角度及びタイポイント位置の初期値０ｘｐＴに補正量１ｘｐＴを加えることにより、上記式（２９）に示す関数における返り値を最小化することを考える。補正量１ｘｐＴの算出は、以下の式（３２）により算出される勾配行列を用いて、以下の式（３３）の右辺における残差ベクトルＶを最小化する問題として定式化される。図２１は、図１９に示す値を用いた場合における勾配行列の算出結果を示している。
【０１５９】
2020186944.tif 000034
【０１６０】
2020186944.tif 000035
【０１６１】
上記式（３２）における残差ベクトルＶを最小化する補正量１ｘｐＴは、以下の式（３４）に基づき、最小二乗法により算出される。図２２は、図２０に示す差分値０ｙｐＴの算出結果及び図２１に示す勾配行列の算出結果に基づく補正量１ｘｐＴの算出結果を示している。
【０１６２】
2020186944.tif 000036
【０１６３】
ただし、以下の式（３５）が成立している。
【０１６４】
2020186944.tif 000037
【０１６５】
このようにして、以下の式（３６）を満たす補正量１ｘｐＴが算出される。
【０１６６】
2020186944.tif 000038
【０１６７】
得られたプリズム２の姿勢角度及びタイポイント位置に対して最小二乗法を繰り返し適用することにより、次段落の（３７）に示す如く、投影座標と画像座標との差分値が最小となるプリズム２の姿勢角度及びタイポイント位置が得られる。ここで、Ｎ２は繰り返し回数を示している。
【０１６８】
2020186944.tif 000039
【０１６９】
図２３は、最小二乗法の計算を６回繰り返すことにより得られたプリズム２の姿勢角度の値及びタイポイント位置の値を示している。すなわち、図２３は、第２最適化処理による最適化後の三次元座標（ｘＴ，ｙＴ，ｚＴ）の値の具体例、及び第２最適化処理による最適化後の姿勢角度（ωｐ，φｐ，κｐ）の値の具体例を示している。
【０１７０】
次に、図２４及び図２５を参照して、ステップＳＴ４における、位置姿勢算出部１４による台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）の値の算出方法の具体例について説明する。また、台車１の姿勢角度（ωｔ，φｔ，κｔ）の値の算出方法の具体例について説明する。
【０１７１】
台車座標系が右眼カメラ座標系と一致する座標系に設定される場合、台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）及び台車１の姿勢角度（ωｔ，φｔ，κｔ）は、上記式（８）に基づき、以下の式（３８）により表される。
【０１７２】
2020186944.tif 000040
【０１７３】
ただし、以下の式（３９）及び式（４０）が成立している。
【０１７４】
2020186944.tif 000041
【０１７５】
2020186944.tif 000042
【０１７６】
この場合、位置姿勢算出部１４は、上記式（３８）を用いて、台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）の値を算出するとともに、台車１の姿勢角度（ωｔ，φｔ，κｔ）の値を算出する。図２４は、この場合における、台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）の値の具体例、及び台車１の姿勢角度（ωｔ，φｔ，κｔ）の値の具体例を示している。なお、図２４に示す値は、図７に示す位置情報、図１２に示すパラメータ、及び図１３に示すパラメータに基づくものである。
【０１７７】
他方、台車座標系が左眼カメラ座標系と一致する座標系に設定される場合、位置姿勢算出部１４は、上記式（３８）と同様の式を用いて、台車１の姿勢角度（ωｔ，φｔ，κｔ）の値を算出するとともに、台車１の姿勢角度（ωｔ，φｔ，κｔ）の値を算出する。図２５は、この場合における、台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）の値の具体例、及び台車１の姿勢角度（ωｔ，φｔ，κｔ）の値の具体例を示している。なお、図２５に示す値は、図７に示す位置情報、図１２に示すパラメータ、及び図１３に示すパラメータに基づくものである。
【０１７８】
このように、ステップＳＴ１〜ＳＴ３の処理が実行されることにより、プリズム２の姿勢を高精度に算出することができる。すなわち、画像情報に加えて位置情報を用いることにより、プリズム２の姿勢を高精度に算出することができる。この結果、ステップＳＴ４の処理が実行されることにより、台車１の姿勢を高精度に推定することができる。また、台車１の位置を高精度に推定することができる。したがって、ステップＳＴ５の処理が実行されることにより、測量対象物の三次元形状を高精度に測定することができる。
【０１７９】
また、姿勢検出用のセンサを不要とすることができる。この結果、測量装置２００を安価に実現することができる。
【０１８０】
次に、測量装置２００の変形例について説明する。
【０１８１】
測量用の移動体は、台車１に限定されるものではない。例えば、移動体は、車両、航空機、船舶又は背負子であっても良い。
【０１８２】
位置測定用の反射材は、全周型のプリズム２に限定されるものではない。例えば、トータルステーション３に対する移動体の相対方向の変動量が小さい場合、反射材は、通常のプリズムを用いたものであっても良い。
【０１８３】
位置測定用の測定器は、自動追尾型のトータルステーション３に限定されるものではない。例えば、各タイミングにて移動体が静止するものである場合、測定器は、自動追尾機能を有しない通常のトータルステーションを用いたものであっても良い。
【０１８４】
撮像機は、２個のデジタルカメラを用いたステレオカメラ４に限定されるものではない。撮像機は、１個以上のデジタルカメラを用いたものであれば良い。ただし、デジタルカメラの個数を増やすことにより、台車１の位置及び姿勢の推定精度を更に向上することができる。他方、デジタルカメラの個数を減らすことにより、測量装置２００を更に安価に実現することができる。
【０１８５】
撮像対象領域は、測量対象物を含む領域に限定されるものではない。また、測量対象物は、地面に限定されるものではない。例えば、測量対象物は、橋梁の表面又はトンネルの内面であっても良い。すなわち、測量対象物は、水平面を含むものであっても良く、鉛直面を含むものであっても良く、傾斜面を含むものであっても良い。また、測量対象物は、平面状であっても良く、曲面状であっても良く、凹凸面状であっても良い。
【０１８６】
なお、測量対象物を含む領域と異なる領域が撮像対象領域に設定されている場合、移動体に距離測定用の測定器（例えばレーザスキャナ）が設けられているものであっても良い。三次元形状算出部１５は、当該測定器により測定された距離、位置姿勢算出部１４により算出された位置、及び位置姿勢算出部１４により算出された姿勢に基づき、測量対象物の三次元形状を算出するものであっても良い。
【０１８７】
姿勢情報生成部１３は、第１最適化処理を実行することなく、第２最適化処理を実行するものであっても良い。すなわち、姿勢情報生成部１３は、ステップＳＴ１３に次いで、ステップＳＴ１５の処理を実行するものであっても良い。この場合における第２最適化処理は、ステップＳＴ１２における初期値に比して最適化された三次元座標（ｘＴ，ｙＴ，ｚＴ）の値を算出するとともに、ステップＳＴ１３における初期値に比して最適化された姿勢角度（ωｐ，φｐ，κｐ）の値を算出する処理であっても良い。
【０１８８】
ただし、第１最適化処理が実行されることにより、ステップＳＴ１２における初期値の真値に対する差分値が大きいときであっても、プリズム２の姿勢を正確に算出することができる。このため、第１最適化処理を実行するのがより好適である。
【０１８９】
以上のように、実施の形態１に係る位置姿勢推定装置１００は、位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定装置１００であって、測定器により測定された位置を示す位置情報を取得する位置情報取得部１１と、撮像機により撮像された画像を示す画像情報を取得する画像情報取得部１２と、位置情報及び画像情報を用いて反射材の姿勢を算出することにより、反射材の姿勢を示す姿勢情報を生成する姿勢情報生成部１３と、位置情報及び姿勢情報を用いて移動体の位置及び移動体の姿勢を算出する位置姿勢算出部１４と、を備える。これにより、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる。
【０１９０】
また、実施の形態１に係る位置姿勢推定方法は、位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定方法であって、位置情報取得部１１が、測定器により測定された位置を示す位置情報を取得するステップＳＴ１と、画像情報取得部１２が、撮像機により撮像された画像を示す画像情報を取得するステップＳＴ２と、姿勢情報生成部１３が、位置情報及び画像情報を用いて反射材の姿勢を算出することにより、反射材の姿勢を示す姿勢情報を生成するステップＳＴ３と、位置姿勢算出部１４が、位置情報及び姿勢情報を用いて移動体の位置及び移動体の姿勢を算出するステップＳＴ４と、を備える。これにより、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる。
【０１９１】
なお、本願発明はその発明の範囲内において、実施の形態の任意の構成要素の変形、もしくは実施の形態の任意の構成要素の省略が可能である。
【符号の説明】
【０１９２】
１  台車（移動体）、２  プリズム（反射材）、３  トータルステーション（測定器）、４  ステレオカメラ（撮像機）、５  右眼カメラ、６  左眼カメラ、７  記憶装置、８  計算機、１１  位置情報取得部、１２  画像情報取得部、１３  姿勢情報生成部、１４  位置姿勢算出部、１５  三次元形状算出部、２１  プロセッサ、２２  メモリ、２３  処理回路、１００  位置姿勢推定装置、２００  測量装置。
【図１】
2020186944.tif 000043
【図２】
2020186944.tif 000044
【図３】
2020186944.tif 000045
【図４Ａ】
2020186944.tif 000046
【図４Ｂ】
2020186944.tif 000047
【図５】
2020186944.tif 000048
【図６】
2020186944.tif 000049
【図７】
2020186944.tif 000050
【図８】
2020186944.tif 000051
【図９Ａ】
2020186944.tif 000052
【図９Ｂ】
2020186944.tif 000053
【図１０】
2020186944.tif 000054
【図１１】
2020186944.tif 000055
【図１２】
2020186944.tif 000056
【図１３】
2020186944.tif 000057
【図１４】
2020186944.tif 000058
【図１５】
2020186944.tif 000059
【図１６】
2020186944.tif 000060
【図１７】
2020186944.tif 000061
【図１８】
2020186944.tif 000062
【図１９】
2020186944.tif 000063
【図２０】
2020186944.tif 000064
【図２１】
2020186944.tif 000065
【図２２】
2020186944.tif 000066
【図２３】
2020186944.tif 000067
【図２４】
2020186944.tif 000068
【図２５】
2020186944.tif 000069