特開 2023-143280 カメラ姿勢の演算装置、演算方法および情報処理プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2023143280

(43)【公開日】2023-10-06

(54)【発明の名称】カメラ姿勢の演算装置、演算方法および情報処理プログラム

(51)【国際特許分類】

   G01B 11/26 20060101AFI20230928BHJP

   G01B 11/00 20060101ALI20230928BHJP

   G06T 7/70 20170101ALI20230928BHJP

   G06T 7/60 20170101ALI20230928BHJP

【ＦＩ】

G01B11/26 H

G01B11/00 H

G06T7/70 Z

G06T7/60 150D

G06T7/60 200G

【審査請求】未請求

【請求項の数】10

【出願形態】ＯＬ

(21)【出願番号】P 2022050569

(22)【出願日】2022-03-25

(71)【出願人】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(71)【出願人】

【識別番号】000003218

【氏名又は名称】株式会社豊田自動織機

(74)【代理人】

【識別番号】110000110

【氏名又は名称】弁理士法人 快友国際特許事務所

(72)【発明者】

【氏名】津坂 祐司

(72)【発明者】

【氏名】田中 稔

(72)【発明者】

【氏名】服部 晋悟

【テーマコード（参考）】

2F065

5L096

【Ｆターム（参考）】

2F065AA04

2F065AA37

2F065CC11

2F065CC40

2F065DD06

2F065FF04

2F065MM06

2F065QQ18

2F065QQ27

2F065QQ29

2F065QQ31

2F065QQ41

2F065SS14

5L096AA06

5L096BA04

5L096CA02

5L096DA02

5L096FA13

5L096FA16

5L096FA66

5L096FA67

5L096FA69

5L096FA76

5L096GA32

(57)【要約】

【課題】カメラの単一視点の撮影画像からカメラの姿勢を演算可能な技術を提供する。
【解決手段】 カメラの姿勢を演算する演算装置は、カメラを用いて撮像した撮影画像上の平行な１対の第１直線および第２直線と、第１直線および第２直線に対して既知の角度を有する１本の第３直線と、を特定する直線特定部を備える。演算装置は、第１直線の両端の位置、第２直線の両端の位置、第３直線の両端の位置、カメラの姿勢および位置、を表す複数の変数を生成する変数生成部を備える。演算装置は、第１直線の両端、第２直線の両端、第３直線の両端をカメラのスクリーン上に投影した投影位置と、第１直線の両端、第２直線の両端、第３直線の両端の撮影画像上における位置と、の偏差の二乗和が最小となるように、複数の変数の解を演算する演算部を備える。演算装置は、複数の変数の解からカメラの姿勢を取得する取得部を備える。
【選択図】図３

【特許請求の範囲】

【請求項1】

カメラの姿勢を演算する演算装置であって、
前記カメラを用いて撮像した撮影画像上の平行な１対の第１直線および第２直線と、前記第１直線および前記第２直線に対して既知の角度を有する１本の第３直線と、を特定する直線特定部と、
前記第１直線の両端の位置、前記第２直線の両端の位置、前記第３直線の両端の位置、前記カメラの姿勢および位置、を表す複数の変数を生成する変数生成部と、
前記第１直線の両端、前記第２直線の両端、前記第３直線の両端を前記カメラのスクリーン上に投影した投影位置と、前記第１直線の両端、前記第２直線の両端、前記第３直線の両端の前記撮影画像上における位置と、の偏差の二乗和が最小となるように、前記複数の変数の解を演算する演算部と、
前記複数の変数の解から前記カメラの姿勢を取得する取得部と、
を備える、演算装置。

【請求項2】

前記第１直線、前記第２直線、前記第３直線は、同一の平面内に存在している直線である、請求項１に記載の演算装置。

【請求項3】

前記第１直線および前記第２直線は、第１平面に存在している直線であり
前記第３直線は、前記第１平面と既知の角度で交差する第２平面に存在している直線である、請求項１に記載の演算装置。

【請求項4】

前記第１平面および前記第２平面の一方は地面であり、他方は前記地面上に配置されている物体が有する平面である、請求項３に記載の演算装置。

【請求項5】

前記既知の角度は直角である、請求項１～４の何れか１項に記載の演算装置。

【請求項6】

前記第１直線と前記第２直線との実際の距離、または、前記カメラのレンズ中心の地面からの高さを測定する測定部をさらに備え、
前記取得部は、前記複数の変数の解のうち、前記第１直線の両端、前記第２直線の両端、前記第３直線の両端、前記カメラの位置、のスケールを、前記測定部の測定結果に基づいて取得する、請求項１～５の何れか１項に記載の演算装置。

【請求項7】

