JP7096274B2

JP7096274B2 - 自己位置推定と環境マップ作成を同時に行う方法及び装置

Info

Publication number: JP7096274B2
Application number: JP2019572827A
Authority: JP
Inventors: 亜慧王; 少駿蔡
Original assignee: Uisee Technologies Beijing Co Ltd
Current assignee: Uisee Technologies Beijing Co Ltd
Priority date: 2018-06-07
Filing date: 2018-12-28
Publication date: 2022-07-05
Anticipated expiration: 2038-12-28
Also published as: EP3806036A1; EP3806036A4; JP2021505979A; WO2019233090A1; US20210082137A1; KR20200014858A; US11017545B2; KR102367361B1

Description

本発明は自己位置推定と環境マップ作成を同時に行う分野に関し、特に広視野カメラに基づいて自己位置推定と環境マップ作成を同時に行う分野に関する。

自己位置推定と環境マップ作成を同時に行うこと（ＳｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｉｚａｔｉｏｎＡｎｄＭａｐｐｉｎｇ、ＳＬＡＭと略称する）はロボットの動きをリアルタイムに追跡しながら周囲環境マップを作成して位置推定やナビゲーション等の目標を実現する技術である。

従来のＳＬＡＭに使用されているカメラは透視カメラ（ｐｅｒｓｐｅｃｔｉｖｅｃａｍｅｒａ）であり、又はピンホールカメラ（ｐｉｎｈｏｌｅｃａｍｅｒａ）と呼ばれる。カメラの画角（Ｆｉｅｌｄ－ｏｆ－Ｖｉｅｗ）が限られるため、取得された画像に存在する共通特徴は不十分であり、ＳＬＡＭアルゴリズムによる追跡が無効になる恐れがある。従来のＳＬＡＭに使用されているピンホールカメラに比べて、広視野カメラは画角がより大きいため、広く研究され、注目されている。
従来の広視野画像に基づくＳＬＡＭの技術案は主として２種類がある。

一方は、まず、広視野カメラによって取得された広視野画像に対して従来の歪み補正方法を用いて歪み補正処理を行い、さらに歪み補正後の画像を普通画像として従来のＳＬＡＭ技術を用いて自己位置推定と環境マップ作成を同時に行うことである。このような技術案は簡単であるが、従来の歪み補正方法は多くの画角の損失を招き、広視野カメラの広角を十分に活用できない。

他方は、広視野カメライメージングモデルに基づいて、歪み補正されていない広視野画像に直接ＳＬＡＭ処理を行うことである。すなわち、歪み補正されていない広視野画像から直接特徴を抽出して処理する。このような技術案は、抽出された特徴が画像歪みの影響を受けることが可能性があり、且つ複雑な広視野カメライメージングモデルによって最適化が非常に複雑になるため、システムの性能を損なってしまう。

従って、画像歪みの影響を回避しながら、広視野カメラのすべての視野を保持するとともに、被写界深度の検出、自己位置推定、環境マップ作成を同時に行うことができる新たなＳＬＡＭ技術が望まれている。

このアプリケーションの目的は、位置決めとマッピングを同時に行う方法を提供することです。この方法では、マルチバーチャルピンホールカメラモデルに基づいて、広視野カメラで取得した広視野画像を削除し、歪みのない画像に基づいて同時の位置決めとマッピングを実行できます。

このアプリケーションは、一方で提供します自己位置推定と環境マップ作成を同時に行う方法であって、広視野カメラによって広視野画像を取得するステップと、マルチ仮想ピンホールカメラモデルに基づいて、前記広視野画像に対応する歪み補正画像を取得するステップと、前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成するステップと、を含み、前記マルチ仮想ピンホールカメラモデルは少なくとも２つの異なる向きの仮想ピンホールカメラを含み、且つ前記少なくとも２つの異なる向きの仮想ピンホールカメラのカメラ中心は前記広視野カメラのカメラ中心と重なることを特徴とする自己位置推定と環境マップ作成を同時に行う方法。

いくつかの実施形態では、前記広視野カメラは単眼広視野カメラであり、前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成する前記ことは、初期化ステップを含み、前記初期化ステップは、第１時点に対応する歪み補正画像及び第２時点に対応する歪み補正画像を取得するステップと、前記第１時点に対応する歪み補正画像と前記第２時点に対応する歪み補正画像の互いにマッチングする特徴点を決定するステップと、前記互いにマッチングする特徴点に基づいて初期マップを作成するステップと。

いくつかの実施形態では、前記互いにマッチングする特徴点に基づいて初期マップを作成する前記ことは、前記第１時点に対応する歪み補正画像中の特徴点及び前記第１時点における前記広視野カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定するステップと、前記第２時点に対応する歪み補正画像中のマッチングする特徴点及び前記第２時点における前記広視野カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定するステップと、前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定するステップと、前記マップ点に基づいて初期マップを作成するステップと。

いくつかの実施形態では、前記広視野カメラは単眼広視野カメラであり、前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成する前記ことは、グローバルバンドル最適化ステップを含み、前記グローバルバンドル最適化ステップは、前記マップ中の各キー広視野フレームに対して、前記キー広視野フレームに関連付けられた各マップ点をマルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー広視野フレームに関連付けられたマップ点の再投影誤差に基づいて再投影誤差を決定するステップと、前記再投影誤差に基づいて、前記キー広視野フレームの位置姿勢及び前記キー広視野フレームに関連付けられたすべてのマップ点の位置を更新するステップと。

いくつかの実施形態では、前記広視野カメラは単眼広視野カメラであり、前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成する前記ことは、追跡ステップを含み、前記追跡ステップは、現在の広視野フレームに関連付けられた各マップ点に対して、前記マップ点をマルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記現在の広視野フレームに関連付けられたマップ点の再投影誤差に基づいて再投影誤差を決定するステップと、前記再投影誤差に基づいて、前記現在の広視野フレームの位置姿勢を更新するステップと。

いくつかの実施形態では、前記広視野カメラは単眼広視野カメラであり、前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成する前記ことは、マップ作成ステップを含み、前記マップ作成ステップは、現在の広視野フレーム及びその基準フレームの互いにマッチングする特徴点を決定するステップと、前記現在の広視野フレームの特徴点及び現在の前記広視野カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定するステップと、前記基準フレームのマッチングする特徴点及び前記基準フレームに対応する前記広視野カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定するステップと、前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定するステップと、前記マップ点に基づいてマップを作成するステップと。

いくつかの実施形態では、前記マップ作成ステップはローカルバンドル最適化ステップをさらに含み、前記ローカルバンドル最適化ステップは、ローカルマップ中の各キー広視野フレームに対して、前記キー広視野フレームに関連付けられた各マップ点をマルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー広視野フレームに関連付けられたマップ点の再投影誤差に基づいて再投影誤差を決定するステップと、前記再投影誤差に基づいて、前記キー広視野フレームの位置姿勢及び該キー広視野フレームに関連付けられたすべてのマップ点の位置を更新するステップと。

いくつかの実施形態では、前記広視野カメラは双眼広視野カメラであり、前記双眼広視野カメラによって左視野画像及び右視野画像を取得するステップと、第１マルチ仮想ピンホールカメラモデルに基づいて、前記左視野画像に対応する左歪み補正画像を取得するステップと、第２マルチ仮想ピンホールカメラモデルに基づいて、前記右視野画像に対応する右歪み補正画像を取得するステップと、前記左歪み補正画像及び前記右歪み補正画像に基づいて、前記双眼広視野カメラの位置姿勢を決定し、マップを作成するステップと、を含み、前記第１マルチ仮想ピンホールカメラモデルは少なくとも２つの異なる向きの仮想ピンホールカメラを含み、且つ前記少なくとも２つの異なる向きの仮想ピンホールカメラのカメラ中心は前記双眼広視野カメラの左側カメラのカメラ中心と重なり、前記第２マルチ仮想ピンホールカメラモデルは少なくとも２つの異なる向きの仮想ピンホールカメラを含み、且つ前記少なくとも２つの異なる向きの仮想ピンホールカメラのカメラ中心は前記双眼広視野カメラの右側カメラのカメラ中心。

いくつかの実施形態では、前記左歪み補正画像及び前記右歪み補正画像に基づいて、前記双眼広視野カメラの位置姿勢を決定し、マップを作成する前記ことは、初期化ステップを含み、前記初期化ステップは、前記左歪み補正画像と前記右歪み補正画像の互いにマッチングする特徴点を決定するステップと、前記互いにマッチングする特徴点に基づいて初期マップを作成するステップと。

いくつかの実施形態では、前記左歪み補正画像と前記右歪み補正画像の互いにマッチングする特徴点を決定する前記ことは、前記右歪み補正画像における前記左歪み補正画像中の特徴点に対応する極線を決定するステップと、前記極線上で前記左歪み補正画像中の特徴点にマッチングする特徴点を検索するステップと、を含み。

いくつかの実施形態では、前記互いにマッチングする特徴点に基づいて初期マップを作成する前記ことは、前記左歪み補正画像中の特徴点及び前記双眼広視野カメラの左側カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定するステップと、前記右歪み補正画像中のマッチングする特徴点及び前記双眼広視野カメラの右側カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定するステップと、前記双眼広視野カメラの基線に基づいて、前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定するステップと、前記マップ点に基づいて初期マップを作成するステップと。

いくつかの実施形態では、前記左歪み補正画像及び前記右歪み補正画像に基づいて、前記双眼広視野カメラの位置姿勢を決定し、マップを作成する前記ことは、グローバルバンドル最適化ステップを含み、前記グローバルバンドル最適化ステップは、前記マップ中の各キー双眼画像フレームに対して、前記キー双眼画像フレームに関連付けられたマップ点を第１マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて左再投影誤差を決定するステップ、又は前記キー双眼画像フレームに関連付けられたマップ点を第２マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて右再投影誤差を決定するステップと、前記左再投影誤差、前記右再投影誤差又は前記左再投影誤差と前記右再投影誤差との和に基づいて、前記キー双眼画像フレームの位置姿勢及び前記キー双眼画像フレームに関連付けられたすべてのマップ点の位置を更新するステップと。

いくつかの実施形態では、前記左歪み補正画像及び前記右歪み補正画像に基づいて、前記双眼広視野カメラの位置姿勢を決定し、マップを作成する前記ことは、追跡ステップを含み、前記追跡ステップは、現在の双眼画像フレームに関連付けられた各マップ点に対して、前記マップ点を第１マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点を取得し前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記現在の双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて左再投影誤差を決定するステップ、又は前記マップ点を第２マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記現在の双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて右再投影誤差を決定するステップと、前記左再投影誤差、前記右再投影誤差又は前記左再投影誤差と前記右再投影誤差との和に基づいて、前記現在の双眼画像フレームの位置姿勢を更新するステップと。

いくつかの実施形態では、前記左歪み補正画像及び前記右歪み補正画像に基づいて、前記双眼広視野カメラの位置姿勢を決定し、マップを作成する前記ことは、マップ作成ステップを含み、前記マップ作成ステップは、現在の左歪み補正画像と現在の右歪み補正画像の互いにマッチングする特徴点を決定するステップと、前記現在の左歪み補正画像の特徴点及び現在の前記双眼広視野カメラの左側カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定するステップと、前記現在の右歪み補正画像の特徴点及び現在の前記双眼広視野カメラの右側カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定するステップと、前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定するステップと、前記マップ点に基づいてマップを作成するステップと。

いくつかの実施形態では、前記マップ作成ステップはローカルバンドル最適化ステップをさらに含み、前記ローカルバンドル最適化ステップは、ローカルマップ中の各キー双眼画像フレームに対して、前記キー双眼画像フレームに関連付けられたマップ点を第１マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて左再投影誤差を決定するステップ、又は前記キー双眼画像フレームに関連付けられたマップ点を第２マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて右再投影誤差を決定するステップと、前記左再投影誤差、前記右再投影誤差又は前記左再投影誤差と前記右再投影誤差との和に基づいて、前記キー双眼画像フレームの位置姿勢及び前記キー双眼画像フレームに関連付けられたすべてのマップ点の位置を更新するステップと。

