(57)【特許請求の範囲】
【請求項１】
ＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法であって、
二眼カメラの画像情報を読み取って前処理するステップＳ１と、
カメラ画像の特徴抽出と特徴マッチング、ＩＣＰアルゴリズムによる姿勢推定を含む、特徴点法で姿勢を推定する、フロントエンドタスクを実行するステップＳ２と、
グラフ最適化に基づくバックエンド最適化により、姿勢を最適化するバックエンド最適化ステップＳ３と、
バッグオブワードのループバック検出を行い、姿勢グラフを構築して最適化するステップＳ４と、を含み、
ステップＳ２では、カメラ画像の特徴抽出と特徴マッチングはＧＵＰで実行され、カメラ画像の特徴抽出と特徴マッチングの以降のステップはＯｐｅｎＣＶでＣＵＤＡ並列化を実現し、
前記前処理は、読み取ったカメラ画像をグレースケール化することであり、
前記ステップＳ２では、カメラ画像の特徴抽出と特徴マッチングは、具体的には、
処理済みのグレースケール図について画像ピラミッドを構築し、ピラミッド画像の各層に抽出すべき特徴点の数を割り当てるステップＳ２１と、
ピラミッド画像の各層の特徴を抽出し、つまり、まず、ピラミッド画像の各層に対して境界処理を実行し、ピラミッド画像においてグリッドを分割して、各グリッドに対して特徴点抽出を行い、具体的には、前記グリッドにおけるいずれかの画素を検出対象の画素とし、検出対象の画素を円心、ｒを半径として円を作成し、該円と交差する画素が合計ｎ個であり、前記ｎ個の画素のうち、連続するＮ個の画素と検出対象の画素のグレースケール値の差が予め設定された閾値Ｔよりも大きい場合、該検出対象の画素が特徴点であると見なされ、次に、各画素を繰り返して検出して、すべての特徴点を得るステップＳ２２と、
ステップＳ２２で抽出した特徴点について、除去及び分布均一化により最適化するステップＳ２３と、
ステップＳ２３で得られたいずれかの特徴点について、隣接する画素のマトリックスを、
【数１３】
0006886212.tif 000014
（ここで、Ｉ（ｘ,ｙ）は点ｘ、ｙでのグレースケール値を表す。）として定義し、
ピラミッド画像の重心として、
【数１４】
0006886212.tif 000015
特徴点の方向をθ＝ａｒｃｔａｎ（ｍ０１／ｍ１０）に定義するステップＳ２４と、
ｒＢＲＩＥＦアルゴリズムを利用して各特徴点の記述子を算出し、前記ピラミッド画像における現在フレームと参照フレームの特徴点のマッチングを実現するステップＳ２５と、を含む、ことを特徴とするＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法。
【請求項２】
前記ステップＳ２では、ＩＣＰアルゴリズム姿勢を推定することは、具体的には、
ＩＣＰアルゴリズムによって、非線形方法を利用してフレーム間の姿勢変換（Ｒ,Ｔ）を推定し、特徴点の深度情報に基づき最小二乗問題を構築するステップと、
【数１５】
0006886212.tif 000016
現在姿勢を最適化変数として、Ｂｕｎｄｌｅ  Ａｄｊｕｓｔｍｅｎｔをコアとするグラフ最適化方法で最適化し、反復方法によって現在姿勢を推定するステップと、を含む、ことを特徴とする請求項１に記載のＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法。
【請求項３】
前記ステップＳ３は、具体的には、
ＳＬＡＭプロセスの観測方程式において、その全体コスト関数を、
【数１６】
0006886212.tif 000017
（ここで、ｚｉｊは姿勢ξｉで道路標識ｐｊを観察して生じた観測データであり、ｅｉｊは観測値と測定値との誤差である。）とするステップと、
上記式の最小二乗の解を求め、姿勢及び道路標識を最適化変数として、Ｂｕｎｄｌｅ  Ａｄｊｕｓｔｍｅｎｔ非線形最適化方法を利用し、下降方向Δｘを継続的に見つけ、上記式の最適解を求め、すなわち、最適化変数に増分を与え、上記式に対して一次テイラー展開を行い、
【数１７】
0006886212.tif 000018
（ここで、Ｆｉｊは現在状態での全体コスト関数のカメラの姿勢への偏微分を表し、Ｅｉｊは道路標識点位置に対する全体コスト関数の偏微分を表す。）に単純化するステップと、
姿勢変数を合わせると、
【数１８】
0006886212.tif 000019
（ここで、ΔｘｃとΔｘｐはそれぞれすべての姿勢変数を統合した全体増分、空間点の変数を統合した全体増分であり、ヤコビ行列Ｆ及びＥは前記全体増分に対する全体コスト関数の導関数である。）に単純化するステップと、
展開して増分線形方程式ＨΔｘ＝ｇを得るステップと、
ＬＭ法で解を求めると、Ｈ＝ＪＴＪ＋λＩ、Ｊ＝［ＦＥ］、ｇ＝−Ｊ（ｘ）Ｔｆ（ｘ）
（ここで、Ｉは単位行列、λは正の実数である。）であるステップと、を含む、ことを特徴とする請求項２に記載のＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法。
【請求項４】
前記ステップＳ４は、具体的には、
バッグオブワードモデルを利用して画像間の類似性を算出し、ループバック発生の識別を加速し、ループバックフレームのスクリーニング閾値を動的算出することで、ループバックキーフレームを特定し、現在キーフレームとループバックキーフレームに対して計算してＳｉｍ３変換を行い、ループバック検出結果を得るステップＳ４１と、
ステップＳ４１のループバック検出結果に応じて作動軌跡を修正し、軌跡がループバック検出結果と一致する姿勢グラフを構築するステップＳ４２と、を含む、ことを特徴とする請求項３に記載のＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法。
【請求項５】
前記ステップＳ４１では、画像間の類似性を算出するステップは、
前記バッグオブワードモデルに基づき、ＴＦ−ＩＤＦアルゴリズムで算出するステップと、
辞書をトレーニングする時のすべての特徴数をｎとし、画像中の単語ｗｉの数をｎｉとすると、単語のＩＤＦが、
ＩＤＦｉ＝ｌｏｇ（ｎ／ｎｉ）になり、
画像中の単語ｗｉの出現回数をｎｉとして、出現する単語数の総数をｎとすると、ＴＦが、
ＴＦｉ＝ｎｉ／ｎになり、
単語ｗｉの重みが、
ｎｉ＝ＴＦｉ×ＩＤＦｉになり、
各画像に複数の特徴点があり、各特徴点をそれぞれ辞書の特定の単語に対応付け、重みを付け、該画像のＢａｇ−ｏｆ−ｗｏｒｄｓを構成し、Ｎは前記辞書の単語数を表し、
【数１９】
0006886212.tif 000020
として表すステップと、
他の画像のＢａｇ−ｏｆ−ｗｏｒｄｓをυＢとすると、２つの画像の類似性は、
【数２０】
0006886212.tif 000021
（ここで、Ｎは前記辞書内の単語数、υＡは１つの画像のＢａｇ−ｏｆ−ｗｏｒｄｓ、υＢは別の画像のＢａｇ−ｏｆ−ｗｏｒｄｓである。）であるステップと、を含む、ことを特徴とする請求項４に記載のＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法。
【請求項６】
前記ステップＳ４１では、ループバックキーフレームを特定することは、具体的には、
現在フレームに接続される局所キーフレームを除去した後、すべてのキーフレームをトラバースするステップと、
現在キーフレームと同じ単語を有するキーフレームを見つけ、すべての閉ループ候補フレームのうち、現在キーフレームと同じ単語が最も多いものの単語数を統計し、最も多い単語の８０％を第１の閾値として設定するステップと、
単語数が前記第１の閾値を超え、かつ類似性検出の結果が隣接キーフレームの最低スコアよりも大きいすべてのキーフレームを見つけ、このキーフレームに最も近く隣接する前の１０個のキーフレームを１グループとし、つまり、各々のキーフレームを中心として、中心となるキーフレームに隣接するキーフレームとともに前記グループを構成し、１つのキーフレームが１つ又は複数のグループに出現するステップと、
グループ分けされた各グループの合計スコア、及び、各グループのスコアが最も高いキーフレームを算出し、グループの最高スコアの０．７５を第２の閾値として、前記第２の閾値よりも高いすべてのグループのうちスコアが最も高いフレームを見つけ、ループバックキーフレームとするステップと、を含む、ことを特徴とする請求項５に記載のＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ドローンの技術分野、特にＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法に関する。
【背景技術】
【０００２】
ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  ｍａｐｐｉｎｇ）は自己位置推定と環境地図作成を同時に行う技術であり、ビデオカメラ及びコンピュータビジョンアルゴリズムを利用してロボットシステムに状態推定を提供し、小型移動ロボット、自動運転車やドローンにおいて人気が高まっている。
【０００３】
屋外で動作するドローンは、ＳＬＡＭ計算を実行するシステムとリアルタイムに通信する必要があり、オフライン処理によりドローンのサイズを縮小し、飛行時間を延ばすが、ドローンがＳＬＡＭ処理システムの無線通信範囲内に動作しなければならないため、該制限は、飛行範囲に深刻な影響を及ぼす。飛行範囲への影響を解消するために、ＳＬＡＭ処理はドローンにて実行される。しかしながら、視覚画像処理に基づくフロントエンド及びバックエンドの姿勢最適化は、計算量が大きく、リソースが制限され、組み込み型コントローラにおいてリアルタイムに実行することが困難である。ただし、処理ハードウェアを追加すると、負荷やサイズがドローンの飛行時間及び機動性に悪影響を及ぼす。従って、オンボード処理システムの体積、消費電力及び計算時間を削減することは、移動ロボット、特にドローンにとって非常に重要である。
【発明の概要】
【発明が解決しようとする課題】
【０００４】
本発明は、オンボード処理システムの体積、消費電力及び計算時間を削減するなど、ドローンに適用される従来の視覚ＳＬＡＭ方法の問題を解決するために、ＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法を提供する。
【課題を解決するための手段】
【０００５】
上記発明の目的を実現するために、以下の技術案を用いる。
ＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法であって、
二眼カメラの画像情報を読み取って前処理するステップＳ１と、
カメラ画像の特徴抽出と特徴マッチング、ＩＣＰアルゴリズムによる姿勢推定を含む、特徴点法で姿勢を推定する、フロントエンドタスクを実行するステップＳ２と、
グラフ最適化に基づくバックエンド最適化により、姿勢を最適化するバックエンド最適化ステップＳ３と、
バッグオブワードのループバック検出を行い、姿勢グラフを構築して最適化するステップＳ４と、を含み、
ステップＳ２では、カメラ画像の特徴抽出と特徴マッチングはＧＵＰで実行され、カメラ画像の特徴抽出と特徴マッチングの以降のステップはＯｐｅｎＣＶでＣＵＤＡ並列化を実現する。
【０００６】
好ましくは、前記前処理は、読み取ったカメラ画像をグレースケール化することである。
【０００７】
好ましくは、前記ステップＳ２では、カメラ画像の特徴抽出と特徴マッチングは、具体的には、
処理済みのグレースケール図について画像ピラミッドを構築し、ピラミッド画像の各層に抽出すべき特徴点の数を割り当てるステップＳ２１と、
ピラミッド画像の各層の特徴を抽出し、つまり、まず、ピラミッド画像の各層に対して境界処理を実行し、ピラミッド画像においてグリッドを分割して、各グリッドに対して特徴点抽出を行い、具体的には、前記グリッドにおけるいずれかの画素を検出対象の画素とし、検出対象の画素を円心、ｒを半径として円を作成し、該円と交差する画素が合計ｎ個であり、前記ｎ個の画素のうち、連続するＮ個（一般的には、Ｎは１１又は９である。）の画素と検出対象の画素のグレースケール値の差が予め設定された閾値Ｔよりも大きい場合、該検出対象の画素が特徴点であると見なされ、次に、各画素を繰り返して検出して、すべての特徴点を得るステップＳ２２と、
ステップＳ２２で抽出した特徴点について、除去及び分布均一化により最適化するステップＳ２３と、
ステップＳ２３で得られたいずれかの特徴点について、隣接する画素のマトリックスを、
【数１】
0006886212.tif 000002
特徴点の方向をθ＝ａｒｃｔａｎ（ｍ０１／ｍ１０）に定義するステップＳ２４と、
ｒＢＲＩＥＦアルゴリズムを利用して各特徴点の記述子を算出し、前記ピラミッド画像における現在フレームと参照フレームの特徴点のマッチングを実現するステップＳ２５と、を含む。
【０００８】
好ましくは、前記ステップＳ２では、ＩＣＰアルゴリズムで姿勢を推定することは、具体的には、
ＩＣＰアルゴリズムによって、非線形方法を利用してフレーム間の姿勢変換（Ｒ,Ｔ）を推定し、特徴点の深度情報に基づき最小二乗問題を構築するステップと、
【数２】
0006886212.tif 000003
現在姿勢を最適化変数として、Ｂｕｎｄｌｅ  Ａｄｊｕｓｔｍｅｎｔをコアとするグラフ最適化方法で最適化し、反復方法によって現在姿勢を推定するステップと、を含む。
【０００９】
好ましくは、前記ステップＳ３は、具体的には、
ＳＬＡＭプロセスの観測方程式において、その全体コスト関数を、
【数３】
0006886212.tif 000004
（ここで、ｚｉｊは姿勢ξｉで道路標識ｐｊを観察して生じた観測データであり、ｅｉｊは観測値と測定値の誤差である。）とするステップと、
上記式の最小二乗の解を求め、姿勢及び道路標識を最適化変数として、Ｂｕｎｄｌｅ  Ａｄｊｕｓｔｍｅｎｔ非線形最適化方法を利用し、下降方向Δｘを継続的に見つけ、上記式の最適解を求め、すなわち、最適化変数に増分を与え、上記式に対して一次テイラー展開を行い、
【数４】
0006886212.tif 000005
（ここで、Ｆｉｊは現在状態での全体コスト関数のカメラの姿勢への偏微分を表し、Ｅｉｊは道路標識点位置に対する全体コスト関数の偏微分を表す。）に単純化するステップと、
姿勢変数を合わせると、
【数５】
0006886212.tif 000006
（ここで、ΔｘｃとΔｘｐはそれぞれすべての姿勢変数を統合した全体増分、空間点の変数を統合した全体増分であり、ヤコビ行列Ｆ及びＥは前記全体増分に対する全体コスト関数の導関数である。）に単純化するステップと、
展開して増分線形方程式ＨΔｘ＝ｇを得るステップと、
ＬＭ法で解を求めると、Ｈ＝ＪＴＪ＋λＩ、Ｊ＝［ＦＥ］、ｇ＝−Ｊ（ｘ）Ｔｆ（ｘ）（ここで、Ｉは単位行列、λは正の実数であり、パラメータλが小さい場合、Ｈは支配的であり、ＬＭ法はガウスニュートン法に近く、λが大きい場合、λＩは支配的であり、ＬＭ法は最急降下法に近い。）になるステップと、を含む。
【００１０】
好ましくは、前記ステップＳ４は、具体的には、
バッグオブワードモデルを利用して画像間の類似性を算出し、ループバック発生の識別を加速し、ループバックフレームのスクリーニング閾値を動的算出することで、ループバックキーフレームを特定し、現在キーフレームとループバックキーフレームに対して計算してＳｉｍ３変換を行い、ループバック検出結果を得るステップＳ４１と、
ステップＳ４１のループバック検出結果に応じて作動軌跡を修正し、軌跡がループバック検出結果と一致する姿勢グラフを構築するステップＳ４２と、を含む。
【００１１】
好ましくは、前記ステップＳ４１では、画像間の類似性を算出するステップは、
前記バッグオブワードモデルに基づき、ＴＦ−ＩＤＦアルゴリズムで算出するステップと、
辞書をトレーニングする時のすべての特徴数をｎとし、画像中の単語ｗｉの数をｎｉとすると、単語のＩＤＦが、
ＩＤＦｉ＝ｌｏｇ（ｎ／ｎｉ）になり、
画像中の単語ｗｉの出現回数をｎｉとして、出現する単語数の総数をｎとすると、ＴＦが、
ＴＦｉ＝ｎｉ／ｎになり、
単語ｗｉの重みが、
ｎｉ＝ＴＦｉ×ＩＤＦｉになり、
各画像に複数の特徴点があり、各特徴点をそれぞれ辞書の特定の単語に対応付け、重みを付け、該画像のＢａｇ−ｏｆ−ｗｏｒｄｓを構成し、Ｎは前記辞書の単語数を表し、
【数６】
0006886212.tif 000007
（ここで、Ｎは前記辞書内の単語数、υＡは１つの画像のＢａｇ−ｏｆ−ｗｏｒｄｓ、υＢは別の画像のＢａｇ−ｏｆ−ｗｏｒｄｓである。）であるステップと、を含む。
【００１２】
好ましくは、前記ステップＳ４１では、ループバックキーフレームを特定することは、具体的には、
現在フレームに接続される局所キーフレームを除去した後、すべてのキーフレームをトラバースするステップと、
現在キーフレームと同じ単語を有するキーフレームを見つけ、すべての閉ループ候補フレームのうち、現在キーフレームと同じ単語が最も多いものの単語数を統計し、最も多い単語の８０％を第１の閾値として設定するステップと、
単語数が前記第１の閾値を超え、かつ類似性検出の結果が隣接キーフレームの最低スコアよりも大きいすべてのキーフレームを見つけ、このキーフレームに最も近く隣接する前の１０個のキーフレームを１グループとし、つまり、各々のキーフレームを中心として、中心となるキーフレームに隣接するキーフレームとともに前記グループを構成し、１つのキーフレームが１つ又は複数のグループに出現するステップと、
グループ分けされた各グループの合計スコア、及び、各グループのスコアが最も高いキーフレームを算出し、グループの最高スコアの０．７５を第２の閾値として、前記第２の閾値よりも高いすべてのグループのうちスコアが最も高いフレームを見つけ、ループバックキーフレームとするステップと、を含む。
【発明の効果】
【００１３】
従来技術に比べて、本発明の技術案の有益な効果は以下のとおりである。
【００１４】
本発明において、ＯＢＲ特徴抽出に基づき、バッグオブワードモデルと組み合わせて特徴マッチング判断を行う方法を用いることによって、視覚ＳＬＡＭフロントエンド処理の計算量、及び計算に必要なハードウェアリソースを減少させ、また、視覚ＳＬＡＭフロントエンド画像処理の特徴抽出部及び特徴マッチング部に対してＣＵＤＡ並列化を実行してＧＰＵを呼び出して加速し、それにより、ＣＰＵの計算負担を軽減し、処理速度を高め、オンボード処理システムの体積、消費電力及び算出時間を削減し、ドローンのリアルタイムな視覚ＳＬＡＭを実現する。
【図面の簡単な説明】
【００１５】
【図１】本発明の方法全体のフローチャートである。
【発明を実施するための形態】
【００１６】
図面は例示的に説明するために過ぎず、本特許を制限するものとして理解できない。
【００１７】
本実施例をよりよく説明するために、図面の部材の一部は省略、拡大又は縮小される場合があり、実際の製品の寸法を表すものではない。図面のいくつかの周知構造及び説明が省略されることは、当業者であれば理解できる。
【００１８】
以下、図面及び実施例を参照しながら本発明の技術案を更に説明する。
【００１９】
実施例１
本実施例では、ドローンの体積及び負荷を減少させるために、Ｎｖｉｄｉａ  Ｊｅｔｓｏｎ  ＴＸ２を、視覚ＳＬＡＭアルゴリズムを実行するオンボードプロセッサとして演算及び画像処理を行い、Ｎｖｉｄｉａ  Ｊｅｔｓｏｎ  ＴＸ２コアモジュールと載置板の重量が合計３００ｇ未満であり、このため、ドローンの負荷を大幅に軽減し、また、Ｎｖｉｄｉａ  Ｊｅｔｓｏｎ  ＴＸ２内にはＡＲＭ  Ｃｏｒｔｅｘ−Ａ５７（クアッドコア）及びＮｖｉｄｉａ  Ｄｅｎｖｅｒ２（デュアルコア）処理コア、２５６個のＣＵＤＡコアＰａｓｃａｌ  ＴＭアーキテクチャ設計が含まれ、本発明のＧＰＵアクセラレーションの要件を満たすことができる。
【００２０】
以下、本実施例に係るＧＰＵアクセラレーションに基づくドローン視覚ＳＬＡＭ方法を説明し、図１に示すように、ステップＳ１〜ステップＳ４を含む。
【００２１】
ステップＳ１、二眼カメラの画像情報を読み取って前処理する。前処理は読み取ったカメラ画像をグレースケール化することである。本実施例で用いた二眼カメラは、解像度が７５２＊４８０であり、６０ｆｐｓの収集周波数に達し、ベースライン長さが１２０ｍｍである。
Ｓ２、フロントエンドタスク：カメラ画像の特徴抽出と特徴マッチング、ＩＣＰアルゴリズムによる姿勢推定を含む、特徴点法で姿勢を推定する。ステップＳ２では、カメラ画像の特徴抽出と特徴マッチングはＧＵＰで実行され、カメラ画像の特徴抽出と特徴マッチングの以降のステップはＯｐｅｎＣＶでＣＵＤＡ並列化を実現する。
カメラ画像の特徴抽出と特徴マッチングは、具体的には、
処理済みのグレースケール図について画像ピラミッドを構築し、ピラミッド画像の各層に抽出すべき特徴点の数を割り当て、ＣＵＤＡ並列化実行後、ここでＯｐｅｎＣＶ  ＣＵＤＡＧｐｕＭａｔ法により画像ピラミッドを事前算出するステップＳ２１と、
ピラミッド画像の各層の特徴を抽出し、つまり、まず、ピラミッド画像の各層に対して境界処理を実行し、ピラミッド画像においてグリッドを分割して、各グリッドに対して特徴点抽出を行い、前記グリッドにおけるいずれかの画素を検出対象の画素とし、検出対象の画素を円心、ｒを半径として円を作成し、該円と交差する画素が合計１６個であり、前記１６個の画素のうち、連続する１２個の画素と検出対象の画素グレースケール値の差が予め設定された閾値Ｔよりも大きい場合、該検出対象の画素が特徴点であると見なされ、ここで、Ｔを原点画素のグレースケール値の２０％とし、次に、各画素を繰り返して検出してすべての特徴点を得て、ＣＵＤＡ並列化後、ブロックにおいて迅速に実行するように、ピラミッドの各層に対して、ＣＵＤＡ非同期カーネルを起動するステップＳ２２と、
ステップＳ２２で抽出した特徴点について、除去及び分布均一化により最適化するステップＳ２３と、
ステップＳ２３で得られたいずれかの特徴点について、隣接する画素のマトリックスを、
【数７】
0006886212.tif 000008
特徴点の方向をθ＝ａｒｃｔａｎ（ｍ０１／ｍ１０）に定義するステップＳ２４と、
ｒＢＲＩＥＦアルゴリズムを利用して各特徴点の記述子を算出し、前記ピラミッド画像における現在フレームと参照フレームの特徴点のマッチングを実現し、ＣＵＤＡ並列化実行後、キーポイントの記述子を算出するために、各層のピラミッドに対してＣＵＤＡ非同期カーネルを起動するステップＳ２５と、を含む。
【００２２】
ＩＣＰアルゴリズムで姿勢を推定することは、具体的には、
ＩＣＰアルゴリズムによって、非線形方法を利用してフレーム間の姿勢変換（Ｒ,Ｔ）を推定し、特徴点の深度情報に基づき最小二乗問題を構築するステップと、
【数８】
0006886212.tif 000009
現在姿勢を最適化変数として、Ｂｕｎｄｌｅ  Ａｄｊｕｓｔｍｅｎｔをコアとするグラフ最適化方法で最適化し、反復方法によって現在姿勢を推定するステップと、を含む。
【００２３】
Ｓ３、バックエンド最適化：本実施例では、グラフ最適化に基づくバックエンド最適化を用い、視覚ＳＬＡＭのフロントエンド視覚走行距離計が短時間の軌跡と地図しか提供しないため、必ず誤差があり、このように経時的に地図の正確性が不十分になり、従って、バックエンド最適化においてより長時間の状態推定の問題を考慮し、ＳＬＡＭにおける運動方程式と観測方程式はいずれも非線形関数であり、従って、本実施例では、非線形方法を用いてこの問題を記述し、具体的には、ＳＬＡＭプロセスの観測方程式において、その全体コスト関数を、
【数９】
0006886212.tif 000010
（ここで、ｚｉｊは姿勢ξｉで道路標識ｐｊを観察して生じた観測データであり、ｅｉｊは観測値と測定値の誤差である。）とするステップと、
上記式の最小二乗の解を求め、姿勢及び道路標識を最適化変数として、つまり、反復方法によって姿勢と道路標識を継続的に調整し、Ｂｕｎｄｌｅ  Ａｄｊｕｓｔｍｅｎｔ非線形最適化方法を利用し、下降方向Δｘを継続的に見つけ、上記式の最適解を求め、すなわち、最適化変数に増分を与え、上記式に対して一次テイラー展開を行い、
【数１０】
0006886212.tif 000011
（ここで、Ｆｉｊは現在状態での全体コスト関数のカメラの姿勢への偏微分を表し、Ｅｉｊは道路標識点位置に対する全体コスト関数の偏微分を表す。）に単純化するステップと、
姿勢変数を合わせると、
【数１１】
0006886212.tif 000012
（ここで、ΔｘｃとΔｘｐはそれぞれすべての姿勢変数を統合した全体増分、空間点の変数を統合した全体増分であり、ヤコビ行列Ｆ及びＥは前記全体増分に対する全体コスト関数の導関数であり、バックエンド最適化においてより長時間の状態推定を考慮するため、これらの行列は各誤差項の導関数からなる非常に大きな行列である。）に単純化するステップと、
展開して増分線形方程式ＨΔｘ＝ｇを得るステップと、
ＬＭ法で解を求めると、Ｈ＝ＪＴＪ＋λＩ、Ｊ＝［ＦＥ］、ｇ＝−Ｊ（ｘ）Ｔｆ（ｘ）（ここで、Ｉは単位行列、λは正の実数である。）であり、
このＨ行列は非常に大きな行列であり、直接的に逆行列化すると、計算リソースがかかるが、該行列の疎行列は特定の特殊構造を有し、この行列の疎性を利用して周辺化すると、求解プロセスを加速できるステップと、を含む。
【００２４】
Ｓ４、バッグオブワードのループバック検出を行い、姿勢グラフを構築して最適化し、具体的には、ステップＳ４１〜ステップＳ４２を含む。
【００２５】
Ｓ４１、画像間の類似性を算出するステップは、
前記バッグオブワードモデルに基づき、ＴＦ−ＩＤＦアルゴリズムで算出するステップと、
辞書をトレーニングする時のすべての特徴数をｎとし、画像中の単語ｗｉの数をｎｉとすると、単語のＩＤＦが、
ＩＤＦｉ＝ｌｏｇ（ｎ／ｎｉ）になり、
画像中の単語ｗｉの出現回数をｎｉとして、出現する単語数の総数をｎとすると、ＴＦが、
ＴＦｉ＝ｎｉ／ｎになり、
単語ｗｉの重みが、
ｎｉ＝ＴＦｉ×ＩＤＦｉになり、
各画像に複数の特徴点があり、各特徴点をそれぞれ辞書の特定の単語に対応させ、重みを付け、該画像のＢａｇ−ｏｆ−ｗｏｒｄｓを構成し、Ｎは前記辞書の単語数を表し、
【数１２】
0006886212.tif 000013
（ここで、Ｎは前記辞書内の単語数、υＡは１つの画像のＢａｇ−ｏｆ−ｗｏｒｄｓ、υＢは別の画像のＢａｇ−ｏｆ−ｗｏｒｄｓである。）であるステップと、を含む。
【００２６】
ループバックキーフレームを特定することは、具体的には、
現在フレームに接続される局所キーフレームを除去した後、すべてのキーフレームをトラバースするステップと、
現在キーフレームと同じ単語を有するキーフレームを見つけ、すべての閉ループ候補フレームのうち、現在キーフレームと同じ単語が最も多いものの単語数を統計し、最も多い単語の８０％を第１の閾値として設定するステップと、
単語数が前記第１の閾値を超え、かつ類似性検出の結果が隣接キーフレームの最低スコアよりも大きいすべてのキーフレームを見つけ、このキーフレームに最も近く隣接する前の１０個のキーフレームを１グループとし、つまり、各々のキーフレームを中心として、中心となるキーフレームに隣接するキーフレームとともに前記グループを構成し、１つのキーフレームが１つ又は複数のグループに出現するステップと、
グループ分けされた各グループの合計スコア、及び、各グループのスコアが最も高いキーフレームを算出し、グループの最高スコアの０．７５を第２の閾値として、前記第２の閾値よりも高いすべてのグループのうちスコアが最も高いフレームを見つけ、ループバックキーフレームとするステップと、を含む。
【００２７】
Ｓ４２、ステップＳ４１のループバック検出結果に応じて動作軌跡を修正し、軌跡がループバック検出結果と一致する姿勢グラフを構築する。姿勢の推定が通常再帰的なプロセスであり、つまり、前のフレームの姿勢から現在フレームの姿勢を解き、従って、この再帰的なプロセスでその誤差がフレームごとに伝送され、それにより累積誤差が生じる。この累積誤差を解消又は低減するために、動作軌跡を修正し、姿勢グラフを構築して姿勢を最適化する。
【００２８】
本実施例では、ＯｐｅｎＣＶでＣＵＤＡ並列化を実現し、並列化はＣＵＤＡ追跡ブロックＩＤ及びスレッドＩＤでなければならない。ＣＵＤＡは同じコードブロックにおいてマルチスレッドを有し、同じコードブロックにおける他のスレッドに比べて、スレッドごとに一意の識別子を有するＩＤがあり、各コードブロックが一意のＩＤを有することにより、各コードブロックにおける１つのスレッドが不意にスレッドの別のコードブロックと見なされることがない。各ブロックのサイズは、各ブロック内のスレッドの数を示すことができる。スレッドは、ブロックに配置され、スレッド追跡情報を記憶し、このようにして、大量のメモリーを必要とせずに済む。
【００２９】
図面における位置関係を記述する用語は、例示的な説明に過ぎず、本特許を限定するものとして理解できない。
【００３０】
もちろん、本発明の上記実施例は、本発明を明確に説明するための単なる例であり、本発明の実施形態を限定するものではない。当業者であれば、上記説明に基づき、他の形態の変化又は変更を行うことができる。ここでは、あらゆる実施形態を列挙する必要はなく、かつ、列挙することは不可能である。本発明の精神及び原理の範囲内で行われるあらゆる修正、等価置換や改良などは、すべて本発明の特許請求の範囲の特許範囲に含まれるものとする。
【図１】
0006886212.tif 000022