前記変数生成部は、前記第１直線および前記第２直線が存在しているＸＹ平面であって、前記第１直線および前記第２直線の方向をＸ軸またはＹ軸の何れか方向とする前記ＸＹ平面を定義することで、前記第１直線の両端を示す３つの変数、前記第２直線の両端を示す３つの変数、前記第３直線の両端を示す３つの変数、をそれぞれ生成する、請求項１～６の何れか１項に記載の演算装置。

【請求項8】

前記変数生成部は、前記カメラの３次元姿勢角を示す３つの変数を生成し、
前記変数生成部は、前記カメラの位置を前記ＸＹ平面の原点に設定することで、前記カメラの位置を示す１つの変数を生成する、請求項７に記載の演算装置。

【請求項9】

カメラの姿勢を演算する演算方法であって、
前記カメラを用いて撮像した撮影画像上の平行な１対の第１直線および第２直線と、前記第１直線および前記第２直線に対して既知の角度を有する１本の第３直線と、を特定するステップと、
前記第１直線の両端の位置、前記第２直線の両端の位置、前記第３直線の両端の位置、前記カメラの姿勢および位置、を表す複数の変数を生成するステップと、
前記第１直線の両端、前記第２直線の両端、前記第３直線の両端を前記カメラのスクリーン上に投影した投影位置と、前記第１直線の両端、前記第２直線の両端、前記第３直線の両端の前記撮影画像上における位置と、の偏差の二乗和が最小となるように、前記複数の変数の解を演算するステップと、
前記複数の変数の解から前記カメラの姿勢を取得するステップと、
を備える、演算方法。

【請求項10】

情報処理装置のコンピュータに読込まれるとともに、カメラの姿勢を演算する情報処理プログラムであって、
前記カメラを用いて撮像した撮影画像上の平行な１対の第１直線および第２直線と、前記第１直線および前記第２直線に対して既知の角度を有する１本の第３直線と、を特定する直線特定手段と、
前記第１直線の両端の位置、前記第２直線の両端の位置、前記第３直線の両端の位置、前記カメラの姿勢および位置、を表す複数の変数を生成する変数生成手段と、
前記第１直線の両端、前記第２直線の両端、前記第３直線の両端を前記カメラのスクリーン上に投影した投影位置と、前記第１直線の両端、前記第２直線の両端、前記第３直線の両端の前記撮影画像上における位置と、の偏差の二乗和が最小となるように、前記複数の変数の解を演算する演算手段と、
前記複数の変数の解から前記カメラの姿勢を取得する取得手段と、
して前記情報処理装置を機能させることを特徴とする情報処理プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本明細書に開示する技術は、カメラの姿勢を演算可能な演算装置および演算方法等に関する。

【背景技術】

【0002】

カメラを使用して各種の制御を行う際には、カメラの３軸の姿勢を求めることが必要となる場合がある。従来、位置が既知の特徴点を３個以上観測することで、カメラの姿勢を得る技術が知られている。また、奥行き方向に十分広がって配置された４個以上の特徴点を観測することで、カメラの焦点距離と位置姿勢を同時に得る技術が知られている。なお、関連する技術が特許文献１および２に開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１９－１７４２４３号公報

【特許文献2】国際公開第２０１６／１８９８７８号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

従来技術では、カメラを設置した現地で、カメラ姿勢推定用の画像を特別に取得するか、画像内の特徴点の位置を別途測量等する必要がある。事前準備の手間がかかるため問題である。

【課題を解決するための手段】

【0005】

本明細書に開示する演算装置の一実施形態は、カメラの姿勢を演算する演算装置である。演算装置は、カメラを用いて撮像した撮影画像上の平行な１対の第１直線および第２直線と、第１直線および第２直線に対して既知の角度を有する１本の第３直線と、を特定する直線特定部を備える。演算装置は、第１直線の両端の位置、第２直線の両端の位置、第３直線の両端の位置、カメラの姿勢および位置、を表す複数の変数を生成する変数生成部を備える。演算装置は、第１直線の両端、第２直線の両端、第３直線の両端をカメラのスクリーン上に投影した投影位置と、第１直線の両端、第２直線の両端、第３直線の両端の撮影画像上における位置と、の偏差の二乗和が最小となるように、複数の変数の解を演算する演算部を備える。演算装置は、複数の変数の解からカメラの姿勢を取得する取得部を備える。

【0006】

上記の構成では、第１直線の両端の位置、第２直線の両端の位置、第３直線の両端の位置、カメラの姿勢および位置、を表す複数の変数を生成する。そして、複数の変数の解を演算することで、カメラの姿勢を取得することができる。カメラの単一視点の撮影画像のみから、カメラの姿勢を簡易に求めることができる。カメラ姿勢推定用の画像を特別に取得したり、画像内の特徴点の位置を別途測量等する必要がないため、事前準備の手間を省略することが可能となる。

【0007】

第１直線、第２直線、第３直線は、同一の平面内に存在している直線であってもよい。効果の詳細は実施例で説明する。