いくつかの実施形態では、前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成する前記ことは、閉ループ検出処理ステップを含み、前記閉ループ検出処理ステップは、現在の広視野フレームがキー広視野フレームである場合、マップデータベース中の前記現在の広視野フレームと類似する閉ループ広視野フレームを決定するステップと、前記現在の広視野フレームと前記閉ループ広視野フレームとの互いにマッチングする特徴点を決定するステップと、前記現在の広視野フレーム中のマッチングする各特徴点に対し、該特徴点に関連付けられたマップ点を、前記閉ループ広視野フレームに対応するマルチ仮想ピンホールカメラモデルの座標系に変換し、さらに前記マルチ仮想ピンホールカメラモデルのイメージング平面に投影し、該マップ点の前記閉ループ広視野フレームにおける再投影点を取得し、該再投影点及び前記閉ループ広視野フレーム中のマッチングする特徴点に基づいて第１再投影誤差を決定するステップと、前記現在の広視野フレーム中のすべてのマッチングする特徴点の第１再投影誤差に基づいて第１累計再投影誤差を決定するステップと、前記閉ループ広視野フレーム中のマッチングする各特徴点に対して、該特徴点に関連付けられたマップ点を、前記現在の広視野フレームに対応するマルチ仮想ピンホールカメラモデルの座標系に変換し、さらに前記マルチ仮想ピンホールカメラモデルのイメージング平面に投影し、該マップ点の前記現在の広視野フレームにおける再投影点を取得し、該再投影点及び前記現在の広視野フレーム中のマッチングする特徴点に基づいて第２再投影誤差を決定するステップと、前記閉ループ広視野フレーム中のすべてのマッチングする特徴点の第２再投影誤差に基づいて第２累計再投影誤差を決定するステップと、前記第１累計再投影誤差及び前記第２累計再投影誤差を利用して、マップ中の前記現在の広視野フレームとは共通視野関係を有するキー広視野フレーム及びそれに関連付けられたマップ点を補正するステップと。
いくつかの実施形態では、前記少なくとも２つの異なる向きは、キューブの前向き、上向き、下向き、左向き又は右向きを。

このアプリケーションは、一方で提供します自己位置推定と環境マップ作成を同時に行う装置であって、一組命令を含む少なくとも１つの記憶デバイスと、前記少なくとも１つの記憶デバイスと通信する少なくとも１つのプロセッサと、を備え、前記一組命令を実行する時、前記少なくとも１つのプロセッサは、前記自己位置推定と環境マップ作成を同時に行う装置に、広視野カメラによって広視野画像を取得するステップと、マルチ仮想ピンホールカメラモデルに基づいて、前記広視野画像に対応する歪み補正画像を取得するステップと、前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成するステップと、を実行させ、前記マルチ仮想ピンホールカメラモデルは少なくとも２つの異なる向きの仮想ピンホールカメラを含み、且つ前記少なくとも２つの異なる向きの仮想ピンホールカメラのカメラ中心は前記広視野カメラのカメラ中心と重なる自己位置推定と環境マップ作成を同時に行う装置。

本発明における追加の特徴は、以下の説明において部分的に説明されるであろう。この説明により、以下の図面および実施形態に記載された内容が当業者に明らかになるであろう。本発明における発明のポイントは、以下で議論される詳細な実施例に記載される方法、手段、およびそれらの組み合わせを実施または使用することにより完全に説明され得る。

以下の図面は、本発明において開示される例示的な実施形態を詳細に説明する。図面のいくつかの図において、同じ参照番号は同様の構造を示している。当業者は、これらの実施形態は非限定的な例示的実施形態であり、図面は例示および説明の目的のためだけであり、本開示の範囲を限定することを意図しないことを理解するであろう。本発明における本発明の意図も満たされる。図面は縮尺通りに描かれていないことを理解されたい。
は、本発明のいくつかの実施例による自己位置推定と環境マップ作成を同時に行うシステムを示す。は、本発明のいくつかの実施例による自己位置推定と環境マップ作成を同時に行う方法のフローチャートを示す。は、本発明のいくつかの実施例による２つの向きを含むマルチ仮想ピンホールカメラモデルを示す。は、本発明のいくつかの実施例による５つの向きを含むマルチ仮想ピンホールカメラモデルを示す。は、本発明のいくつかの実施例によるマルチ仮想ピンホールカメラモデルに基づいて歪み補正を行う模式図である。

は、本発明のいくつかの実施例による原単眼魚眼画像、従来の歪み補正後の単眼魚眼画像及び本開示の方法による歪み補正後の単眼魚眼画像を示す。は、本発明のいくつかの実施例による原双眼魚眼画像及び従来の歪み補正後の双眼魚眼画像を示す。は、本発明のいくつかの実施例によるカメラの位置姿勢特定及びマップ作成のフローチャートを示す。は、本発明のいくつかの実施形態による、単眼の広視野カメラによって構築されたマップポイントの概略図を示す。は、本発明のいくつかの実施例による双眼広視野カメラの極線検索の模式図である。は、本発明のいくつかの実施例による双眼広視野カメラのマップ点作成の模式図である。

以下の説明は、当業者がこのアプリケーションでコンテンツを作成および使用できるようにするために、このアプリケーションの特定のアプリケーションシナリオおよび要件を提供する。開示された実施形態に対する様々な修正は、当業者には明らかであり、本明細書で定義された一般原理は、本開示の精神および範囲から逸脱することなく他の実施形態に適用できる。アプリケーション。したがって、本開示は、例示された実施形態に限定されず、特許請求の範囲と一致する最も広い範囲である。

本明細書で使用される用語は、特定の例示的な実施形態のみを説明するためのものであり、限定するものではない。例えば、本明細書で使用されるように、文脈がそうではないことを明確に示さない限り、単数形「a」、「an」および「the」は複数形を含み得る。この仕様で使用される「含む」、「含む」、および/または「含む」という用語は、関連する整数、ステップ、操作、要素、および/またはコンポーネントが存在することを意味しますが、1つ以上の他の機能を除外するものではありません、整数、ステップ、操作、要素、コンポーネント、および/またはその他の機能、整数、ステップ、操作、要素、コンポーネントをシステム/メソッドに追加できます。

以下の説明を考慮すると、本開示のこれらおよび他の特徴、ならびに構造の関連要素の動作および機能、ならびに構成要素の組み合わせおよび製造の経済性を大幅に改善することができる。図面を参照すると、そのすべてが本開示の一部を形成する。ただし、図面は説明と説明のみを目的としており、開示の制限の定義を意図したものではないことを明確に理解する必要があります。

本開示で使用されるフローチャートは、本開示のいくつかの実施形態によるシステムによって実施される動作を示す。フローチャートの動作は順不同で実装できることを明確に理解する必要があります。代わりに、操作を逆順または同時に実行できます。さらに、1つ以上の他のアクションをフローチャートに追加できます。フローチャートから1つ以上のアクションを削除できます。

本開示の一態様は自己位置推定と環境マップ作成を同時に行う方法に関する。具体的には、該方法は、マルチ仮想ピンホールカメラモデルに基づいて、広視野カメラによって取得された広視野画像を歪み補正して歪み補正画像を取得するステップと、歪み補正画像に基づいて広視野カメラの位置姿勢を決定し、マップを作成するステップと、を含む。前記マルチ仮想ピンホールカメラモデルは少なくとも２つの異なる向きの仮想ピンホールカメラを含み、且つ前記少なくとも２つの異なる向きの仮想ピンホールカメラのカメラ中心は前記広視野カメラのカメラ中心と重なる。
図１は本発明のいくつかの実施例による自己位置推定と環境マップ作成を同時に行うシステムを示す。

自己位置推定と環境マップ作成を同時に行うシステム１００は広視野画像を取得し、自己位置推定と環境マップ作成を同時に行う方法を実行することができる。前記自己位置推定と環境マップ作成を同時に行う方法は図２～図１１についての説明を参照すればよい。

図示するように、自己位置推定と環境マップ作成を同時に行うシステム１００は広視野カメラ１０１、及び自己位置推定と環境マップ作成を同時に行う装置１０２を備える。前記広視野カメラ１０１と自己位置推定と環境マップ作成を同時に行う装置１０２は一体として装着されてもよく、個別装着されてもよい。本開示の発明についての説明の便宜上、本開示の広視野カメラは魚眼カメラを例とする。

広視野カメラ１０１は被写体の魚眼画像を取得する。いくつかの実施例では、広視野カメラ１０１は魚眼カメラ、反射屈折カメラ、パノラマカメラであってもよい。いくつかの実施例では、広視野カメラ１０１は単眼広視野カメラ、双眼広視野カメラ又は多眼広視野カメラであってもよい。

例として、広視野カメラ１０１は単眼魚眼カメラ及び双眼魚眼カメラを含む。双眼魚眼カメラの左側カメラは左眼と呼ばれ、双眼魚眼カメラの右側カメラは右眼と呼ばれる。左眼によって取得された画像は左魚眼画像（左視野画像）と呼ばれ、右眼によって取得された画像は右魚眼画像（右視野画像）と呼ばれる。

自己位置推定と環境マップ作成を同時に行う装置１０２は自己位置推定と環境マップ作成を同時に行う方法を実行できる例示的なコンピューティング装置である。

位置決めおよびマッピングのためのデバイス１０２は、データ通信を促進するためにＣＯＭポート１５０を含むことができる。同時に位置決めおよびマッピングするためのデバイス102は、コンピュータ命令を実行するための1つまたは複数のプロセッサの形態のプロセッサ120をさらに含むことができる。コンピュータ命令には、たとえば、本明細書で説明する特定の機能を実行するルーチン、プログラム、オブジェクト、コンポーネント、データ構造、手順、モジュール、および機能を含めることができます。例えば、プロセッサ１２０は、複数の仮想ピンホールカメラモデルに基づいて魚眼画像の歪んだ画像を決定してもよい。別の例として、プロセッサ１２０は、広視野カメラ１０１の姿勢を決定し、歪んだ画像に基づいて地図を構築してもよい。

いくつかの実施形態では、プロセッサ１２０は、マイクロコントローラ、マイクロプロセッサ、縮小命令セットコンピュータ（ＲＩＳＣ）、特定用途向け集積回路（ＡＳＩＣ）、特定用途向けなどの１つ以上のハードウェアプロセッサを含み得る。命令セットプロセッサ（ASIP）、中央処理装置（CPU）、グラフィック処理装置（GPU）、物理処理装置（PPU）、マイクロコントローラユニット、デジタルシグナルプロセッサ（DSP）、フィールドプログラマブルゲートアレイ（FPGA））、アドバンストRISCマシン（ARM）、プログラマブルロジックデバイス（PLD）、1つ以上の機能を実行できる回路またはプロセッサなど、またはそれらの組み合わせ。

いくつかの実施形態では、同時位置決めおよびマッピングのためのデバイス１０２は、内部通信バス１１０、プログラムストレージ、および異なる形式のデータストレージ（例えば、ディスク１７０、読み取り専用メモリ（ＲＯＭ）１３０、またはランダムアクセス）を含み得る。メモリ（RAM）140）。同時位置決めおよびマッピングのためのデバイス102は、ROM 130、RAM 140、および/またはプロセッサ120によって実行される他のタイプの非一時的記憶媒体に格納されたプログラム命令も含み得る。本発明の方法および／またはプロセスは、プログラム命令として実装されてもよい。同時位置決めおよびマッピングのためのデバイス１０２はまた、コンピュータと他のコンポーネント（例えば、ユーザーインターフェース要素）との間の入力／出力をサポートするＩ／Ｏコンポーネント１６０を含む。位置決めとマッピングを同時に行うデバイス102は、ネットワーク通信を通じてプログラミングとデータを受信することもできます。

例示のみを目的として、本発明における同時の位置決めおよびマッピングのために、デバイス102には1つのプロセッサのみが記載されている。しかしながら、本発明における同時測位およびマッピングのためのデバイス102は複数のプロセッサを含むこともできることに留意すべきであり、したがって、本発明に開示される動作および/または方法ステップは、本開示に記載される1つのプロセッサによって実行され得る、複数のプロセッサが共同で実行することもできます。例えば、本発明における位置決めおよびマッピングのためのデバイス１０２のプロセッサ１２０がステップＡおよびＢを実行する場合、ステップＡおよびＢは、情報処理における２つの異なるプロセッサによって結合または分離され得ることを理解されたい。実行（たとえば、第1プロセッサがステップAを実行し、第2プロセッサがステップBを実行するか、第1プロセッサと第2プロセッサがステップAとBを一緒に実行します）。

図２は本発明のいくつかの実施例による自己位置推定と環境マップ作成を同時に行う方法のフローチャートを示す。プロセス２００は自己位置推定と環境マップ作成を同時に行う装置１０２中の非一時的な記憶媒体における）一組命令として実施できる。自己位置推定と環境マップ作成を同時に行う装置１０２は該一組命令を実行し且つプロセス２００のステップを対応付けて実行する。

以下に提示される例示されたプロセス２００の動作は、例示的であり、限定的ではないことが意図される。いくつかの実施形態では、プロセス２００は、説明されていない１つ以上の追加の動作を追加し、および／または本明細書で説明された１つ以上の動作を削除することができる。さらに、図２に示され、以下で説明される動作の順序は、これに対する制限ではない。
２１０では、自己位置推定と環境マップ作成を同時に行う装置１０２は広視野カメラ１０１によって広視野画像を取得することができる。

広視野カメラ１０１が単眼広視野カメラである場合、単眼広視野カメラが広視野画像を取得し、広視野カメラ１０１が双眼広視野カメラである場合、双眼広視野カメラが、左視野画像及び右視野画像を含む広視野画像を取得する。

２２０では、自己位置推定と環境マップ作成を同時に行う装置１０２はマルチ仮想ピンホールカメラモデルに基づいて、前記広視野画像に対応する歪み補正画像を取得することができる。