【0008】

第１直線および第２直線は、第１平面に存在している直線であってもよい。第３直線は、第１平面と既知の角度で交差する第２平面に存在している直線であってもよい。効果の詳細は実施例で説明する。

【0009】

第１平面および第２平面の一方は地面であり、他方は地面上に配置されている物体が有する平面であってもよい。効果の詳細は実施例で説明する。

【0010】

既知の角度は直角であってもよい。

【0011】

演算装置は、第１直線と第２直線との実際の距離、または、カメラのレンズ中心の地面からの高さを測定する測定部をさらに備えていてもよい。取得部は、複数の変数の解のうち、第１直線の両端、第２直線の両端、第３直線の両端、カメラの位置、のスケールを、測定部の測定結果に基づいて取得してもよい。効果の詳細は実施例で説明する。

【0012】

変数生成部は、第１直線および第２直線が存在しているＸＹ平面であって、第１直線および第２直線の方向をＸ軸またはＹ軸の何れか方向とするＸＹ平面を定義することで、第１直線の両端を示す３つの変数、第２直線の両端を示す３つの変数、第３直線の両端を示す３つの変数、をそれぞれ生成してもよい。効果の詳細は実施例で説明する。

【0013】

変数生成部は、カメラの３次元姿勢角を示す３つの変数を生成してもよい。変数生成部は、カメラの位置をＸＹ平面の原点に設定することで、カメラの位置を示す１つの変数を生成してもよい。効果の詳細は実施例で説明する。

【0014】

本明細書に開示する演算方法の一実施形態は、カメラの姿勢を演算する演算方法である。演算方法は、カメラを用いて撮像した撮影画像上の平行な１対の第１直線および第２直線と、第１直線および第２直線に対して既知の角度を有する１本の第３直線と、を特定するステップを備える。演算方法は、第１直線の両端の位置、第２直線の両端の位置、第３直線の両端の位置、カメラの姿勢および位置、を表す複数の変数を生成するステップを備える。演算方法は、第１直線の両端、第２直線の両端、第３直線の両端をカメラのスクリーン上に投影した投影位置と、第１直線の両端、第２直線の両端、第３直線の両端の撮影画像上における位置と、の偏差の二乗和が最小となるように、複数の変数の解を演算するステップを備える。演算方法は、複数の変数の解からカメラの姿勢を取得するステップを備える。効果の詳細は実施例で説明する。

【0015】

本明細書に開示する情報処理プログラムの一実施形態は、情報処理装置のコンピュータに読込まれるとともに、カメラの姿勢を演算する情報処理プログラムである。情報処理プログラムは、カメラを用いて撮像した撮影画像上の平行な１対の第１直線および第２直線と、第１直線および第２直線に対して既知の角度を有する１本の第３直線と、を特定する直線特定手段として情報処理装置を機能させる。情報処理プログラムは、第１直線の両端の位置、第２直線の両端の位置、第３直線の両端の位置、カメラの姿勢および位置、を表す複数の変数を生成する変数生成手段として情報処理装置を機能させる。情報処理プログラムは、第１直線の両端、第２直線の両端、第３直線の両端をカメラのスクリーン上に投影した投影位置と、第１直線の両端、第２直線の両端、第３直線の両端の撮影画像上における位置と、の偏差の二乗和が最小となるように、複数の変数の解を演算する演算手段として情報処理装置を機能させる。情報処理プログラムは、複数の変数の解からカメラの姿勢を取得する取得手段として情報処理装置を機能させる。効果の詳細は実施例で説明する。

【図面の簡単な説明】

【0016】

【図1】フォークリフト１の概略斜視図である。

【図2】フォークリフト１のブロック図である。

【図3】演算処理の具体的内容について説明するフロー図である。

【図4】実施例１におけるカメラ３０の撮影画像の一例である。

【図5】実施例１における演算結果を示す図である。

【図6】実施例２におけるカメラ３０の撮影画像の一例である。

【図7】実施例２における演算結果を示す図である。

【図8】トラックの荷台の姿勢・位置を演算する場合の撮影画像例である。

【図9】カメラキャリブレーションを行う場合の撮影画像例である。

【発明を実施するための形態】

【実施例0017】

（フォークリフト１の構成）
以下、図１および図２を参照して、本実施例のフォークリフト１について説明する。図１はフォークリフト１の概略斜視図であり、図２はブロック図である。フォークリフト１は、無人フォークリフトである。図１に示すように、フォークリフト１は、車体２、制御部１０（図２に図示）、マスト２０、フォーク２２、バックレスト２３、カメラ固定部２４、カメラ３０、を備えている。

【0018】

車体２は、その両側面のそれぞれに前輪２８及び後輪２９を備えている。前輪２８は、駆動機構を介して駆動輪モータ（不図示）が接続されており、移動制御部１７（図２に図示）によって回転駆動されるようになっている。後輪２９は、操舵装置（不図示）に接続されており、移動制御部１７によって車輪の向きが調整される。

【0019】

マスト２０は、車体２の前面に取付けられている支柱であり、その軸線は上下方向に伸びている。フォーク２２は、マスト２０に上下方向に移動可能に取付けられている。フォーク２２は、一対のツメ２２ａ、２２ｂを有している。カメラ固定部２４は、カメラ３０を固定する部材である。

【0020】

図２のブロック図を説明する。フォークリフト１は、制御部１０、フォーク２２、前輪２８、後輪２９、カメラ３０、エンコーダ４０、を備える。制御部１０は、フォークリフト１を制御する装置である。制御部１０は、例えば、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータによって構成することができる。コンピュータがプログラムを実行することで、制御部１０は、図２に示す測定部１１、直線特定部１２、変数生成部１３、演算部１４、収束判断部１５、取得部１６、等として機能する。すなわち制御部１０は、カメラ３０の位置および姿勢を演算する演算装置として機能する。

【0021】

エンコーダ４０は、車輪の回転角を検出するセンサを備えた部位である。車体位置姿勢算出部１９は、エンコーダ４０によって検出された前輪２８の回転角に基づいて、車体２の移動方向及び移動量を求める。フォーク２２は、荷役装置制御部１８により昇降される。フォーク２２の上下方向の位置は、荷役装置制御部１８の駆動量によって特定可能となっている。

【0022】

（カメラ３０の姿勢・位置の演算処理）
カメラ３０を使用して、フォークリフト１の各種の制御（例：車体２の移動、フォーク２２の操作）を行う場合には、カメラ３０の姿勢および位置を求めることが必要となる。しかし画像に写っている物に関する事前知識無しで、カメラ３０で取得した単一画像のみからカメラの位置や姿勢を求めることは、非常に困難である。本明細書の技術は、マンハッタンワールド仮説（人工的な屋外（都市）・屋内（倉庫・工場等）構造は、平行・直角の強い特徴を持つという仮説）に基づき、単一画像から、カメラ３０の姿勢および位置を推定することを可能とするものである。以下に説明する。

【0023】

なお本明細書では、カメラ３０はピンホールモデルで近似可能であるとし、位置姿勢推定は幾何学的な問題として扱う。また床面等含め、水平面を全て「地面」と記述する場合がある。

【0024】

図３のフローを用いて、カメラ３０の姿勢・位置の演算処理の具体的内容について説明する。ステップＳ５において、測定部１１は、後述するスケールを決定するための実測値を取得する。実測値を得るための計測対象は、様々であって良い。例えば、後述する第１直線ＳＬと第２直線ＳＬ２との実際の距離を、ポケットレーザー距離計などで測定してもよい。また例えば、カメラ３０のレンズ中心の、地面からの高さを測定してもよい。測定は、測定部１１によって自動で行われてもよいし、ユーザが測定した結果を制御部１０に入力してもよい。

【0025】

なお、実測値を制御部１０のメモリに保存しておき、読み出して使用する場合には、ステップＳ５を省略することが可能である。

【0026】

ステップＳ１０において、カメラ３０で撮影画像が取得される。図４に、カメラ３０の撮影画像の一例を示す。

【0027】

ステップＳ２０において直線特定部１２は、カメラ３０で取得された撮影画像上で、第１直線ＳＬ１、第２直線ＳＬ２、第３直線ＳＬ３を特定する。第１直線ＳＬ１～第３直線ＳＬ３は、リアルワールドにおいて、同一平面（地面）内に存在している直線である。第１直線ＳＬ１および第２直線ＳＬ２は、平行な１対の直線である。第３直線ＳＬ３は、第１直線ＳＬ１および第２直線ＳＬ２に対して既知の角度を有する直線である。第１直線ＳＬ１～第３直線ＳＬ３の選択は、直線特定部１２によって自動で行われてもよいし、ユーザによって実行されてもよい。またＲＡＮＳＡＣ（RANdom SAmple Consensus）など、様々な手法を用いることができる。

【0028】

図４に、第１直線ＳＬ１～第３直線ＳＬ３の特定例を示す。また図５に、後述するステップＳ５０で演算された演算結果を示す。図５は、図４を垂直方向に投影した図（すなわち、地面を上空から垂直に見下ろした図）に対応する。図４および図５の例では、第３直線ＳＬ３は、第１直線ＳＬ１および第２直線ＳＬ２に対して直角な直線である。

【0029】

ステップＳ３０において、変数生成部１３は、第１直線ＳＬ１の両端の位置、第２直線ＳＬ２の両端の位置、第３直線ＳＬ３の両端の位置、カメラ３０の姿勢および位置、を表す１３個の変数を生成する。図４および図５を用いて、以下に説明する。