広視野カメラ１０１が単眼広視野カメラである場合、自己位置推定と環境マップ作成を同時に行う装置１０２はマルチ仮想ピンホールカメラモデルに基づいて、前記広視野画像に対応する歪み補正画像を取得することができる。

上記マルチ仮想ピンホールカメラモデルは少なくとも２つの異なる向きの仮想ピンホールカメラを含み、且つ前記少なくとも２つの異なる向きの仮想ピンホールカメラのカメラ中心は前記単眼広視野カメラのカメラ中心と重なる。

広視野カメラ１０１が双眼広視野カメラである場合、自己位置推定と環境マップ作成を同時に行う装置１０２は第１マルチ仮想ピンホールカメラモデルに基づいて、前記左視野画像に対応する左歪み補正画像を取得し、第２マルチ仮想ピンホールカメラモデルに基づいて、前記右視野画像に対応する右歪み補正画像を取得する。前記第１マルチ仮想ピンホールカメラモデルと前記第２マルチ仮想ピンホールカメラモデルは同じであってもよく、異なってもよい。

上記第１マルチ仮想ピンホールカメラモデルは少なくとも２つの異なる向きの仮想ピンホールカメラを含み、前記少なくとも２つの異なる向きの仮想ピンホールカメラのカメラ中心は広視野カメラ１０１の左眼のカメラ中心と重なるようにしてもよく、上記第２マルチ仮想ピンホールカメラモデルは少なくとも２つの異なる向きの仮想ピンホールカメラを含み、前記少なくとも２つの異なる向きの仮想ピンホールカメラのカメラ中心は広視野カメラ１０１の右眼のカメラ中心と重なるようにしてもよい。

例として、図３は本発明のいくつかの実施例による２つの向きを含むマルチ仮想ピンホールカメラモデルを示す。２つの仮想ピンホールカメラの向きは９０°の夾角をなし、且つカメラ中心は広視野カメラのカメラ中心とＣ点で重なる。

例として、図４は本発明のいくつかの実施例による５つの向きを含むマルチ仮想ピンホールカメラモデルを示す。図示するように、該マルチ仮想ピンホールカメラモデルはキューブの前向き、上向き、下向き、左向き及び右向きの計５つの向きの仮想ピンホールカメラを含む。５つの仮想ピンホールカメラのカメラ中心は広視野カメラのカメラ中心とＣ点で重なる。このとき、上記歪み補正方法は、キューブマップに基づく歪み補正方法（ｃｕｂｅｍａｐ－ｂａｓｅｄｕｎｄｉｓｔｏｒｔｉｏｎｍｅｔｈｏｄ）（以下、Ｃｕｂｅ又はキューブモデルと略称する）と呼ばれる。

具体的には、自己位置推定と環境マップ作成を同時に行う装置１０２は広視野画像（又は左視野画像、右視野画像）をマルチ仮想ピンホールカメラモデル（又は第１マルチ仮想ピンホールカメラモデル、第２マルチ仮想ピンホールカメラモデル）に投影し、少なくとも２つの異なる向きの仮想ピンホールカメラの投影図を取得し、前記少なくとも２つの異なる向きの仮想ピンホールカメラの投影図を展開すると、前記左魚眼画像に対応する歪み補正画像を取得することができる。

図５に示すように、本発明のいくつかの実施例によるマルチ仮想ピンホールカメラモデルに基づいて歪み補正を行う模式図である。以下、第１マルチ仮想ピンホールカメラモデル及び左視野画像を例とする。

点Ａは双眼広視野カメラの左眼のカメラ中心、点Ｂ、点Ｃ及び点Ｄは左視野画像中の例示的な画素である。第１マルチ仮想ピンホールカメラ５１０はキューブモデルであり、５つの向きの仮想ピンホールカメラを含み、それぞれキューブの前向き、上向き、下向き、左向き及び右向きである。前記５つの向きの仮想ピンホールカメラのカメラ中心は点Ａで重なる。

図示するように、左視野画像を第１マルチ仮想ピンホールカメラモデル５１０の５つの異なる向きの仮想ピンホールカメラのイメージング平面に投影する。それに対応して、５つの異なる向きの投影図を取得する。前記５つの異なる向きの投影図を展開すると、左歪み補正画像を取得する。

図６に示すように、本発明のいくつかの実施例による原単眼魚眼画像、従来の歪み補正後の単眼魚眼画像及び本開示の方法による歪み補正後の単眼魚眼画像を示す。

図６１０は単眼魚眼カメラによって取得された広視野画像である。図示からわかるように、該広視野画像は普通のカメラによって取得された画像よりも広視野があるが、全画像に空間の歪みが存在し、画像の中心から離れるほご、歪みが大きい。

図６２０は従来の歪み補正方法によって該広視野画像に対して歪み補正処理を行って取得された歪み補正画像である。普通のカメラによって取得された画像の画角は一般には約８０°であるが、図６２０の画角は１００°である。普通のカメラによって取得された画像の画角よりも拡大したが、歪み補正処理前の画像に比べて多くの画角が損失してしまう。従って、広視野画像を含むすべての画角のマップを作成できない。

図６３０は本発明の一実施例による５つの向きのマルチ仮想ピンホールカメラモデル基づく歪み補正後に展開された広視野歪み補正画像であり、すなわち、キューブモデルによって取得された歪み補正画像である。図示するように、図６３０は広視野画像のすべての画角を保持する。該広視野歪み補正画像に基づいてＳＬＡＭを行うことによって、元のすべての画角コンテンツを含むマップを作成できる。
図７は本発明のいくつかの実施例による原双眼魚眼画像及び従来の歪み補正後の双眼魚眼画像を示す。

図示するように、画像７０１及び画像７０２はそれぞれ広視野カメラ１０１によって実世界で取得された原左魚眼画像及び右魚眼画像である。画像７０３及び画像７０４はそれぞれ従来の歪み補正後の左歪み補正画像及び右歪み補正画像である。

キューブモデルによって処理された左歪み補正画像及び右歪み補正画像（図示せず）との比較として、画像６０１及び画像６０２から従来の歪み補正方法によって取得された単一画像は、画像の縦方向と横方向の画角がともに１００°だけである。以上からわかるように、広視野カメラ１０１によって取得された広視野画像に対し、本発明に係る歪み補正方法は広視野を保持するとともに、画像歪みを効果的に防止することができる。

２３０では、自己位置推定と環境マップ作成を同時に行う装置１０２は前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成する。

いくつかの実施例では、単眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２は歪み補正画像の特徴点を抽出し、抽出された特徴点に基づいて対応する広視野フレームを作成し、次に前記広視野フレームに基づいて、単眼広視野カメラの位置姿勢を決定し、マップを作成することができる。

好ましくは、広視野歪み補正画像の特徴点、すなわち、広視野歪み補正画像のキーポイント及び記述子を抽出することによって、広視野歪み補正画像の特徴点に基づいて、カメラの動きの位置姿勢を追跡し、マップを作成する。好ましくは、キーポイント及び記述子を計算せずに、直接広視野歪み補正画像中の画素輝度情報に基づいてカメラの動きの位置姿勢を推定し、マップを作成する。

上記マルチ仮想ピンホールカメラモデルに基づく歪み補正方法によって取得された広視野歪み補正画像は原広視野画像のすべての画角を保持する。それにより、広視野画像間の豊富な共有特徴に基づいて自己位置推定と環境マップ作成を同時に行うことができ、より効率的な自己位置推定及びより正確なマップを取得できる。それと同時に、上記方法によって、さらに広視野カメラの複雑な投影モデルによるシステムの計算コストの増加を回避できる。

いくつかの実施例では、双眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２は左歪み補正画像及び右歪み補正画像の特徴点を抽出し、抽出された特徴点に基づいて対応する双眼画像フレームを作成し、次に、前記双眼画像フレームに基づいて双眼広視野カメラの位置姿勢を決定し、マップを作成することができる。

広視野フレーム（又は双眼画像フレーム）に歪み補正画像（又は左歪み補正画像、右歪み補正画像）中のすべての特徴点の情報が含まれるため、それによって広視野カメラ１０１の動きの位置姿勢を追跡し、マップを作成することができる。

例として、自己位置推定と環境マップ作成を同時に行う装置１０２は上記歪み補正画像（又は左歪み補正画像、右歪み補正画像）を拡大縮小し、該歪み補正画像（又は左歪み補正画像、右歪み補正画像）に対応する画像ピラミッド（ＩｍａｇｅＰｙｒａｍｉｄ）を取得する。該画像ピラミッドの各スケール画像からコーナーを抽出し記述子を計算する。画像の特徴点が前記コーナー及び記述子で構成される。前記コーナーは、画像中の認識度が高くて代表性がある領域であり、特徴点の画像における位置情報を示すためのものである。記述子はベクトルで示してもよく、コーナーの周囲画素の情報を記述するためのものである。記述子は、外観が類似する特徴点に、類似する記述子を付与するように設計されてもよい。

歪み補正画像（又は左歪み補正画像、右歪み補正画像）に対してその特徴点を抽出し、抽出された特徴点に基づいて対応する広視野フレーム（又は双眼画像フレーム）を作成する。該広視野フレーム（又は双眼画像フレーム）は対応する歪み補正画像（又は左歪み補正画像、右歪み補正画像）中のすべての特徴点を含む。広視野フレーム（又は双眼画像フレーム）の作成完了後、該広視野フレーム（又は双眼画像フレーム）に対応する歪み補正画像（又は左歪み補正画像、右歪み補正画像）の画素データを破棄し、それにより保存領域を節約し、システムの電力消費量を低減させる。
ステップ２３０についての更なる説明は図８及びその関連説明を参照すればよい。

なお、広視野カメラ１０１が双眼広視野カメラであ場合、双眼広視野カメラの左眼と右眼の光軸は平行ではない可能性がある。従って、プロセス２００は、広視野カメラ１０１の左眼と右眼の光軸を平行にするステップをさらに含んでもよい。例えば、自己位置推定と環境マップ作成を同時に行う装置１０２は双眼カメラ補正プログラムによって双眼魚眼カメラの左眼と右眼の仮想光軸を平行に調整することができる。

図８は本発明のいくつかの実施例によるカメラの位置姿勢特定及びマップ作成のフローチャートを示す。プロセス２３０は自己位置推定と環境マップ作成を同時に行う装置１０２中の非一時的な記憶媒体における一組命令として実施されてもよい。自己位置推定と環境マップ作成を同時に行う装置１０２は該一組命令を実行し、それに対応してプロセス２００のステップを実行することができる。

以下、説明されるプロセス２３０の操作は単に説明を目的とし、限定的なものではない。いくつかの実施例では、プロセス２３０は実現時、説明されていない１つ又は複数の操作を追加し、及び／又はここで説明される１つ又は複数の操作を削除することができる。また、図８に示される後述される操作の順序を限定しない。

８１０では、自己位置推定と環境マップ作成を同時に行う装置１０２は初期化ステップを実行することができ、前記初期化ステップでは、初期マップを作成することができる。

単眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２は２つの異なる時点における歪み補正画像（又は広視野フレーム）を取得し、前記２つの異なる時点における歪み補正画像（又は広視野フレーム）の互いにマッチングする特徴点を決定し、前記互いにマッチングする特徴点に基づいて初期マップを作成することができる。

例として、自己位置推定と環境マップ作成を同時に行う装置１０２は第１時点に対応する歪み補正画像（又は広視野フレーム）と第２時点に対応する歪み補正画像（又は広視野フレーム）を取得し、前記第１時点に対応する歪み補正画像（又は広視野フレーム）と前記第２時点に対応する歪み補正画像（又は広視野フレーム）の互いにマッチングする特徴点を決定し、前記互いにマッチングする特徴点に基づいて初期マップを作成することができる。

いくつかの実施例では、第１時点に対応する広視野フレームと第２時点に対応する広視野フレームは現在の広視野フレームと基準広視野フレームであってもよい。前記現在の広視野フレームと基準広視野フレームは連続フレームであってもよく、それらの間に１つ又は複数の間隔フレームがあってもよい。初期化をスムーズに行うことを確保するように、現在の広視野フレームと基準広視野フレームの間に所定の視差を必要とする。

いくつかの実施例では、自己位置推定と環境マップ作成を同時に行う装置１０２はマルチ仮想ピンホールカメラモデル（例えば、図４に示されるマルチ仮想ピンホールカメラモデル）に基づいて、第１時点に対応する歪み補正画像（又は広視野フレーム）と第２時点に対応する歪み補正画像（又は広視野フレーム）を、それぞれ各仮想ピンホールカメラに対応するサブ視野フレームに分解することができる。それにより各仮想ピンホールカメラに、それに対応する２つのサブ視野フレームを取得し、これら２つのサブ視野フレームはそれぞれ第１時点に対応する歪み補正画像（又は広視野フレーム）及び第２時点に対応する歪み補正画像（又は広視野フレーム）の由来である。これら２つのサブ視野フレームに対してフレーム間マッチングを行うことで、互いにマッチングする特徴点を決定する。