【0030】

変数生成部１３は、第１直線ＳＬ１～第３直線ＳＬ３が存在している地面を、ＸＹ平面と定義する。また、地面に対して垂直上方方向をＺ軸方向と定義する。すなわち、ｚ＝０の面を地面とする。また、第１直線ＳＬ１および第２直線ＳＬ２の方向をＹ軸方向とするとともに、第３直線ＳＬ３の方向をＸ軸とするように、座標系を決定する。これにより、原点ＯＣが決定される。原点ＯＣは、システム全体の基準点である。原点ＯＣは、あとで座標変換すれば良いため、任意の位置であってよい。

【0031】

図５に示すように、第１直線ＳＬ１の端点Ｐ１１の座標値は、（ｐ_１ｘ，ｐ_１ｙ，０）となる。第１直線ＳＬ１の端点Ｐ１２の座標値は、（ｐ_１ｘ，ｐ_２ｙ，０）となる。端点Ｐ１１とＰ１２とは、ｘ座標は共通である。またｚ座標は０である。よって端点Ｐ１１およびＰ１２は、３つの変数ｐ_１ｘ，ｐ_１ｙ，ｐ_２ｙを用いて表すことができる。

【0032】

同様に、第２直線ＳＬ２の端点Ｐ２１の座標値は、（ｑ_１ｘ，ｑ_１ｙ，０）となる。第２直線ＳＬ２の端点Ｐ２２の座標値は、（ｑ_１ｘ，ｑ_２ｙ，０）となる。よって端点Ｐ２１およびＰ２２は、３つの変数ｑ_１ｘ，ｑ_１ｙ，ｑ_２ｙを用いて表すことができる。

【0033】

第３直線ＳＬ３の端点Ｐ３１の座標値は、（ａ_１ｘ，ａ_１ｙ，０）となる。第３直線ＳＬ３の端点Ｐ３２の座標値は、（ａ_２ｘ，ａ_１ｙ，０）となる。端点Ｐ３１とＰ３２とは、ｙ座標は共通である。またｚ座標は０である。よって端点Ｐ３１およびＰ３２は、３つの変数ａ_１ｘ，ａ_１ｙ，ａ_２ｘを用いて表すことができる。

【0034】

以上により、第１直線ＳＬ１、第２直線ＳＬ２、第３直線ＳＬ３の両端６点の座標を表す変数を、１２個のＸＹ座標値ではなく、９個とすることができる。これは上述したように、平行の条件や直角の条件が存在し、これらの条件が変数の決定方法に組み込まれているためである。換言すると、マンハッタンワールド仮説が、最適化の目的関数に組み込まれているためである。

【0035】

なお第３直線ＳＬ３は、第１直線ＳＬ１および第２直線ＳＬ２と直交せずに、傾きθ（既知の値）を有していてもよい。この場合、端点Ｐ３２の座標値を、（ａ_２ｘ，（ａ_２ｘ－ａ_１ｙ）ｔａｎθ，０）とすればよい。この場合においても、両端６点の座標を表す変数を９個とすることができる。

【0036】

また変数生成部１３は、カメラ３０の３次元姿勢角を示す３つの変数ｒｘ，ｒｙ，ｒｚを生成する。（ｒｘ，ｒｙ，ｒｚ）は、長さ１に単位化した回転軸ベクトルに、その軸まわりの回転角Φをかけた変数である。

【0037】

また変数生成部１３は、カメラ３０の位置を示す変数を生成する。具体的には、カメラ３０の位置のＸＹ座標値を０とする。すなわち図５の原点ＯＣの鉛直上方にカメラ３０が位置しているとする。これにより、カメラ３０の位置を、Ｚ方向の高さを示す１つの変数ｈで表すことができる。不要な変数を増やさないようにすることが可能となる。

【0038】

以上のように定式化すると、３本の直線の両端で９変数、カメラ３０の姿勢で３変数、カメラ３０の高さ（位置のＺ座標値）で１変数となる。よって、合計１３変数の最適化問題とすることができる。変数を減らす効果を説明する。第１直線ＳＬ１～第３直線ＳＬ３の位置およびカメラ３０の姿勢・位置は、全体の座標軸の方向と原点位置および前述のスケールに関して、任意性が存在する。そこで、上述のように変数を減少させることで、最小化問題としての任意性を極力減らすことができる。そして任意性を、スケールのみにすることができる。

【0039】

ステップＳ４０において、演算部１４は、第１直線ＳＬ１、第２直線ＳＬ２、第３直線ＳＬ３を、カメラパラメータで、カメラの画像に変換する。すなわち、第１直線ＳＬ１の端点Ｐ１１およびＰ１２、第２直線ＳＬ２の端点Ｐ２１およびＰ２２、第３直線ＳＬ３の端点Ｐ３１およびＰ３２をカメラのスクリーン上に投影した、投影位置を求める。以下に具体的に説明する。３次元での点の位置を（ｘ，ｙ，ｚ）^Ｔとする。Ｔは行列の転置を表す。スクリーン上の２次元のＸ，Ｙの位置を（ｕ，ｖ）とする。下式により、３次元の点の位置（ｘ，ｙ，ｚ）^Ｔを、２次元のＸ，Ｙの位置（ｕ，ｖ）に変換することができる。

【数1】

【数2】

【数3】

ここで、Ｒは回転変換（行列）である。行列で書くと９個要素があるが、自由度は３である。具体的には、前述した（ｒｘ，ｒｙ，ｒｚ）を使用すればよい。（０，０，ｈ）^Ｔは、カメラ３０の位置である。ｆはカメラ３０の焦点距離である。ｃ_ｘ、ｃ_ｙはスクリーンの中心点の２次元ＸＹ値であり、定数である。

【0040】

上記式に基づいて、第１直線ＳＬ１の両端、第２直線ＳＬ２の両端、第３直線ＳＬ３の両端の、２次元のＸ，Ｙの位置（ｕ，ｖ）を計算する。これにより、端点Ｐ１１（ｐ_１ｕ，ｐ_１ｖ）、端点Ｐ１２（ｐ_２ｕ，ｐ_２ｖ）、端点Ｐ２１（ｑ_１ｕ，ｑ_１ｖ）、端点Ｐ２２（ｑ_２ｕ，ｑ_２ｖ）、端点Ｐ３１（ａ_１ｕ，ａ_１ｖ）、端点Ｐ３２（ｑ_２ｕ，ｑ_２ｖ）、が算出される。

【0041】

ステップＳ５０において、演算部１４は、撮影画像上における第１直線ＳＬ１の両端、第２直線ＳＬ２の両端、第３直線ＳＬ３の両端の各々の値と、上記で算出された２次元のＸ，Ｙの位置（ｕ，ｖ）と、の偏差の二乗和が最小（正確には極小）となるように、１３個の変数の解を演算する。すなわち、ｐ_１ｘ，ｐ_１ｙ，ｐ_２ｙ，ｑ_１ｘ，ｑ_１ｙ，ｑ_２ｙ，ａ_１ｘ，ａ_１ｙ，ａ_２ｘ（第１直線ＳＬ、第２直線ＳＬ２、第３直線ＳＬ３の両端を表す変数）、および、ｒｘ，ｒｙ，ｒｚ（カメラの３次元姿勢角を表す変数）、および、h（カメラの高さを表す変数）、の変数の組の解が（同時に）計算される。そして収束に正孔すると、１組の解が得られる。これは唯一の解ではなく特解であるが、任意性はスケールにしか残っていない。

【0042】

この最適化問題は、目的関数が単なる座標変換とピンホールカメラの中心射影の変換の組み合わせから作られているため、代数式で機械的に微分可能である。レーベンバーグマーカート法（準ニュートン法）、あるいはニュートン法で解くことができ、産業車両などのオンボードに組み込める程度の計算速度も得ることができる。

【0043】

ステップＳ６０において、収束判断部１５は、演算が収束したか否かの判定を行う。任意性を残したまま最適化を行うため、条件によっては収束しづらいこともあるためである。収束判定は、一般的なニュートン法の収束判定で行うことができる。

【0044】

また裏表の任意性も存在するため、反対側の解（例：地面の下のカメラ位置）に収束していないかも確認する必要がある。なお、おおよその初期値を与えることで、反対側の解に収束することを抑制することが可能である。初期値は、車体位置姿勢算出部１９による自己位置推定（オドメトリ等）によって決定してもよい。収束したと判断された場合（Ｓ６０：ＹＥＳ）には、ステップＳ７０へ進む。

【0045】

ステップＳ７０において、取得部１６は、全体のスケールを決定する。前述したように、ステップＳ５０の演算では、全体のスケールだけが決定不能な量である。そのため、ステップＳ５で測定部１１が測定した測定結果に基づいて、スケールを求める。そして、姿勢パラメータ以外の全体を、求めたスケールに基づいて定数倍することで、一般解（実世界の解）を得ることができる。その結果、カメラ３０の姿勢および位置を取得することが可能となる。

【0046】

（効果）
本実施例の技術では、カメラ３０の単一視点の撮影画像のみから、カメラ３０の姿勢を簡易に求めることができる。カメラ姿勢の推定用の画像を特別に取得したり、画像内の特徴点の位置を別途測量等する必要がないため、事前準備の手間を省略することが可能となる。

【0047】