いくつかの実施例では、互いにマッチングする特徴点に基づいて初期マップを作成することは、前記第１時点に対応する歪み補正画像中の特徴点及び前記第１時点における前記広視野カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定するステップと、前記第２時点に対応する歪み補正画像中のマッチングする特徴点及び前記第２時点における前記広視野カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定するステップと、前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定するステップと、前記マップ点に基づいて初期マップを作成するステップと、を含む。

具体的には、自己位置推定と環境マップ作成を同時に行う装置１０２は基準広視野フレームＦ１を該マルチ仮想ピンホールカメラモデルに基づいて、それぞれ各仮想ピンホールカメラに対応するサブ視野フレームＦ１１、Ｆ１２、Ｆ１３、Ｆ１４及びＦ１５に分解し、現在の広視野フレームＦ２も該マルチ仮想ピンホールカメラモデルに基づいて、それぞれ各仮想ピンホールカメラに対応するサブ視野フレームＦ２１、Ｆ２２、Ｆ２３、Ｆ２４及びＦ２５に分解する。ここで、サブ視野フレームＦ１１及びＦ２１は前向きの仮想ピンホールカメラに対応し、サブ視野フレームＦ１２及びＦ２２は上向きの仮想ピンホールカメラに対応し、サブ視野フレームＦ１３及びＦ２３は下向きの仮想ピンホールカメラに対応し、サブ視野フレームＦ１４及びＦ２４は左向きの仮想ピンホールカメラに対応し、サブ視野フレームＦ１５及びＦ２５は右向きの仮想ピンホールカメラに対応する。サブ視野フレームＦ１１及びＦ２１、Ｆ１２及びＦ２２、Ｆ１３及びＦ２３、Ｆ１４及びＦ２４、並びにＦ１５及びＦ２５に対してフレーム間マッチングを行うことで、現在の広視野フレームと基準広視野フレームの互いにマッチングする特徴点を決定する。ここで、サブ視野フレームのマッチングは２つの視野フレームの互いにマッチングする特徴点を決定し、さらに方向ベクトルに基づいて三角測量を行うことで新たなマップ点を作成する。
以下、サブ視野フレームＦ１１及びＦ２１を例にフレーム間マッチングを説明する。

まず、サブ視野フレームＦ１１及びＦ２１の特徴点をマッチングし、マッチングする特徴点ペアの数が初期化閾値以上であるか否かを検出し、初期化閾値未満であると、初期化が失敗する。マッチングする特徴点ペアの数が初期化閾値を超える場合、例えば、ランダムサンプルコンセンサスアルゴリズム（ＲａｎｄｏｍＳａｍｐｌｅＣｏｎｓｅｎｓｕｓ、ＲＡＮＳＡＣと略称する）を用いて、マッチングする特徴点ペアの方向ベクトルに基づいて、２つのフレーム間のエッセンシャル（Ｅｓｓｅｎｔｉａｌ）行列を計算する。初期化閾値は初期マップを作成することに必要な最小限の特徴点ペアの数を示し、直接デフォルト値、例えば１００を使用してもよく、ユーザーによって予め設定されてもよい。

次に、エッセンシャル行列を分解して現在の広視野フレームと基準広視野フレームとの相対位置姿勢を取得し、前記相対位置姿勢は位置姿勢行列で示され得る。現在の広視野フレームと基準広視野フレームとの相対位置姿勢に基づいて、マッチングする特徴点ペアに三角測量を行い、該特徴点ペアに対応するマップ点の三次元座標、すなわちマップ点の位置を取得する。

図９に示すように、Ｏ１点はサブ視野フレームＦ１１に対応する仮想ピンホールカメラのカメラ中心、Ｏ２点はサブ視野フレームＦ１２に対応する仮想ピンホールカメラのカメラ中心、ｐ１とｐ２はマッチングする特徴点である。ベクトルＯ１ｐ１の方向とベクトルＯ２ｐ２の方向によって、マップ点の三次元座標、すなわちＰ点の位置を決定できる。ＳＬＡＭでは、ノイズの影響によって、ベクトルＯ１ｐ１とベクトルＯ２ｐ２が交差しない可能性があり、この場合、例えば、最小二乗法で誤差を最小にするＰ点の座標を求める。Ｏ１とＯ２の間の距離は三角測量の誤差に大きな影響を与える。距離が小さすぎ、つまり、カメラのシフトが小さすぎる場合、Ｐ点で観測される角度誤差は大きな深度誤差につながる。一方、距離が大きすぎる場合、シーンの重畳部分が大幅に減少し、特徴マッチングが困難になる。従って、現在の広視野フレームと基準広視野フレームの間に所定の視差を必要とする。選択された２つの広視野フレームがこの要件を満たさないと、初期化が失敗し、これら２つの広視野フレームを破棄し、再初期化を行う。

最後に、上記三角測量に基づいてマップ点の三次元座標を取得し初期マップ点を作成する。該三次元座標をマップ点の座標とし、該三次元座標に対応する特徴点の記述子をマップ点の記述子とする。

双眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２は上記単眼広視野カメラの初期化ステップを実行してもよく、同一時点における左歪み補正画像と右歪み補正画像の互いにマッチングする特徴点に基づいて初期マップを作成してもよい。

例として、自己位置推定と環境マップ作成を同時に行う装置１０２は左歪み補正画像と右歪み補正画像の互いにマッチングする特徴点を決定し、前記互いにマッチングする特徴点に基づいて初期マップを作成することができる。

いくつかの実施例では、自己位置推定と環境マップ作成を同時に行う装置１０２は、右歪み補正画像における左歪み補正画像中の特徴点に対応する極線を決定し、次に、前記極線上で前記左歪み補正画像中の特徴点にマッチングする特徴点を検索することができる。前記極線は多線分折れ線である。

図１０に示すように、本発明のいくつかの実施例による双眼広視野カメラの極線検索の模式図である。左歪み補正画像１０１０に極線１００１を有し、右歪み補正画像１０２０に極線１００２を有する。左歪み補正画像１０１０の特徴点にマッチングする特徴点は必ず極線１００２にある。一方、右歪み補正画像１０２０の特徴点にマッチングする特徴点は極線１００１にある。従って、極線検索によって、左歪み補正画像と右歪み補正画像の互いにマッチングする特徴点を素早くに見つけることができる。
図示するように、極線１００１と極線１００２は３線分折れ線であり、２本の傾斜した線分及び１本の水平線分を含む。

図示するように、左歪み補正画像１０１０と右歪み補正画像１０２０はそれぞれ左魚眼画像と右魚眼画像のすべての画角を保持する。左歪み補正画像１０１０と右歪み補正画像１０２０に基づいて自己位置推定と環境マップ作成を同時に行うことによって、元のすべての画角コンテンツを含むマップを作成することができる。

いくつかの実施例では、上記した前記互いにマッチングする特徴点に基づいてマップを作成することは、まず、左歪み補正画像中の特徴点及び広視野カメラ１０１の左眼のカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定し、次に、右歪み補正画像中のマッチングする特徴点及び広視野カメラ１０１の右眼のカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定し、さらに、前記双眼魚眼カメラの基線に基づいて、前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定し、最後に、前記マップ点に基づいてマップを作成するステップを含む。

図１１に示すように、本発明のいくつかの実施例による双眼広視野カメラのマップ点作成の模式図である。以下、広視野カメラ１０１の前方のマップ点を例とする。

点Ｏ１は広視野カメラ１０１の左眼のカメラ中心であり、左歪み補正画像中の特徴点と点Ｏ１を結んで第１特徴点に対応する方向ベクトルを取得する。点Ｏ２は広視野カメラ１０１の右眼のカメラ中心であり、右歪み補正画像中のマッチングする特徴点と点Ｏ２を結んで第２特徴点に対応する方向ベクトルを取得する。いくつかの実施例では、第１特徴点に対応する方向ベクトル及び第２特徴点に対応する方向ベクトルは単位化後のベクトルである。

前記第１特徴点に対応する方向ベクトルと前記第２特徴点に対応する方向ベクトルが点Ｅで交差し、それぞれ線分Ｏ１Ｅと線分Ｏ２Ｅを取得する。点Ｏ１と点Ｏ２を結んで線分Ｏ１Ｏ２を取得し、線分Ｏ１Ｏ２の長さがｂ（すなわち、広視野カメラ１０１の基線）である。線分Ｏ１Ｏ２と線分Ｏ１Ｅと線分Ｏ２Ｅとが三角形を形成する。前記三角形を解いて、線分Ｏ１Ｅの長さをｄ１、線分Ｏ２Ｅの長さをｄ２、Ｏ１Ｏ２と線分Ｏ１Ｅとの夾角をα1、Ｏ１Ｏ２と線分Ｏ２Ｅとの夾角をと取得しα2、さらに特徴点に対応するマップ点Ｅの座標を取得する。さらに、広視野カメラ１０１の現在の位置姿勢に合わせて、マップ点Ｅを広視野カメラ１０１の座標系から世界座標系に変換する。次に、世界座標系中の点Ｅの位置に基づいてマップを作成する。

具体的には、自己位置推定と環境マップ作成を同時に行う装置１０２は以下の式に基づいて三角測量を行う。まず正弦・余弦定理に基づいて式（１）、（２）及び（３）を取得する。

いくつかの実施例では、単眼広視野カメラと双眼広視野カメラによって新たに作成されたマップ点に対して、さらに関連付け処理を行う必要がある。１つのマップ点は複数のキー広視野フレームによって観測でき、このマップ点を観測したキー広視野フレームをこのマップ点に関連付けると同時に、このマップ点に関連付けられたキー広視野フレーム上の特徴点、すなわち、このマップ点を取得するように測量に適用できる特徴点を記録する。上記初期化によって取得されたマップ点に対して、初期化によって作成された２つのキー広視野フレームを関連付ける必要があると同時に、該マップ点に関連付けられたこれらの２つのキー広視野フレーム上の特徴点を記録する。
作成された初期マップは、上記２つのキー広視野フレーム及び上記初期マップ点、並びにそれらの関連付け関係の情報を含む。

いくつかの実施例では、初期化ステップは、マッチングする特徴点ペアの数が初期化閾値を超える場合、上記２つのキー広視野フレームに基づいて、単語の袋（ＢａｇｏｆＷｏｒｄ）モデルに基づくベクトルを作成し、前記単語の袋モデルに基づくベクトルをマップデータベースに追加するステップをさらに含む。単語の袋モデルにおいて、各種の画像特徴に基づいてクラスタリングする。例えば、目、鼻、耳、口、各種の特徴のエッジ及び角等は異なる特徴クラスである。１００００クラスがあるとすると、各々の広視野フレームに対して、含まれるクラスを分析し、有りは１で示され、無しは０で示される。この場合、この広視野フレームは１つの１００００次元のベクトルで表現できる。異なる広視野フレームに対して、それらの単語の袋モデルに基づくベクトルを比較してそれらの類似度を判断できる。前記マップデータベースはキー広視野フレームに基づいて作成された単語の袋モデルに基づくベクトルを格納するために用いられる。

８２０では、自己位置推定と環境マップ作成を同時に行う装置１０２はグローバルバンドル最適化ステップを実行することができる。グローバルバンドル最適化は、ＳＬＡＭによって現在作成されているマップ（以下、現在のマップと略称する）中のすべてのキー広視野フレーム（又はキー双眼画像フレーム）及びすべてのマップ点を最適化する。例えば、ステップ８１０で作成された初期マップに対してグローバルバンドル最適化を行うことは、上記２つのキー広視野フレーム及びマップ点のみを有するマップに対してグローバルバンドル最適化を行うことである。なお、初期マップを除き、マップ作成中の任意の時点において、該時点における現在のマップに対して該グローバルバンドル最適化を実行してもよい。バンドル最適化は、マップ中のキー広視野フレーム（又はキー双眼画像フレーム）の位置姿勢及びマップ点の位置を微調整し、ＳＬＡＭによって作成されたマップ中のマップ点のキー広視野フレーム（又はキー双眼画像フレーム）における再投影誤差を最小化し、それにより作成されたマップを最適化することを目的とする。

単眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２はマップ中のキー広視野フレームに関連付けられた各マップ点をマルチ仮想ピンホールカメラモデルに投影し、各マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、各マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点及び該マップ点に対応する特徴点に基づいて、各マップ点の再投影誤差を決定し、すべての前記キー広視野フレームに関連付けられたマップ点の再投影誤差に基づいて再投影誤差を決定し、前記再投影誤差に基づいて、前記キー広視野フレームの位置姿勢及び前記キー広視野フレームに関連付けられたすべてのマップ点の位置を更新する。

なお、本発明では、フレームの位置姿勢（例えば、キー広視野フレームの位置姿勢）は広視野カメラ１０１が該フレームを取得する時点における広視野カメラ１０１の動きの位置姿勢であり、簡潔にするために、フレームの位置姿勢と呼ぶ。

図４に示される５つの向きのマルチ仮想ピンホールカメラモデルを例に、該マルチ仮想ピンホールカメラモデルに基づいて、マップ点を対応する仮想ピンホールカメラの座標系に変換し、例えば、該マップ点に対応する仮想ピンホールカメラは前向きの仮想ピンホールカメラであり、さらに該前向きの仮想ピンホールカメラのイメージング平面に投影して該マップ点の再投影点を取得する。ここで使用されるマルチ仮想ピンホールカメラモデルがステップ２２０で広視野画像歪み補正処理に使用されるマルチ仮想ピンホールカメラモデルと同じであるため、該イメージング平面は前向きの仮想ピンホールカメラに分解される該キー広視野フレームのサブ視野フレームに対応する。該再投影点は、該サブ視野フレームの位置姿勢に基づく該マップ点の観測値として理解され得る。該マップ点に関連付けられた該キー広視野フレーム上の特徴点（すなわち、三角測量によって該マップ点の根拠となる特徴点を取得する）及び該マップ点の再投影点に基づいて該マップ点の再投影誤差を決定する。ＳＬＡＭによって作成されたマップに誤差がなくて理想的な場合、再投影誤差が０である。しかし、現実条件において測量誤差等の誤差が不可避的に発生するため、再投影誤差を完全に無くすことができず、ＳＬＡＭは該再投影誤差を最小化することによって、作成されたマップを最適化する。

双眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２はマップ中のキー双眼画像フレームに関連付けられた各マップ点を第１マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて左再投影誤差を決定する。

或いは、自己位置推定と環境マップ作成を同時に行う装置１０２は前記キー双眼画像フレームに関連付けられたマップ点を第２マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて右再投影誤差を決定する。

さらに、自己位置推定と環境マップ作成を同時に行う装置１０２は左再投影誤差、右再投影誤差又はそれらの和に基づいて、キー双眼画像フレームの位置姿勢及び前記キー双眼画像フレームに関連付けられたすべてのマップ点の位置を更新することができる。具体的には、単眼マップ点の場合、自己位置推定と環境マップ作成を同時に行う装置１０２は左再投影誤差又は右再投影誤差に基づいて、キー双眼画像フレームの位置姿勢及び双眼画像フレームに関連付けられたすべてのマップ点の位置を更新し、双眼マップ点の場合、自己位置推定と環境マップ作成を同時に行う装置１０２は左再投影誤差と右再投影誤差との和に基づいて、キー双眼画像フレームの位置姿勢及び双眼画像フレームに関連付けられたすべてのマップ点の位置を更新することができる。

いくつかの実施例では、自己位置推定と環境マップ作成を同時に行う装置１０２は再投影誤差（例えば、左再投影誤差、右再投影誤差、左再投影誤差と右再投影誤差との和）に基づいて、損失関数を決定することができる。損失関数を取得した後、好ましくは、ガウスニュートン法（Ｇａｕｓｓ－Ｎｅｗｔｏｎ）、レーベンバーグ－マルカート法（Ｌｅｖｅｎｂｅｒｇ－Ｍａｒｑｕａｒｄｔ）等の勾配降下方法によって反復してキー広視野フレーム（又はキー双眼画像フレーム）の位置姿勢及びそれに関連付けられたマップ点の位置のそれぞれに対応する勾配を求め、それぞれに対応する勾配によってキー広視野フレーム（又はキー双眼画像フレーム）の位置姿勢及びそれに関連付けられたマップ点の位置を更新し、最終的に現在のマップを、再投影誤差が最小になる最適状態にする。

上記バンドル最適化は広視野画像の歪み補正処理と同様なマルチ仮想ピンホールカメラモデルに基づいて、複雑な広視野カメラの投影モデルを複数の仮想ピンホールカメラ投影モデルに変換する。それにより広視野カメラの複雑な投影モデルによる複雑な最適化処理を回避し、それによりシステムの処理性能を向上させる。

８３０では、自己位置推定と環境マップ作成を同時に行う装置１０２は追跡ステップを実行することができる。前記追跡ステップは現在の広視野フレーム（又は現在の双眼画像フレーム）におけるマップ点の再投影誤差を最小化することによって現在の広視野カメラの位置姿勢を最適化する。追跡ステップでは、現在の広視野カメラの位置姿勢のみを最適化し、ほかの時点における広視野カメラの位置姿勢及びマップ点の位置を維持する。ステップ８３０はマップ作成中の任意の時点において行われてもよく、例えば上記初期化ステップ８１０で作成された初期マップ又は上記グローバルバンドル最適化ステップ８２０で最適化されたマップに基づいて、ＳＬＡＭは新たな広視野フレーム（又は双眼画像フレーム）によって広視野カメラの動きの位置姿勢を連続的に追跡する。

単眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２は現在の広視野フレームに関連付けられた各マップ点をマルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記現在の広視野フレームに関連付けられたマップ点の再投影誤差に基づいて再投影誤差を決定し、前記再投影誤差に基づいて、前記現在の広視野フレームの位置姿勢を更新する。

いくつかの実施例では、自己位置推定と環境マップ作成を同時に行う装置１０２は以下の３つのサブステップを実行することによって追跡ステップを行うことができる。
追跡サブステップ１では、現在の広視野フレームの基準広視野フレームを決定する。
好ましくは、現在の広視野フレームの1つ前の広視野フレームを基準広視野フレームとして決定する。

好ましくは、ローカルマップ中の、現在の広視野フレームとの共通視野程度が最も高いキー広視野フレームを基準広視野フレームとして選択する。現在のマップ中のキー広視野フレームの数がＮ（Ｎは２より大きい整数）未満の場合、ローカルマップは現在のマップ中のすべてのキー広視野フレーム及びすべてのマップ点を含む。Ｎは直接デフォルト値、例えば１０を使用してもよく、ユーザーによって予め設定されてもよい。現在のマップが初期化によって取得されたマップであると、ローカルマップは現在のマップであり、初期の２つのキー広視野フレーム及びそれに関連付けられたマップ点を含む。現在のマップ中のキー広視野フレームの数がＮ以上の場合、ローカルマップは現在のマップ中の、現在の広視野フレームとの共通視野程度が最も高い少なくともＮ個のキー広視野フレーム及び前記少なくともＮ個のキー広視野フレームに関連付けられたマップ点を含む。

好ましくは、ローカルマップ中の、現在の広視野フレームの1つ前の広視野フレームとの共通視野程度が最も高いキー広視野フレームを基準広視野フレームとして選択する。現在の広視野フレームとその1つ前の広視野フレームとの共通視野程度が一般的に高いため、後者に基づいて現在の広視野フレームの基準広視野フレームを選択できる。作成直後の現在の広視野フレームとの共通視野程度が最も高いキー広視野フレームに比べて、その1つ前の広視野フレームとの共通視野程度が最も高いキー広視野フレームのほうは選択しやすい。それにより、ＳＬＡＭ方法をスムーズに実現することに有利である。

好ましくは、グローバルマッチングによって基準広視野フレームを決定する。まず、現在の広視野フレームに基づいて、単語の袋モデルに基づくベクトルを作成する。次に、該単語の袋モデルに基づくベクトルに基づいて、初期化ステップ８１０で作成されたマップデータベースを照会し、現在の広視野フレームにマッチングするキー広視野フレームを、基準広視野フレームとして取得する。

一例では、現在の広視野フレームとその1つ前の広視野フレームとをマッチングし、マッチングする特徴点ペアを取得する。マッチングする特徴点ペアの数が追跡閾値より大きい場合、現在の広視野フレームの1つ前の広視野フレームを基準広視野フレームとして決定する。追跡閾値は広視野カメラの位置姿勢追跡に必要な最小限の特徴点ペアの数を示し、直接デフォルト値、例えば２０を使用してもよく、ユーザーによって予め設定されてもよい。

現在の広視野フレームとその1つ前の広視野フレームとのマッチングする特徴点ペアの数が追跡閾値以下である場合、ローカルマップ中の、現在の広視野フレーム又はその前1つの広視野フレームとの共通視野程度が最も高いキー広視野フレームを選択して、現在の広視野フレームと該キー広視野フレームとをマッチングし、マッチングする特徴点ペアを取得する。マッチングする特徴点ペアの数が追跡閾値より大きい場合、該キー広視野フレームを基準広視野フレームとして決定する。

現在の広視野フレームと該キー広視野フレームとのマッチングする特徴点ペアの数が追跡閾値以下である場合、グローバルマッチングによって基準広視野フレームを決定する。具体的な決定プロセスは上述した通りであり、簡潔にするために、ここでは重複説明を省略する。
それにより、現在の広視野フレームの参照性がより高い基準広視野フレームを取得でき、ＳＬＡＭ追跡がさらに正確になり、マップ作成も効率的になる。

追跡サブステップ２では、現在の広視野フレーム及び上記決定された基準広視野フレームによって、マルチ仮想ピンホールカメラモデルに基づいて現在の広視野フレームの位置姿勢を決定する。一例では、現在の広視野フレームと基準広視野フレームとの相対位置姿勢を決定することで、現在の広視野フレームの位置姿勢を決定する。

マルチ仮想ピンホールカメラモデルに基づいて、現在の広視野フレームを、それぞれ各仮想ピンホールカメラに対応するサブ視野フレームに分解し、基準広視野フレームにも同様な操作を実行する。それにより、各仮想ピンホールカメラに、それに対応する２つのサブ視野フレームを取得する。それぞれ異なる仮想ピンホールカメラに対応するサブ視野フレームペアのうち、マッチングする特徴点ペアの数が最も多いサブ視野フレームペアを選択する。該サブ視野フレームペア中の２つのサブ視野フレームにフレーム間マッチングを行うことで、それらの相対位置姿勢を取得する。具体的なサブ視野フレームのフレーム間マッチングプロセスは初期化ステップ８１０におけるフレーム間マッチング処理と同じであるため、簡潔にするために、ここでは重複説明を省略する。

各仮想ピンホールカメラのカメラ中心が広視野カメラのカメラ中心と重なるため、マルチ仮想ピンホールカメラモデル中の各仮想ピンホールカメラと広視野カメラとの間に一定の回転角度が存在する。各仮想ピンホールカメラの回転角度は１つの確定した回転行列に対応する。それにより、広視野フレームの位置姿勢行列は対応する回転行列でサブ視野フレームの位置姿勢行列に変換することができる。一方、サブ視野フレームの位置姿勢行列も対応する回転行列で広視野フレームの位置姿勢行列に変換できる。

上記技術案はマルチ仮想ピンホールカメラモデルによって、複雑な広視野カメラ投影モデルに基づく位置姿勢取得を、シンプルな仮想ピンホールカメラ投影モデルに基づく位置姿勢取得に変換することで、広視野ＳＬＡＭのアルゴリズムを大幅に簡略化し、性能を大幅に向上させる。
追跡サブステップ３では、上記追跡サブステップ２で取得された現在の広視野フレームの位置姿勢を更新する。

現在の広視野フレームと基準広視野フレームとのマッチングする特徴点ペアによって、基準広視野フレーム中のマッチングする各特徴点に対して、マルチ仮想ピンホールカメラモデルに基づいて、該特徴点に関連付けられたマップ点を、現在の広視野フレームの、対応する仮想ピンホールカメラの座標系に変換する。次に、該マップ点を該仮想ピンホールカメラのイメージング平面に投影して、該マップ点の現在の広視野フレームにおける再投影点を取得する。

一例では、現在の広視野フレームと基準広視野フレームとの間に大きな視差がある。図４に示される５つの向きのマルチ仮想ピンホールカメラモデルに基づいて処理を行う。基準広視野フレーム中の特定のマッチングする特徴点が左向きの仮想ピンホールカメラのイメージング平面上にある。該特徴点に関連付けられたマップ点をマルチ仮想ピンホールカメラモデルに基づいて変換した後、現在の広視野フレームの前向きの仮想ピンホールカメラの座標系に対応付ける。現在の広視野フレームの前向きの仮想ピンホールカメラのイメージング平面上で該マップ点の再投影点を取得する。なお、基準広視野フレームの位置姿勢でマルチ仮想ピンホールカメラモデル中の左向きの仮想ピンホールカメラによって該マップ点を観察でき、現在の広視野フレームの位置姿勢でマルチ仮想ピンホールカメラモデル中の前向きの仮想ピンホールカメラによって該マップ点を観察できる。

該再投影点と現在の広視野フレーム中のマッチングする特徴点に基づいて、該マップ点の再投影誤差を決定する。基準広視野フレーム中のすべてのマッチングする特徴点に関連付けられたマップ点の再投影誤差に基づいて、現在の広視野フレームの位置姿勢を更新する。本ステップにおける再投影誤差計算及び再投影誤差に基づいて現在の広視野フレームの位置姿勢を更新する処理はステップ８２０におけるグローバルバンドル最適化の処理方法と同じであるため、簡潔にするために、ここでは重複説明を省略する。

現在の広視野フレームの位置姿勢をさらに最適化して更新することで、現在の広視野フレームの位置姿勢の信頼度を向上させ、追跡誤差を減少させる。それによりＳＬＡＭ追跡がさらに正確になり、マップ作成がさらに効率的になる。

双眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２は現在の双眼画像フレームに関連付けられた各マップ点を第１マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点を取得でき、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記現在の双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて左再投影誤差を決定することができる。