本実施例の技術では、（同一平面上の）１組の平行線と、平行線との角度が既知の１本の直線が写った画像があれば、カメラ３０の姿勢および位置を求めることができる。そしてこれらの直線の組み合わせは、道路と横断歩道や、直方体の建造物の外枠の一部など、リアルワールドの多くの場所に存在している。従って本実施例の技術は、適用範囲が広く、実用に適した技術である。

【実施例0048】

実施例２は、第１直線ＳＬ１、第２直線ＳＬ２、第３直線ＳＬ３の特定の方法が実施例１とは異なっている態様である。実施例１と異なる部分のみ説明する。

【0049】

ステップＳ２０において直線特定部１２は、第１平面に存在している、平行な１対の第１直線ＳＬ１および第２直線ＳＬ２を特定する。また、第１平面と既知の角度で交差する第２平面を特定し、その第２平面に存在している第３直線ＳＬ３を特定する。

【0050】

具体例を用いて説明する。図６に、実施例２で用いるカメラ３０の撮影画像の一例を示す。また図７に、ステップＳ５０で演算された演算結果を示す。図７は、図６を垂直方向に投影した図に対応する。第１直線ＳＬ１および第２直線ＳＬ２は、第１平面（地面）に存在している、平行な１対の直線である。近隣の建造物の壁面ＷＦは、第１平面（地面）と既知の角度（直角）で交差する第２平面に対応する。第３直線ＳＬ３は、第２平面（壁面ＷＦ）に存在している。具体的には、第３直線ＳＬ３は、倉庫の壁板と倉庫の壁の下のコンクリート部分との間に位置する、水平線である。

【0051】

ステップＳ３０において、変数生成部１３は、第１直線ＳＬ１および第２直線ＳＬ２が存在している地面を、ＸＹ平面と定義する。また、第１直線ＳＬ１および第２直線ＳＬ２の方向をＸ軸方向とする。そして、右手系でＹ軸方向を決定する。

【0052】

図７に示すように、第１直線ＳＬ１の端点Ｐ１１の座標値は、（ａ_１ｘ，ａ_１ｙ，０）となる。第１直線ＳＬ１の端点Ｐ１２の座標値は、（ａ_２ｘ，ａ_１ｙ，０）となる。同様に、第２直線ＳＬ２の端点Ｐ２１の座標値は、（ｂ_１ｘ，ｂ_１ｙ，０）となる。第２直線ＳＬ２の端点Ｐ２２の座標値は、（ｂ_２ｘ，ｂ_１ｙ，０）となる。第３直線ＳＬ３の端点Ｐ３１の座標値は、（ｋ_１ｘ，ｋ_１ｙ，ｋ_１ｚ）となる。第３直線ＳＬ３の端点Ｐ３２の座標値は、（ｋ_２ｘ，ｋ_１ｙ，ｋ_１ｚ）となる。実施例１と同様にして、平行の条件や直角の条件を用いることで、変数の数を減少させることができる。

【0053】

ステップＳ４０およびＳ５０における演算部１４による演算内容は、実施例１と同様であるため、詳細な説明を省略する。しかし、実施例２のように２つの平面を用いる場合には、実施例１のように１つの平面を用いる場合に比べて、局所最適解が持つ任意性の次元が１次元高いことになる。従って実施例２では、第１平面（地面）と第２平面（壁面ＷＦ）との間の位置関係（両面が交わる直線がどこにあるか）は未知のまま解が求められる。そのため、地面のスケールと壁面ＷＦのスケールとは、別々に決まる。換言すると、地面の高さと壁面ＷＦのＹ座標値とに、独立に任意性が残る。よって計算上は、第３直線ＳＬ３のＹ座標値（ｋ_１ｙ）を任意に定めることができる。しかし、地面を用いてカメラの姿勢・位置を求める場合には、壁面ＷＦのＹ座標値ｋ_１ｙ（すなわちスケールと等価）は使用しないため、何れの値となってもよく、特に正確な値に定める必要はない。

【0054】

リアルワールド（図６参照）では、第３直線ＳＬ３のＹ軸方向位置は、第２直線ＳＬ２よりも負側に位置している。しかしステップＳ５０の演算結果（図７参照）では、第３直線ＳＬ３のＹ軸方向位置は、第２直線ＳＬ２よりも正側に位置している。すなわち、第３直線ＳＬ３のＹ軸方向位置は、リアルワールドと演算結果とで全く異なっている。これは壁面ＷＦの大きさ（スケール）を無視して、演算を収束させているためである。しかし上述したように、壁面ＷＦ（第３直線ＳＬ３）が何れの場所に位置しても、地面座標系におけるカメラの姿勢・位置を正確に求めることが可能であるため、問題はない。

【0055】

地面のスケールを決定するためには、地面上の２点間の距離、または、カメラ３０の地面からの高さの何れかの情報が必要である。地面上の２点は、カメラ３０の撮影画像（図６）に写っていて、地面上にあることが常識的に分かっている点であれば、何れの点を用いてもよい。また点間の距離は、画面上の位置（ｕ，ｖ）の値が正確に特定出来る値であればよい。