或いは、自己位置推定と環境マップ作成を同時に行う装置１０２は前記マップ点を第２マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点を取得でき、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記現在の双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて右再投影誤差を決定することができる。

さらに、自己位置推定と環境マップ作成を同時に行う装置１０２は左再投影誤差、右再投影誤差又はそれらの和に基づいて、現在の双眼画像フレームの位置姿勢を更新することができる。例えば、単眼マップ点の場合、自己位置推定と環境マップ作成を同時に行う装置１０２は左再投影誤差又は右再投影誤差に基づいて、現在の双眼画像フレームの位置姿勢を更新でき、双眼マップ点の場合、自己位置推定と環境マップ作成を同時に行う装置１０２は左再投影誤差と右再投影誤差との和に基づいて、現在の双眼画像フレームの位置姿勢を更新することができる。

具体的には、自己位置推定と環境マップ作成を同時に行う装置１０２は左再投影誤差、右再投影誤差、又は左再投影誤差と右再投影誤差との和を求めて広視野カメラ１０１の位置姿勢増分を決定し、次に、事前情報に合わせて、広視野カメラ１０１の現在の位置姿勢を決定することができる。

いくつかの実施例では、上記事前情報は前1つのフレーム広視野カメラ１０１の位置姿勢、又は1つ前のフレーム広視野カメラ１０１の位置姿勢と1つ前のフレーム位置姿勢増分との和であってもよい。前記1つ前のフレーム位置姿勢増分は２つ前のフレーム広視野カメラ１０１の位置姿勢に対する1つ前のフレーム広視野カメラ１０１の位置姿勢の位置姿勢増分である。

いくつかの実施例では、自己位置推定と環境マップ作成を同時に行う装置１０２は以下の複数の式によって左投影誤差及び／又は右投影誤差を計算し、位置姿勢増分を求める。式（７）は以下の通りである。

式中、Ｐ２はマルチ仮想ピンホールカメラモデルの座標系におけるマップ点Ｐの投影点、Ｐ１は前記マルチ仮想ピンホールカメラモデルの１つの面の座標系における点Ｐ２の投影点を示す。
従って、チェーンルールによってｕからカメラ位置姿勢までのヤコビ行列を導出できる。式（９）に示される。

広視野カメラ１０１に対して、自己位置推定と環境マップ作成を同時に行う装置１０２は式（７）、（８）、（９）及び（１０）に基づいて広視野カメラ１０１の左再投影誤差を決定し、広視野カメラ１０１の位置姿勢を決定することができる。

なお、同様な原理に基づいて、自己位置推定と環境マップ作成を同時に行う装置１０２は広視野カメラ１０１の右再投影誤差を決定し、次に、前記右再投影誤差又は左再投影誤差と右再投影誤差との和に基づいて、広視野カメラ１０１の位置姿勢を決定することができる。

８４０では、自己位置推定と環境マップ作成を同時に行う装置１０２はマップ作成ステップ（又はマップ更新ステップと呼ばれる）を実行することができる。マップ作成ステップでは、現在のマップをもとに、広視野カメラの動きに伴ってマップを拡張する。換言すれば、マップ作成ステップでは、新たなマップ点を現在のマップに挿入する。好ましくは、マップ作成ステップ８４０は追跡ステップ８３０の後に行われ、現在の広視野フレーム（又は現在の双眼画像フレーム）に対して、追跡ステップ８３０でその位置姿勢を決定し、すなわち、現在の時点における広視野カメラの動きの位置姿勢を決定することができる。

単眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２は現在の広視野フレーム及びその基準フレームの互いにマッチングする特徴点を決定し、前記現在の広視野フレームの特徴点及び現在の前記広視野カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定でき、前記基準フレームのマッチングする特徴点及び前記基準フレームに対応する前記広視野カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定し、前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定し、前記マップ点に基づいてマップを作成する。
いくつかの実施例では、自己位置推定と環境マップ作成を同時に行う装置１０２は、３つのサブステップを実行してマップ作成ステップを行うことができる。
マップ作成サブステップ１では、現在の広視野フレームがキー広視野フレームであるか否かを決定する。

広視野カメラが連続的に動いてデータを収集するため、取得された各広視野フレームにマップ更新操作を行うことは膨大な計算量をもたらす。従って、いくつかの重要な広視野フレームをキー広視野フレームとして選択し、さらにキー広視野フレームに基づいてマップ更新操作を行うようにしてもよい。キー広視野フレームの決定は任意の公知技術の又は将来開発される技術を採用できる。例えば、初期キー広視野フレームに基づいて、１０個の広視野フレームごとに１個をキー広視野フレームとして選択する。すなわち、１１番目、２１番目、３１番目…をキー広視野フレームとして選択する。また、例えば、１つ前のキー広視野フレームとは適切な視差を有する広視野フレームをキー広視野フレームとして選択する。

現在の広視野フレームがキー広視野フレームである場合、マップ更新サブステップ２に進み、現在の広視野フレームに基づいてマップ更新処理を行う。現在の広視野フレームがキー広視野フレームではない場合、マップ更新サブステップ３に進み、現在の広視野フレームにマップ点関連付け処理を行う。
マップ作成サブステップ２では、現在の広視野フレームがキー広視野フレームである場合、現在の広視野フレームに基づいてマップ更新処理を行う。

ローカルマップ中の各キー広視野フレームに対して、マルチ仮想ピンホールカメラモデルに基づいて、該キー広視野フレームを、それぞれ各仮想ピンホールカメラに対応するサブ視野フレームに分解し、現在の広視野フレームにも同様な操作を実行する。それにより各仮想ピンホールカメラに、それに対応する２つのサブ視野フレームを取得し、該２つのサブ視野フレームにフレーム間マッチングを行うことで新たなマップ点を作成する。

好ましくは、該２つのサブ視野フレームにフレーム間マッチングを行う過程では、単語の袋モデルに基づくベクトルを用いて特徴点間のマッチングを速める。単語の袋モデルによってマッチングする特徴点ペアに対して、さらにエピポーラ拘束を満たすか否かを検出する。エピポーラ拘束を満たす特徴点ペアに対して、該特徴点ペアに基づく三角測量によって新たなマップ点の三次元座標点を取得する。

ここで、サブ視野フレームのフレーム間マッチング処理及び特徴点ペアに基づく三角測量によって新たなマップ点の三次元座標点を取得するプロセスは、初期化ステップ８１０中の対応する処理と同じであるため、簡潔にするために、ここでは重複説明を省略する。

新たなマップ点を作成した後、現在の広視野フレームの位置姿勢に基づいて該新たなマップ点を世界座標系中のマップ点に変換して現在のマップに挿入し、現在の広視野フレームを現在のマップに挿入する。一般には、初期化においてマップを作成するための１番目のキー広視野フレームの座標系を世界座標系とする。その後、新たなマップ点に対して、カメラ座標系と世界座標系との変換を行う必要がある。
当業者であれば、現在のマップは新たなマップ点及び新たなキー広視野フレームを絶えず挿入されることで徐々に拡張し完備になることを理解できる。

好ましくは、現在の広視野フレームに基づいて新たな単語の袋モデルに基づくベクトルを作成し、前記新たな単語の袋モデルに基づくベクトルを上記マップデータベースに追加する。マップデータベースに基づいて、単語の袋モデルに基づくベクトルによって特徴点間のマッチングを速め、それによりＳＬＡＭ追跡及びマップ作成の効率を向上させる。
マップ作成サブステップ３では、現在の広視野フレームがキー広視野フレームではない場合、現在の広視野フレームにマップ点関連付け処理を行う。

ローカルマップ中の各マップ点に対して、現在の広視野フレームの位置姿勢によって、マルチ仮想ピンホールカメラモデルに基づいて、該マップ点を、現在の広視野フレームの、対応する仮想ピンホールカメラの座標系に変換する。さらに該マップ点を該仮想ピンホールカメラのイメージング平面に投影して、該マップ点の現在の広視野フレームにおける再投影点を取得する。投影が失敗する場合、現在の広視野フレームの位置姿勢で該マップ点を観測できない。投影が成功する場合、現在の広視野フレームの位置姿勢で該マップ点を観測でき、該マップ点の再投影点を取得する。現在の広視野フレームのすべての特徴点のうち、該再投影点付近の特徴点から該マップ点と最もマッチングする特徴点を選択して該マップ点に関連付ける。なお、このステップによって、現在の広視野フレームと該現在の広視野フレームの位置姿勢で観測できるマップ点とが関連付けられる。このように、次の広視野フレームの処理時、該現在の広視野フレームは次の広視野フレームの１つ前の広視野フレームとして追跡処理に用いられる。それによりＳＬＡＭ追跡がさらにスムーズになり、位置推定精度がさらに高くなり、作成されたマップがさらに正確になる。

双眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２は上記単眼広視野カメラのマップ作成ステップを実行してもよく、同一時点における左歪み補正画像と右歪み補正画像の互いにマッチングする特徴点に基づいてマップを作成してもよい。

後者の場合、自己位置推定と環境マップ作成を同時に行う装置１０２は現在の左歪み補正画像と現在の右歪み補正画像の互いにマッチングする特徴点を決定することができ、前記現在の左歪み補正画像の特徴点及び現在の前記双眼広視野カメラの左側カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定し、前記現在の右歪み補正画像の特徴点及び現在の前記双眼広視野カメラの右側カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定し、前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定し、前記マップ点に基づいてマップを作成する。

いくつかの実施例では、双眼広視野カメラの場合、自己位置推定と環境マップ作成を同時に行う装置１０２は初期化ステップ８１０における関連説明を参照して上記第１特徴点に対応する方向ベクトル及び第２特徴点に対応する方向ベクトルを決定し、三角測量を行うことができる。

現在の広視野フレーム（又は現在の双眼画像フレーム）がキー広視野フレーム（又はキー双眼画像フレーム）である場合、マップ作成ステップ８４０はローカルバンドル最適化をさらに含んでもよい。ローカルバンドル最適化は、ローカルマップ中のキー広視野フレーム（又はキー双眼画像フレーム）の位置姿勢及びマップ点の位置を微調整し、ローカルマップ中のマップ点のキー広視野フレーム（又はキー双眼画像フレーム）における再投影誤差を最小化し、それにより作成されたマップを最適化することを目的とする。
単眼広視野カメラの場合、ローカルマップ中の各キー広視野フレームに対するバンドル最適化処理は以下の通りである。

該キー広視野フレームに関連付けられた各マップ点に対して、前記マルチ仮想ピンホールカメラモデルに基づいて、該マップ点を対応する仮想ピンホールカメラの座標系に変換し、さらに該仮想ピンホールカメラのイメージング平面に投影して、該マップ点の再投影点を取得する。且つ該マップ点に関連付けられた特徴点及び該マップ点の再投影点に基づいて該マップ点の再投影誤差を決定する。該キー広視野フレームに関連付けられたすべてのマップ点の再投影誤差に基づいて、該キー広視野フレームの位置姿勢及び該キー広視野フレームに関連付けられたすべてのマップ点の位置を更新する。本ステップのバンドル最適化処理プロセスは上記グローバルバンドル最適化ステップ８２０の処理プロセスと同じであるため、簡潔にするために、ここでは重複説明を省略する。
双眼広視野カメラの場合、ローカルマップ中の各キー双眼画像フレームに対するバンドル最適化処理は以下の通りである。

前記キー双眼画像フレームに関連付けられたマップ点を第１マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて左再投影誤差を決定する。

或いは、前記キー双眼画像フレームに関連付けられたマップ点を第２マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて右再投影誤差を決定する。

さらに、前記左再投影誤差、前記右再投影誤差又は前記左再投影誤差と前記右再投影誤差との和に基づいて、前記キー双眼画像フレームの位置姿勢及び前記キー双眼画像フレームに関連付けられたすべてのマップ点の位置を更新する。

８５０では、自己位置推定と環境マップ作成を同時に行う装置１０２は閉ループ検出処理ステップを実行することができる。単眼広視野カメラ及び双眼広視野カメラの閉ループ検出処理ステップは同じであってもよく、以下、単眼広視野カメラの閉ループ検出処理を例とする。

現在の広視野フレームがキー広視野フレームである場合、単語の袋モデルに基づくベクトルによって、現在のマップデータベース中の、現在の広視野フレームと類似する閉ループ広視野フレームを検出する。

該閉ループ広視野フレームと現在の広視野フレームのマッチングする特徴点ペアを決定する。好ましくは、単語の袋モデルに基づくベクトルを用いて特徴点間のマッチングを速める。

該閉ループ広視野フレームと現在の広視野フレームのマッチングする特徴点ペアに基づいて、相似変換演算子（Ｓｉｍ３Ｓｏｌｖｅｒ）及びＲＡＮＳＡＣアルゴリズムによって該閉ループ広視野フレームと現在の広視野フレームの間の相似変換行列を計算する。

現在の広視野フレーム中のマッチングする各特徴点に対して、マルチ仮想ピンホールカメラモデルに基づいて、該特徴点に関連付けられたマップ点を該閉ループ広視野フレームの、対応する仮想ピンホールカメラの座標系に変換する。さらに該マップ点を該仮想ピンホールカメラのイメージング平面に投影して、該マップ点の該閉ループ広視野フレームにおける再投影点を取得する。該再投影点及び該閉ループ広視野フレーム中のマッチングする特徴点に基づいて、第１再投影誤差を決定する。現在の広視野フレーム中のすべてのマッチングする特徴点の第１再投影誤差に基づいて第１累計再投影誤差を決定する。

該閉ループ広視野フレーム中のマッチングする各特徴点に対して、マルチ仮想ピンホールカメラモデルに基づいて、該特徴点に関連付けられたマップ点を現在の広視野フレームの、対応する仮想ピンホールカメラの座標系に変換する。さらに該仮想ピンホールカメラのイメージング平面に投影して、該マップ点の現在の広視野フレームにおける再投影点を取得する。該再投影点及び現在の広視野フレーム中のマッチングする特徴点に基づいて第２再投影誤差を決定する。該閉ループ広視野フレーム中のすべてのマッチングする特徴点の第２再投影誤差に基づいて第２累計再投影誤差を決定する。
前記第１累計再投影誤差及び前記第２累計再投影誤差に基づいて損失関数を決定する。損失を最小化することによって上記相似変換行列を最適化する。

閉ループプロセスで累計された誤差をなくすために、現在のマップ中の、現在の広視野フレームとは共通視野関係を有するキー広視野フレーム及びそれに関連付けられたマップ点を補正する必要がある。まず、現在の広視野フレームとは共通視野関係を有するキー広視野フレームを取得する。２つの広視野フレームで観察される共通マップ点の数が共通視野関係閾値より大きいと、これら２つの広視野フレームは共通視野関係を有する。共通視野関係閾値は、２つのキー広視野フレームが共通視野関係を有することを判断することに必要な最小限の共通マップ点の数を示し、直接デフォルト値、例えば２０を使用してもよく、ユーザーによって予め設定されてもよい。次に、上記相似変換行列によってこれらのキー広視野フレームの位置姿勢及びこれらのキー広視野フレームに関連付けられたマップ点の位置を補正する。ここまで、閉ループ検出処理が完了する。

広視野カメラの動きに伴って、追跡によって計算される広視野カメラの位置姿勢及び三角測量によって取得されるマップ点の位置はともに誤差が生じる。ローカルバンドル最適化又はグローバルバンドル最適化によって最適化を行うとしても、累積誤差が生じる。上記閉ループ検出処理によって、累計誤差を効果的になくし、それによりＳＬＡＭで作成されたマップがさらに正確になる。

好ましくは、閉ループ検出処理はポーズグラフ最適化（ｐｏｓｅ－ｇｒａｐｈｏｐｔｉｍｉｚａｔｉｏｎ）によって現在のマップ中のすべてのキー広視野フレームの位置姿勢及びすべてのマップ点の位置をさらに最適化するステップをさらに含む。好ましくは、閉ループ検出処理は冗長なキーフレーム及びマップ点を検索して削除するステップをさらに含み、それによりシステムの保存領域を節約するとともに、冗長な計算操作を回避する。

上記実施例のステップ８１０～８５０はマルチ仮想ピンホールカメラモデルに基づく広視野ＳＬＡＭのステップ２３０の実施形態を提供した。なお、ステップ２２０で取得された歪み補正画像に基づいて、任意の公知の又は将来開発される広視野ＳＬＡＭ方法を採用できる。例えば、上記マルチ仮想ピンホールカメラモデルに基づいて再投影誤差計算を行う最適化更新処理のかわりに、単位方向ベクトル誤差計算に基づいて最適化更新処理を行うようにしてもよい。前記単位方向ベクトルに基づく誤差計算は、マップ点に対応する単位方向ベクトルと該マップ点に関連付けられた特徴点に対応する単位方向ベクトルとの差を最小化することで、最終的な最適化目標を達成する。前記最適化される目標損失は単位方向ベクトル間の距離、又は単位ベクトル間の夾角、又はベクトル誤差を記述するほかの指標であってもよい。

最後に、このアプリケーションで言及されている「左」と「右」、たとえば「左目」、「右目」、「左魚眼画像」、「右魚眼画像」、「左歪み補正画像」、「右のゆがんだ画像」、「左の再投影エラー」、「右の再投影エラー」は説明のみを目的としており、このアプリケーションの保護範囲を制限するものではありません。

要約すると、この詳細な開示を読んだ後、当業者は、前述の詳細な開示が例としてのみ提示されてもよく、限定的ではないことを理解することができる。ここでは明示的に述べていないが、当業者は、本発明が実施形態に対する様々な合理的な変更、改善、および修正を網羅することを意図していることを理解することができる。これらの変更、改善、および修正は、本開示によって示唆されることを意図しており、本開示の例示的な実施形態の精神および範囲内にある。

さらに、本発明の特定の用語は、本開示の実施形態を説明するために使用されてきた。例えば、「一実施形態」、「実施形態」、および/または「いくつかの実施形態」は、実施形態に関連して説明される特定の特徴、構造、または特性を本開示の少なくとも1つの実施形態に含めることができることを意味する。したがって、本明細書の様々な部分における「実施形態」または「一実施形態」または「代替実施形態」への2つ以上の言及は、必ずしもすべて同じ実施形態を指すとは限らないことを強調および理解できる。さらに、特定の特徴、構造、または特性は、本開示の1つまたは複数の実施形態において適切に組み合わされてもよい。

本開示の実施形態の前述の説明において、特徴の理解を助けるために、本開示を単純化する目的で、本発明は、単一の実施形態、図面、またはその説明において様々な特徴を時々組み合わせることが理解されるべきである。あるいは、本発明は、本発明の複数の実施形態において様々な特徴を分散させる。しかし、これは、これらの特徴の組み合わせが必要であることを言うものではなく、当業者は、本発明を読むときにこれらの特徴のいくつかを別個の実施形態として抽出する可能性が高い。すなわち、本発明の実施形態は、複数の二次実施形態の統合として理解することもできる。各二次実施形態の内容が、以前に開示された単一の実施形態のすべての特徴よりも少ないことも事実である。

いくつかの実施形態では、本発明の特定の実施形態を説明および請求するために使用される量または特性を表す数字は、用語「約」、「およそ」または「実質的に」によって変更される場合がある。たとえば、特に明記しない限り、「約」、「およそ」、または「実質的に」とは、説明する値が±20％変化することを意味する場合があります。したがって、いくつかの実施形態では、記載された説明および添付の特許請求の範囲に記載された数値パラメータは、特定の実施形態によって得られることが求められる所望の特性に応じて変わり得る近似値である。一部の実施形態では、数値パラメータは、報告された有効桁数に基づいて、一般的な丸め手法を適用することにより解釈されるべきである。本発明に記載されているいくつかの実施形態は、広範囲の数値範囲およびパラメータを概算しているが、特定の例は、可能な限り正確な数値を列挙している。

各特許、特許出願、特許出願の公開、および記事、書籍、説明書、出版物、文書、記事など、本明細書で引用された他の資料は、参照により本明細書に組み込まれ得る。関連する検察文書の履歴、この文書と矛盾または矛盾する可能性のあるもの、またはクレームの最も広い範囲に制限的な影響を与える可能性のある同一の検察文書を除く、すべての目的の内容全体歴史。現在または今後、このドキュメントに関連付けられます。たとえば、このドキュメントに含まれる資料のいずれかに関連する説明、定義、および/または用語の使用に矛盾または矛盾がある場合、用語が優先されます。

最後に、本明細書に開示された本発明の実施形態は、本発明の実施形態の原理の例示であることを理解されたい。他の修正された実施形態も本発明の範囲内である。したがって、本発明で開示される実施形態は単なる例であり、限定するものではない。当業者は、本発明の実施形態に従って本発明で本発明を実施するための代替構成をとることができる。したがって、本発明の実施形態は、どの実施形態が本発明で正確に説明されるかに限定されない。

Claims