【0056】

（効果）
地面上で１組の平行線しか特定できないような場合においても、地面とのなす角度が既知の壁面に含まれる直線を使用することで、カメラ３０の姿勢・位置を演算することができる。本明細書の技術の応用範囲を拡張することが可能となる。

【0057】

地面よりも建造物の壁面の方が、平面の精度が高い。これは、排水性などを考慮して、地面には様々な傾きが形成されているためである。本実施例の技術では、精度の高い壁面を用いてカメラ３０の姿勢・位置を演算することができるため、演算結果の精度を高めることが可能となる。

【0058】

建造物の壁面は、一般的に、地面に対して垂直である。よって、壁面に含まれる直線を用いることで、マンハッタンワールド仮説に従う直線を簡易に特定することが可能となる。

【0059】

以上、本明細書が開示する技術の実施例について詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。

【0060】

（変形例）
本明細書の技術の適用範囲は、移動体や建造物に設置したカメラの姿勢・位置を推定する内容に限られない。例えば、カメラに写った対象物（例：トラックの荷台、棚）の姿勢・位置を求めることにも適用可能である。図８の撮影画像例を用いて、トラックの荷台の姿勢・位置を演算する場合を説明する。ステップＳ２０において直線特定部１２は、同一平面（サイドパネル）内に存在している平行な１対の直線（第１直線ＳＬ１、第２直線ＳＬ２）、および、１対の直線に直交している直線（第３直線ＳＬ３）を特定する。サイドパネルの車両前後方向の長さや、車高方向の高さは既知であるため、本明細書の技術を用いて、荷台に対するカメラの姿勢・位置を演算することができる。その結果、トラックの荷台の姿勢・位置を求めることが可能となる。

【0061】

本明細書の技術は、カメラに写った対象物（例：フォークリフトのフォーク）とカメラとの位置姿勢関係を求める、カメラキャリブレーションにも適用可能である。図９の撮影画像例を用いて説明する。図９は、フォークリフト１のバックレスト２３（図１参照）にカメラ３０が搭載されている場合における、撮影画像例である。ステップＳ２０において直線特定部１２は、同一平面内に存在している平行な１対の直線（第１直線ＳＬ１、第２直線ＳＬ２）、および、１対の直線に直交している直線（第３直線ＳＬ３）を特定する。ツメ２２ａおよび２２ｂの長さや幅は既知であるため、本明細書の技術を用いて、フォーク２２に対するカメラ３０の姿勢・位置を演算することができる。フォークを付け替えた場合も、容易にフォーク先端の姿勢・位置を求めることが可能となる。

【0062】

本実施例では、ステップＳ２０において、直線特定部１２が３本の直線を特定する場合を説明したが、この形態に限られない。４本以上の直線を特定することが可能である。正確な平面上にあることが分かっている直線の数を増加させるほど、演算結果の精度を高めることが可能となる。なお、マンハッタンワールド仮説に従う場合、直線を１本増加させるごとに、変数が３個増加する。しかし、変数の数が数十個の範囲であれば、演算処理の負担を抑制できるため、産業車両などの制御サイクルに組み込むことが可能である。また変数の数が数百個の範囲であれば、安定して解くことが可能である。

【0063】

本実施例では、平行な１対の直線が地面で特定される場合を説明したが、この形態に限られない。例えば図６の撮影画像において、壁面ＷＦに存在している直線ＳＬ４およびＳＬ５を、平行な１対の直線として特定してもよい。現実的には、地面に比べて壁面ＷＦの方が平面の精度が高い。そのため、壁面ＷＦを用いてカメラ３０の姿勢・位置を演算することで、演算結果の精度を高めることが可能となる。

【0064】

回転変換（行列）Ｒは、様々な態様であってよい。例えば自動車でよく使用される、Ｚ軸まわりにθｚ回転させ、その後のＹ軸まわりにθｙ回転させ、その後のＸ軸まわりにθｘ回転させるという、回転行列を３個掛けたものを使用することも可能である。この場合、回転変換のパラメータは、θｘ，θｙ，θｚとなる。また回転の順番は、決めごととしておく必要がある。

【0065】

本実施形態では、カメラ３０を備える移動体としてフォークリフト１を説明したが、この形態に限られない。他の様々な移動体（例：移動ロボット、モバイルマニュピレータ、ドローン）に対しても、本明細書の技術を適用可能である。また移動体に限らず、固定物に搭載されているカメラ３０に対しても、本明細書の技術を適用可能である。

【0066】

制御部１０はフォークリフト１に搭載される形態に限られない。例えば、インターネットなどのネットワーク上に配置されたサーバであってもよい。

【0067】

本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。