自己位置推定と環境マップ作成を同時に行う方法であって、
広視野カメラによって広視野画像を取得する広視野画像取得ステップと、
マルチ仮想ピンホールカメラモデルに基づいて、前記広視野画像に対応する歪み補正画像を取得する補正画像取得ステップと、
前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成するマップ作成ステップと、を含み、
前記マルチ仮想ピンホールカメラモデルは、５つの異なる向きの仮想ピンホールカメラを含み、且つ前記５つの異なる向きの仮想ピンホールカメラのカメラ中心は前記広視野カメラのカメラ中心と重なり、
前記５つの仮想ピンホールカメラの向きは、前向き、上向き、下向き、左向き及び右向きであって、前記５つの仮想ピンホールカメラのイメージング平面であるサブ視野フレームは、キューブの５つの面の位置関係にあり、
前記広視野画像取得ステップは、第１時点に対応する現在の広視野フレームと、第２時点に対応する基準広視野フレームをそれぞれ取得し、
前記補正画像取得ステップは、前記基準広視野フレームを前記５つのイメージング平面に投影して５つのサブ視野フレームに分解することにより歪み補正画像を取得し、前記現在の広視野フレームを前記５つのイメージング平面に投影して５つのサブ視野フレームに分解することにより歪み補正画像を取得し、
前記マップ作成ステップは、前記仮想ピンホールカメラの向きが対応する前記基準広視野フレームの５つのサブ視野フレームと前記現在の広視野フレームの５つのサブ視野フレームをペアにして、フレーム間マッチングを行って特徴点を抽出し、マッチングする特徴点が最も多いサブ視野フレームペアを選択し、選択したサブ視野フレームペア間の相対位置姿勢を取得し、取得した相対位置姿勢を前記広視野カメラの位置姿勢とする
ことを特徴とする自己位置推定と環境マップ作成を同時に行う方法。
前記広視野カメラは単眼広視野カメラであり、前記マップ作成ステップは、
前記マッチングする特徴点に基づいて初期マップを作成する初期マップ作成ステップを含むことを特徴とする請求項１に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記初期マップ作成ステップは、
前記第１時点に対応する歪み補正画像中の特徴点及び前記第１時点における前記広視野カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定するステップと、
前記第２時点に対応する歪み補正画像中のマッチングする特徴点及び前記第２時点における前記広視野カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定するステップと、
前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定するステップと、
前記マップ点に基づいて初期マップを作成するステップと、を含むことを特徴とする請求項２に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記広視野カメラは単眼広視野カメラであり、前記マップ作成ステップは、グローバルバンドル最適化ステップを含み、前記グローバルバンドル最適化ステップは、
前記マップ中の各キー広視野フレームに対して、
前記キー広視野フレームに関連付けられた各マップ点をマルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー広視野フレームに関連付けられたマップ点の再投影誤差に基づいて再投影誤差を決定するステップと、
前記再投影誤差に基づいて、前記キー広視野フレームの位置姿勢及び前記キー広視野フレームに関連付けられたすべてのマップ点の位置を更新するステップと、を含むことを特徴とする請求項１に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記広視野カメラは単眼広視野カメラであり、前記マップ作成ステップは、追跡ステップを含み、前記追跡ステップは、
前記現在の広視野フレームに関連付けられた各マップ点に対して、
前記マップ点をマルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記現在の広視野フレームに関連付けられたマップ点の再投影誤差に基づいて再投影誤差を決定するステップと、
前記再投影誤差に基づいて、前記現在の広視野フレームの位置姿勢を更新するステップと、を含むことを特徴とする請求項１に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記広視野カメラは単眼広視野カメラであり、前記マップ作成ステップは、
前記現在の広視野フレーム及び前記基準広視野フレームの互いにマッチングする特徴点を決定するステップと、
前記現在の広視野フレームの特徴点及び現在の前記広視野カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定するステップと、
前記基準広視野フレームのマッチングする特徴点及び前記基準広視野フレームに対応する前記広視野カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定するステップと、
前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定するステップと、
前記マップ点に基づいてマップを作成するステップと、を含むことを特徴とする請求項１に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記マップ作成ステップはローカルバンドル最適化ステップをさらに含み、前記ローカルバンドル最適化ステップは、
ローカルマップ中の各キー広視野フレームに対して、
前記キー広視野フレームに関連付けられた各マップ点をマルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー広視野フレームに関連付けられたマップ点の再投影誤差に基づいて再投影誤差を決定するステップと、
前記再投影誤差に基づいて、前記キー広視野フレームの位置姿勢及び該キー広視野フレームに関連付けられたすべてのマップ点の位置を更新するステップと、を含むことを特徴とする請求項６に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記広視野カメラは双眼広視野カメラであり、
前記広視野画像取得ステップは、前記双眼広視野カメラによって左視野画像及び右視野画像を取得し、
前記補正画像取得ステップは、第１マルチ仮想ピンホールカメラモデルに基づいて、前記左視野画像に対応する左歪み補正画像を取得するステップと、第２マルチ仮想ピンホールカメラモデルに基づいて、前記右視野画像に対応する右歪み補正画像を取得するステップとを含み、
前記マップ作成ステップは、前記左歪み補正画像及び前記右歪み補正画像に基づいて、前記双眼広視野カメラの位置姿勢を決定し、マップを作成し、
前記第１マルチ仮想ピンホールカメラモデルは５つの異なる向きの仮想ピンホールカメラを含み、且つ前記５つの異なる向きの仮想ピンホールカメラのカメラ中心は前記双眼広視野カメラの左側カメラのカメラ中心と重なり、
前記第２マルチ仮想ピンホールカメラモデルは５つの異なる向きの仮想ピンホールカメラを含み、且つ前記５つの異なる向きの仮想ピンホールカメラのカメラ中心は前記双眼広視野カメラの右側カメラのカメラ中心と重なることを特徴とする請求項１に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記マップ作成ステップは、初期化ステップを含み、前記初期化ステップは、
前記左歪み補正画像と前記右歪み補正画像の互いにマッチングする特徴点を決定する特徴点決定ステップと、
前記互いにマッチングする特徴点に基づいて初期マップを作成する初期マップ作成ステップと、を含むことを特徴とする請求項８に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記特徴点決定ステップは、
前記右歪み補正画像における前記左歪み補正画像中の特徴点に対応する極線を決定するステップと、
前記極線上で前記左歪み補正画像中の特徴点にマッチングする特徴点を検索するステップと、を含み、
前記極線は多線分折れ線であることを特徴とする請求項９に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記初期マップ作成ステップは、
前記左歪み補正画像中の特徴点及び前記双眼広視野カメラの左側カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定するステップと、
前記右歪み補正画像中のマッチングする特徴点及び前記双眼広視野カメラの右側カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定するステップと、
前記双眼広視野カメラの基線に基づいて、前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定するステップと、
前記マップ点に基づいて初期マップを作成するステップと、を含むことを特徴とする請求項９に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記マップ作成ステップは、グローバルバンドル最適化ステップを含み、前記グローバルバンドル最適化ステップは、
前記マップ中の各キー双眼画像フレームに対して、
前記キー双眼画像フレームに関連付けられたマップ点を第１マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて左再投影誤差を決定するステップ、又は
前記キー双眼画像フレームに関連付けられたマップ点を第２マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて右再投影誤差を決定するステップと、
前記左再投影誤差、前記右再投影誤差又は前記左再投影誤差と前記右再投影誤差との和に基づいて、前記キー双眼画像フレームの位置姿勢及び前記キー双眼画像フレームに関連付けられたすべてのマップ点の位置を更新するステップと、を含むことを特徴とする請求項８に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記マップ作成ステップは、追跡ステップを含み、前記追跡ステップは、
現在の双眼画像フレームに関連付けられた各マップ点に対して、
前記マップ点を第１マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点を取得し前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記現在の双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて左再投影誤差を決定するステップ、又は
前記マップ点を第２マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記現在の双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて右再投影誤差を決定するステップと、
前記左再投影誤差、前記右再投影誤差又は前記左再投影誤差と前記右再投影誤差との和に基づいて、前記現在の双眼画像フレームの位置姿勢を更新するステップと、を含むことを特徴とする請求項８に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記マップ作成ステップは、
現在の左歪み補正画像と現在の右歪み補正画像の互いにマッチングする特徴点を決定するステップと、
前記現在の左歪み補正画像の特徴点及び現在の前記双眼広視野カメラの左側カメラのカメラ中心に基づいて、第１特徴点に対応する方向ベクトルを決定するステップと、
前記現在の右歪み補正画像の特徴点及び現在の前記双眼広視野カメラの右側カメラのカメラ中心に基づいて、第２特徴点に対応する方向ベクトルを決定するステップと、
前記第１特徴点に対応する方向ベクトル及び前記第２特徴点に対応する方向ベクトルに三角測量を行い、前記特徴点に対応するマップ点を決定するステップと、
前記マップ点に基づいてマップを作成するステップと、を含むことを特徴とする請求項８に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記マップ作成ステップはローカルバンドル最適化ステップをさらに含み、前記ローカルバンドル最適化ステップは、
ローカルマップ中の各キー双眼画像フレームに対して、
前記キー双眼画像フレームに関連付けられたマップ点を第１マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第１マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて左再投影誤差を決定するステップ、又は
前記キー双眼画像フレームに関連付けられたマップ点を第２マルチ仮想ピンホールカメラモデルに投影し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点を取得し、前記マップ点の前記第２マルチ仮想ピンホールカメラモデルにおける再投影点及び前記マップ点に対応する特徴点に基づいて、前記マップ点の再投影誤差を決定し、すべての前記キー双眼画像フレームに関連付けられたマップ点の再投影誤差に基づいて右再投影誤差を決定するステップと、
前記左再投影誤差、前記右再投影誤差又は前記左再投影誤差と前記右再投影誤差との和に基づいて、前記キー双眼画像フレームの位置姿勢及び前記キー双眼画像フレームに関連付けられたすべてのマップ点の位置を更新するステップと、を含むことを特徴とする請求項１４に記載の自己位置推定と環境マップ作成を同時に行う方法。
前記マップ作成ステップは、閉ループ検出処理ステップを含み、前記閉ループ検出処理ステップは、
現在の広視野フレームがキー広視野フレームである場合、マップデータベース中の前記現在の広視野フレームと類似する閉ループ広視野フレームを決定するステップと、
前記現在の広視野フレームと前記閉ループ広視野フレームとの互いにマッチングする特徴点を決定するステップと、
前記現在の広視野フレーム中のマッチングする各特徴点に対し、該特徴点に関連付けられたマップ点を、前記閉ループ広視野フレームに対応するマルチ仮想ピンホールカメラモデルの座標系に変換し、さらに前記マルチ仮想ピンホールカメラモデルのイメージング平面に投影し、該マップ点の前記閉ループ広視野フレームにおける再投影点を取得し、該再投影点及び前記閉ループ広視野フレーム中のマッチングする特徴点に基づいて第１再投影誤差を決定するステップと、
前記現在の広視野フレーム中のすべてのマッチングする特徴点の第１再投影誤差に基づいて第１累計再投影誤差を決定するステップと、
前記閉ループ広視野フレーム中のマッチングする各特徴点に対して、該特徴点に関連付けられたマップ点を、前記現在の広視野フレームに対応するマルチ仮想ピンホールカメラモデルの座標系に変換し、さらに前記マルチ仮想ピンホールカメラモデルのイメージング平面に投影し、該マップ点の前記現在の広視野フレームにおける再投影点を取得し、該再投影点及び前記現在の広視野フレーム中のマッチングする特徴点に基づいて第２再投影誤差を決定するステップと、
前記閉ループ広視野フレーム中のすべてのマッチングする特徴点の第２再投影誤差に基づいて第２累計再投影誤差を決定するステップと、
前記第１累計再投影誤差及び前記第２累計再投影誤差を利用して、マップ中の前記現在の広視野フレームとは共通視野関係を有するキー広視野フレーム及びそれに関連付けられたマップ点を補正するステップと、を含むことを特徴とする請求項１に記載の自己位置推定と環境マップ作成を同時に行う方法。
自己位置推定と環境マップ作成を同時に行う装置であって、
一組命令を記憶する少なくとも１つの記憶デバイスと、
前記少なくとも１つの記憶デバイスと通信する少なくとも１つのプロセッサと、を備え、前記一組命令を実行する時、前記少なくとも１つのプロセッサは、前記自己位置推定と環境マップ作成を同時に行う装置に、
広視野カメラによって広視野画像を取得する広視野画像取得ステップと、
マルチ仮想ピンホールカメラモデルに基づいて、前記広視野画像に対応する歪み補正画像を取得する補正画像取得ステップと、
前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成するマップ作成ステップと、を実行させ、
前記マルチ仮想ピンホールカメラモデルは、５つの異なる向きの仮想ピンホールカメラを含み、且つ前記５つの異なる向きの仮想ピンホールカメラのカメラ中心は前記広視野カメラのカメラ中心と重なり、
前記５つの仮想ピンホールカメラの向きは、前向き、上向き、下向き、左向き及び右向きであって、前記５つの仮想ピンホールカメラのイメージング平面であるサブ視野フレームは、キューブの５つの面の位置関係にあり、
前記広視野画像取得ステップは、第１時点に対応する現在の広視野フレームと、第２時点に対応する基準広視野フレームをそれぞれ取得し、
前記補正画像取得ステップは、前記基準広視野フレームを前記５つのイメージング平面に投影して５つのサブ視野フレームに分解することにより歪補正画像を取得し、前記現在の広視野フレームを前記５つのイメージング平面に投影して５つのサブ視野フレームに分解することにより歪み補正画像を取得し、
前記マップ作成ステップは、前記仮想ピンホールカメラの向きが対応する前記基準広視野フレームの５つのサブ視野フレームと前記現在の広視野フレームの５つのサブ視野フレームをペアにして、フレーム間マッチングを行って特徴点を抽出し、マッチングする特徴点が最も多いサブ視野フレームペアを選択し、選択したサブ視野フレームペア間の相対位置姿勢を取得し、取得した相対位置姿勢を前記広視野カメラの位置姿勢とする
ことを特徴とする自己位置推定と環境マップ作成を同時に行う装置。
前記広視野カメラは単眼広視野カメラであり、前記マップ作成ステップは、前記マッチングする特徴点に基づいて初期マップを作成する初期マップ作成ステップを含むことを特徴とする請求項１７に記載の自己位置推定と環境マップ作成を同時に行う装置。
コンピュータに、
広視野カメラによって広視野画像を取得する広視野画像取得ステップと、
マルチ仮想ピンホールカメラモデルに基づいて、前記広視野画像に対応する歪み補正画像を取得する補正画像取得ステップと、
前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成するマップ作成ステップと、を実行させるプログラムであって、
前記マルチ仮想ピンホールカメラモデルは、５つの異なる向きの仮想ピンホールカメラを含み、且つ前記５つの異なる向きの仮想ピンホールカメラのカメラ中心は前記広視野カメラのカメラ中心と重なり、
前記５つの仮想ピンホールカメラの向きは、前向き、上向き、下向き、左向き及び右向きであって、前記５つの仮想ピンホールカメラのイメージング平面であるサブ視野フレームは、キューブの５つの面の位置関係にあり、
前記広視野画像取得ステップは、第１時点に対応する現在の広視野フレームと、第２時点に対応する基準広視野フレームをそれぞれ取得し、
前記補正画像取得ステップは、前記基準広視野フレームを前記５つのイメージング平面に投影して５つのサブ視野フレームに分解することにより歪み補正画像を取得し、前記現在の広視野フレームを前記５つのイメージング平面に投影して５つのサブ視野フレームに分解することにより歪み補正画像を取得し、
前記マップ作成ステップは、前記仮想ピンホールカメラの向きが対応する前記基準広視野フレームの５つのサブ視野フレームと前記現在の広視野フレームの５つのサブ視野フレームをペアにして、フレーム間マッチングを行って特徴点を抽出し、マッチングする特徴点が最も多いサブ視野フレームペアを選択し、選択したサブ視野フレームペア間の相対位置姿勢を取得し、取得した相対位置姿勢を前記広視野カメラの位置姿勢とする
ことを特徴とするプログラム。
コンピュータに、
広視野カメラによって広視野画像を取得する広視野画像取得ステップと、
マルチ仮想ピンホールカメラモデルに基づいて、前記広視野画像に対応する歪み補正画像を取得する補正画像取得ステップと、
前記歪み補正画像に基づいて、前記広視野カメラの位置姿勢を決定し、マップを作成するマップ作成ステップと、を実行させるプログラムを記録したコンピュータ読み取り可能な記憶媒体であって、
前記マルチ仮想ピンホールカメラモデルは、５つの異なる向きの仮想ピンホールカメラを含み、且つ前記５つの異なる向きの仮想ピンホールカメラのカメラ中心は前記広視野カメラのカメラ中心と重なり、
前記５つの仮想ピンホールカメラの向きは、前向き、上向き、下向き、左向き及び右向きであって、前記５つの仮想ピンホールカメラのイメージング平面であるサブ視野フレームは、キューブの５つの面の位置関係にあり、
前記広視野画像取得ステップは、第１時点に対応する現在の広視野フレームと、第２時点に対応する基準広視野フレームをそれぞれ取得し、
前記補正画像取得ステップは、前記基準広視野フレームを前記５つのイメージング平面に投影して５つのサブ視野フレームに分解することにより歪み補正画像を取得し、前記現在の広視野フレームを前記５つのイメージング平面に投影して５つのサブ視野フレームに分解することにより歪み補正画像を取得し、
前記マップ作成ステップは、前記仮想ピンホールカメラの向きが対応する前記基準広視野フレームの５つのサブ視野フレームと前記現在の広視野フレームの５つのサブ視野フレームをペアにして、フレーム間マッチングを行って特徴点を抽出し、マッチングする特徴点が最も多いサブ視野フレームペアを選択し、選択したサブ視野フレームペア間の相対位置姿勢を取得し、取得した相対位置姿勢を前記広視野カメラの位置姿勢とする
ことを特徴とするプログラムを記録したコンピュータ読み取り可能な記憶媒体。