ARM NEONの使い方 データ型編

NEONデータ型の概要

NEONのデータ型は以下の規則で名前が付けられています

<type><size>x<number of lanes>_t

例えば、 int16x4_t は符号付き16bit整数を4個保持する型となります
それぞれの要素を「レーン」と呼びます

NEONのデータ型は64bitのものと128bitのものがあります

NEONデータ型一覧

NEONのデータ型一覧を示します

64bit型	128bit型
int8x8_t	int8x16_t
int16x4_t	int16x8_t
int32x2_t	int32x4_t
int64x1_t	int64x2_t
uint8x8_t	uint8x16_t
uint16x4_t	uint16x8_t
uint32x2_t	uint32x4_t
uint64x1_t	uint64x2_t
float16x4_t	float16x8_t
float32x2_t	float32x4_t
poly8x8_t	poly8x16_t
poly16x4_t	poly16x8_t

以上をさらに配列にした型もあり、以下の規則で名前が付けられています

<type><size>x<number of lanes>x<length of array>_t

例えば、 int16x4x2_t は int16x4_t を2つ要素に持つ型です
各要素はvalというメンバに格納されています

struct int16x4x2_t
{
    int16x4_t val[2];
};

これらの配列型は一部のNEON命令で使用されます
使いどころについては今後の記事で解説したいと思います

polyって何?

データ型一覧の中に、見慣れないpolyという型がありました
気になってググったところ、以下の投稿を発見しました

stackoverflow.com

どうやら多項式(polynomial)算術演算というものをするための型らしく
誤り検出符号や暗号化に利用されているようです

参考

ARM Information Center

次回

個別の組み込み関数について説明する前に、全体像に触れておきたいと思います
ARM NEONの使い方 組み込み関数一覧編

ARM NEONの使い方 予告編

はじめに

ここ最近、NEON(ARMのSIMD命令)を使ったプログラムを初めて書きました
NEONについて調べていて思ったのが、NEONの使い方を初心者向けに
日本語で説明したページがあまり見当たらないということ
そんなわけで、自分なりにNEONの使い方をまとめておこうと思いました

書く予定のもの

• データ型編
• 組み込み関数一覧編
• ロード・ストア編
• 加算編
• 減算編
• 乗算編
• 除算編
• 初期化編
• 比較・選択編
• シフト編
• 結合・分離編

とりあえず自分がある処理をNEON化する上で必要だった知識を取り上げます
すべてのNEON命令をカバーしているわけではありませんので悪しからず

次回

まずはNEONのデータ型について解説します
ARM NEONの使い方 データ型編

今使ってるvimプラグイン

最近の私はIDE(Visual Studio、Eclipse)を中心に開発することが多かったのですが
休日は久々にvimで遊びました
というわけで(?)、現在私が使っているvimプラグインをご紹介します
私は普段c++を使うことが多いので、c++向けの設定も入ってます

目次

プラグイン管理ツール

まずは管理ツールを入れなきゃねってことで
これはNeoBundleで決まりでしょ!
…って思ってたら気になる記事がいくつか

• おい、NeoBundle もいいけど vim-plug 使えよ - Qiita
• NeoBundle から vim-plug に乗り換えてみた - Qiita

このvim-plug、導入が楽なので気に入った!
NeoBundleも十分楽だけどこっちはもっと楽でした

それでは本編

scrooloose/nerdtree

GitHub - scrooloose/nerdtree: A tree explorer plugin for vim.

言わずと知れたファイルエクスプローラ
私はF2でエクスプローラを開閉するように設定しています

nnoremap <F2> :NERDTreeToggle<CR>

tomasr/molokai

GitHub - tomasr/molokai: Molokai color scheme for Vim

カラースキーマ
Sublime Textで使われてるやつ
molokai_originalの方が柔らかくて好き

colorscheme molokai
let g:molokai_original = 1

tyru/caw.vim

GitHub - tyru/caw.vim: Vim comment plugin: supported operator/non-operator mappings, repeatable by dot-command, 300+ filetypes

自動コメント/コメント解除
ノーマルモード + <C-k>で現在行をコメント
ビジュアルモード + <C-k>で複数行を一括コメント!

nmap <C-k> <plug>(caw:i:toggle)
vmap <C-k> <plug>(caw:i:toggle)

itchyny/lightline.vim

GitHub - itchyny/lightline.vim: A light and configurable statusline/tabline for Vim

ステータスライン
現在の編集モードなどをカラフルに表示してくれる

octol/vim-cpp-enhanced-highlight

GitHub - octol/vim-cpp-enhanced-highlight: Additional Vim syntax highlighting for C++ (including C++11/14)

c++のシンタックスハイライト強化版
デフォルトのハイライトが寂しいと感じたらおススメ

Townk/Vim-autoclose

GitHub - Townk/vim-autoclose: This plugin for Vim enable an auto-close chars feature for you. For instance if you type an '(', ``autoclose`` will automatically insert a ')' and put the cursor between than.

最近のIDEなら必ずあるであろう
対応する括弧の自動補完

junegunn/vim-easy-align

GitHub - junegunn/vim-easy-align: A Vim alignment plugin

選択範囲を指定した文字(<Space>, =, :, etc.)で揃えてくれる
これが決まると超絶気持ちがいい
vimを使ってない人にドヤりたくなる(やらないけどw)プラグイン

justmao945/vim-clang

GitHub - justmao945/vim-clang: Clang completion plugin for vim

こっからは補完系プラグインの紹介
vim-clangはclangを使ったc++の補完プラグイン
標準ライブラリの補完ができるのは強い

Shougo/neocomplete

GitHub - Shougo/neocomplete.vim: Next generation completion framework after neocomplcache

言わずと知れた補完プラグイン
タイプしたその場で補完候補が出るので一気にIDEっぽくなる
ファイルパスを保管してくれるのも地味に便利

Shougo/neosnippet

GitHub - Shougo/neosnippet.vim: neo-snippet plugin contains neocomplcache snippets source

コードスニペット補完
今日入れましたが、なにこれ超便利
挿入モードでp<C-k>を打つと↓が挿入されて感動!

std::cout <<  << std::endl;

Shougo/neosnippet-snippets

GitHub - Shougo/neosnippet-snippets: The standard snippets repository for neosnippet

スニペットの辞書
neosnippetと一緒にいれておく

Free Space Computation

以前の記事で紹介したStixelのアルゴリズムに登場する
Free Space Computationを実装しようと頑張ってます

Free Space Computationとは

視差画像をもとにシーンのFree Space(移動可能な領域、路面)を推定する手法です
推定結果のイメージです(赤く塗った領域がFree Space)

より具体的には、Free Space Computationは
画像の列ごとに「路面と物体の下端との境界」となる行を見つける問題となります

参考文献

[1]Efficient Representation of Traffic Scenes by Means of Dynamic Stixels
[2]Free Space Computation Using Stochastic Occupancy Grids and Dynamic Programming

考え方

下図に、画像のある列におけるステレオ観測値(=奥行き)の分布を示します

緑線が路面、赤線が物体、青線が背景の領域です
この図から、

1. 「路面と物体の下端との境界」から手前側(カメラ側)は路面の領域
2. 物体領域の奥行きはほぼ一定(物体は大抵の場合直立しているため)

ということがわかると思います

これをより数学的に書くと次のようになります

1. を「路面と物体の下端との境界」となる行、を画像下端の行とすると、]を満たすは路面領域内にあり、その視差はで表される
2. を物体の高さ(画素単位)とすると、]を満たすは物体領域内にあり、その視差はで一定である

ここではある行に対応する路面上の視差を返す関数であり
論文では観測した路面の視差をB-Splineでフィッティングすることでを求めています

この考えをもとに、境界の位置で最小となるようなコスト関数を定義します

コスト関数

コスト関数は以下のように、2つのサブコスト と の重み付き和となります

は路面(Road)のコストで、以下のように表されます

は物体(Object)のコストで、以下のように表されます

これを各画素について計算します
コードで書くとこんな感じです

for (int u = 0; u < disp.cols; ++u)
{
	for (int vb = 0; vb < disp.rows; ++vb)
	{
		float objectCost = 0, roadCost = 0;

		for (int v = vb; v < disp.rows; ++v)
			roadCost += fabsf(disp(v, u) - roadDisp[v]);

		for (int v = max(vb - hv, 0); v < vb; ++v)
			objectCost += fabsf(disp(v, u) - roadDisp[vb]);

		score(vb, u) = alpha1 * roadCost + alpha2 * objectCost;
	}
}

DPによる境界の計算

最終的な出力である「路面と物体の下端との境界」を計算します
これは先ほど求めたスコアの値だけではなく、境界の空間的な連続性も考慮する必要があります
そこで画像の各画素をノードとして、左端と右端を結ぶ最適な経路を求める問題を考えます

これは動的計画法的に以下のように書けます

for (int v = 0; v < score.rows; v++)
{
	dpcost(v, 0) = score(v, 0)
}

for (int u = 1; u < score.cols; u++)
{
	for (int v = 0; v < score.rows; v++)
	{
		float mincost = FLT_MAX;
		int minpath = 0;

		for (int vv = 0; vv < score.rows; vv++)
		{
			int jump = abs(disp(v, u) - disp(vv, u - 1));
			int penalty = std::min(P1 * jump, P1 * P2);
			float cost = score(v, u) + dpcost(vv, u - 1) + penalty;

			if (cost < mincost)
			{
				mincost = cost;
				minpath = vv;
			}
		}

		dpcost(v, u) = mincost;
		dppath(v, u) = minpath;
	}
}

// back track
float mincost = FLT_MAX;
int minv = 0;
for (int v = 0; v < dpcost.rows; v++)
{
	if (dpcost(v, dpcost.cols - 1) < mincost)
	{
		mincost = dpcost(v, dpcost.cols - 1);
		minv = v;
	}
}

for (int u = dppath.cols - 1; u >= 0; u--)
{
	path[u] = minv;
	minv = dppath(minv, u);
}

上記コードで境界の空間的な連続性を与えるのが、以下の部分です

int jump = abs(disp(v, u) - disp(vv, u - 1));
int penalty = std::min(P1 * jump, P1 * P2);

隣接ノードとの視差の絶対差分に比例したペナルティを与えます
(ただし、ある程度の不連続性を許容するため差分がP2以上であればペナルティは一定とする)

ただこの計算、オーダーがとなって結構重いです
これであってるんかな…

実装結果

前回紹介したデータセットを使って実験した結果が最初に見せた図です
入力の視差画像はデータセットから与えられます
路面上の視差はB-Splineではなく、直線で近似してます
まあ何となくあってそうに見える…

おわりに

Free Space Computationについて理解したこと、実装したことを紹介しました
今後はB-splineによる路面モデルの推定を実装したいです

EISATS

EISATS | Reinhard Klette: CCVというコンピュータビジョン向けのデータセットがありまして

EISATS
The .enpeda.. Image Sequence Analysis Test Site (EISATS) offers sets of image sequences for the purpose of comparative performance evaluation of stereo vision, optic flow, motion analysis, or further techniques in computer vision.

そのうちSET2は車載シーンが合成画像で作成されており
オプティカルフロー、視差、エゴモーション等の正解データが取得できます

ときどきOpenCV上で利用するのですが、毎度データの読み込み方法を忘れるので
自分へのメモとして各データを読み込んで表示するサンプルプログラムを作成しました
github.com

使用する際は前述のSET2から
SEQUENCE2のデータを適当なディレクトリにダウンロード・解凍して

/home/hoge/EISATS/SET2/S2/
├── colour-left-S2
├── disparityGT-S2
├── egoMotion
├── flowX-S2
├── flowY-S2

そのディレクトリをプログラムに渡します

./eisats-synthesized-sequences /home/hoge/EISATS/SET2/S2/

ソースファイルは1個だけなので全部載せておきます

#include <iostream>
#include <iomanip>
#include <string>
#include <sstream>
#include <fstream>
#include <opencv2/opencv.hpp>

namespace {

	std::string numberString(int n)
	{
		std::stringstream ss;
		ss << std::setw(3) << std::setfill('0') << n;
		return ss.str();
	}

	cv::Mat readRaw(const std::string& filename)
	{
		std::ifstream ifs(filename, std::ios::binary);
		if (ifs.fail())
			return cv::Mat();

		std::string line;
		while (getline(ifs, line) && line[0] == '#') {}

		int width, height, depth;
		sscanf(line.c_str(), "%d %d %d", &width, &height, &depth);
		CV_Assert(depth == 32);

		cv::Mat img(height, width, CV_32F);
		ifs.read((char *)img.data, sizeof(float) * width * height);

		return img;
	}

	void readEgoMotion(const std::string& filename, cv::Matx33d& R, cv::Matx31d& t)
	{
		std::ifstream ifs(filename);
		if (ifs.fail())
			return;

		std::string line;
		while (getline(ifs, line) && line[0] == '#') {}

		sscanf(line.c_str(), "%lf %lf %lf %lf", &R(0, 0), &R(0, 1), &R(0, 2), &t(0)); getline(ifs, line);
		sscanf(line.c_str(), "%lf %lf %lf %lf", &R(1, 0), &R(1, 1), &R(1, 2), &t(1)); getline(ifs, line);
		sscanf(line.c_str(), "%lf %lf %lf %lf", &R(2, 0), &R(2, 1), &R(2, 2), &t(2));
	}

	void drawOpticalFlow(cv::Mat& img, const cv::Mat1f& flowx, const cv::Mat1f& flowy)
	{
		float maxnorm = 0;
		for (int i = 0; i < flowx.rows; ++i)
		{
			for (int j = 0; j < flowx.cols; ++j)
			{
				float x = flowx(i, j);
				float y = flowy(i, j);
				maxnorm = std::max(maxnorm, sqrtf(x * x + y * y));
			}
		}

		img.create(flowx.size(), CV_8UC3);
		for (int i = 0; i < flowx.rows; ++i)
		{
			for (int j = 0; j < flowx.cols; ++j)
			{
				float x = flowx(i, j);
				float y = flowy(i, j);

				// Convert flow angle to hue
				float angle = atan2f(y, x);
				if (angle < 0.f) angle += (float)(2 * CV_PI);
				angle /= (float)(2 * CV_PI);
				uchar hue = static_cast<uchar>(180 * angle);

				// Convert flow norm to saturation
				float norm = sqrtf(x * x + y * y) / maxnorm;
				uchar sat = static_cast<uchar>(255 * norm);

				img.at<cv::Vec3b>(i, j) = cv::Vec3b(hue, sat, 255);
			}
		}

		cv::cvtColor(img, img, cv::COLOR_HSV2BGR);
	}

} /* namespace */

int main(int argc, char *argv[])
{
	if (argc < 2)
	{
		std::cerr << "Usage: " << argv[0] << " [data path]" << std::endl;
		return -1;
	}

	std::string dataPath(argv[1]);

	for (int frameNo = 2; ; ++frameNo)
	{
		// Read left image
		cv::Mat leftImg = cv::imread(dataPath + "/colour-left-S2/img_c0_" + numberString(frameNo) + ".ppm");
		if (leftImg.empty())
			break;

		// Read optical flow map
		cv::Mat flowx = readRaw(dataPath + "/flowX-S2/flowU_from_" + numberString(frameNo - 1) + "_to_" + numberString(frameNo) + ".raw");
		cv::Mat flowy = readRaw(dataPath + "/flowY-S2/flowV_from_" + numberString(frameNo - 1) + "_to_" + numberString(frameNo) + ".raw");
		if (flowx.empty() || flowy.empty())
			break;

		// Read disparity map
		cv::Mat disp = readRaw(dataPath + "/disparityGT-S2/stereo_" + numberString(frameNo) + ".raw");
		if (disp.empty())
			break;

		// Read ego-motion
		cv::Matx33d R;
		cv::Matx31d t, r;
		readEgoMotion(dataPath + "/egoMotion/from_" + numberString(frameNo - 1) + "_to_" + numberString(frameNo) + ".txt", R, t);
		cv::Rodrigues(R, r);

		// Draw and display image
		cv::Mat flowImg, dispImg;
		drawOpticalFlow(flowImg, flowx, flowy);
		cv::normalize(disp, dispImg, 0.0, 1.0, cv::NORM_MINMAX);

		cv::putText(leftImg, "Rotation: " + std::to_string(r(0)) + " " + std::to_string(r(1)) + " " + std::to_string(r(2)),
			cv::Point(10, 20), 0, 0.5, cv::Scalar(0, 255, 255));
		cv::putText(leftImg, "Translation: " + std::to_string(t(0)) + " " + std::to_string(t(1)) + " " + std::to_string(t(2)),
			cv::Point(10, 40), 0, 0.5, cv::Scalar(0, 255, 255));

		cv::imshow("Left image", 16 * leftImg);
		cv::imshow("Flow image", flowImg);
		cv::imshow("Disp image", dispImg);

		char c = cv::waitKey(100);
		if (c == 27) // ESC
			break;
	}

	return 0;
}

6D-Visionについて調べてみた

車載ステレオカメラによる画像認識技術「6D-Vision」について調べてみました

6D-Visionとは

6D-Visionとは自動車メーカーのダイムラー(Daimler AG)が開発している
自動運転のための画像認識技術です
メルセデス・ベンツの資料に

2012 年からは、メルセデス･ ベンツによる高度
支援システムの総称として、「インテリジェントド
ライブ」という名称が導入されました。その基
盤となる革新的な 6D ビジョンは、車載ステレオ
カメラで撮影した映像を処理・解析する技術で
す。車両センサーでクルマの周囲の状況を瞬時
に検知。レーダーセンサーと連動したステレオ
カメラによって、車両や歩行者などの動く対象物
を認識し、その位置や運動方向（速度も含む）
を測定することで、その後の行動を予測します。
危険な状況が発生すると、車載支援システムが
適切に、しかも瞬時に対処します。

とあるように、6D-Visionの特徴は

• ステレオカメラを使用
• 車両や歩行者などの動く対象物を認識
• その位置や運動方向（速度も含む）を測定することで行動を予測

とのことです

6D-Visionの詳細

6D-Visionはどのように実現されているのか
6D-Visionには公式ページがあり、研究の紹介や論文へのリンクが載ってます
www.6d-vision.com
うーん、とても勉強になる

Stixel World

公式ページの中で興味を引いたのが「Stixel」と呼ばれるシーンの表現手法です
こちらのデモ動画をご覧下さい

www.youtube.com

物体を覆う棒状のSuperpixel(ピクセルをグループ化したもの)がStixelと呼ばれます
路面から物体を覆う高さまで伸びています
また距離の情報も備えており、動画ではカメラからの距離によって色付けされています

物体から伸びる白い矢印は、物体の移動方向と0.5秒後に到達する位置を示しています
これにより自動車が危険な状況を察知できるというわけですね
6D-Visionの6Dとは、物体の3次元位置(X,Y,Z)と速度(Vx,Vy,Vz)を合わせた
6次元のパラメータに由来し、ここでは各Stixelについて運動を推定しています

アルゴリズム

まだ理解できてないのでちゃんと説明できませんが
わかったことを処理の流れにそって書いておきます
読んだ論文はこれです

Dence Stereo

まずステレオ画像から密な(全ピクセルの)視差を求めます
論文ではSGM(Semi-Global Matching)と呼ばれるアルゴリズムを使用しており
FPGA上に実装して高速化しているようです

Stixelの構築

まず視差画像を路面と物体の領域に分け(動的計画法を使うらしい)
次に物体の高さを推定、最後に物体の領域をStixelの幅で区切っているようです

Stixelの運動推定

カルマンフィルタによってStixelの正確な運動を推定しています
状態ベクトルは3次元位置(X,Y,Z)と速度(Vx,Vy,Vz)からY成分を除いた
4次元ベクトルになります(物体が垂直方向に移動しないと仮定)
またこの箇所ではエゴモーション(自車の運動)と密なオプティカルフローを計算しており
それぞれ状態の予測ステップと修正ステップで使用しています

おわりに

今車載関連の仕事をしてるのですが
最近6D-Visionについて耳にすることがあり
気になったので調べてみました
アルゴリズムをちゃんと理解したいです

Mean-shift Trackingを実装してみた

勉強のためにMean-shift Trackingを実装してみました

ソースコード

以下に公開してあります
github.com

参考

実装の際に参考にしたのはOpenCVのCamShiftのサンプルコードと
こちらのintelの論文です
http://opencv.jp/opencv-1.0.0_org/docs/papers/camshift.pdf
URLから分かるようにCamShiftのオリジナルっぽいですね

アルゴリズム

アルゴリズムを簡単に紹介します

追跡の開始

最初に追跡対象の色ヒストグラムを計算します

1. 追跡ウィンドウの位置とサイズを設定
2. ウィンドウ内の色ヒストグラムを計算

追跡ウィンドウの更新

枚フレーム以下の処理を行います

1. 追跡対象の2次元確率分布を計算
2. 追跡ウィンドウ内の確率分布の重心を求める
3. ウィンドウの中心が重心の位置に来るようウィンドウを更新
4. 収束するまで2、3を繰り返す

2次元確率分布は要するに「追跡対象らしさマップ」みたいなものです
最初に計算したヒストグラムを2次元に逆投影(back projection)することで求められます
2、3がMean-shiftの本処理です
確率分布のピークに向かってウインドウを移動させます
さらにウィンドウのサイズや傾きを計算する処理が加わると
CamShiftと呼ばれる手法になります

あとはこことかも参考になりました
OpenCV: Meanshift and Camshift

ソースの解説

MeanShiftTrackerクラス

start()で追跡の開始、update()で追跡ウィンドウの更新を行います
勉強用ということでメンバは公開にしました

class MeanShiftTracker
{
public:
	MeanShiftTracker();
	void start(const cv::Mat& img, const cv::Rect& window);
	int update(const cv::Mat& img, cv::Rect& window);
	int vmin_, vmax_, smin_;
	cv::Mat hist_, backProject_;
};

追跡の開始

ウィンドウ内の色ヒストグラムを計算します
純粋に色だけに着目したい(輝度や彩度の影響を無視したい)ので
RGBをHSVに変換しそのHue成分だけを使用します
また輝度や彩度が低い画素では色の表現力も弱くなるので
そのような画素もマスクによって計算から除外します

void MeanShiftTracker::start(const cv::Mat& img, const cv::Rect& window)
{
	CV_Assert(img.type() == CV_8UC3);

	// ROIの設定
	cv::Mat roi = img(window);

	// HSVに変換
	cv::Mat hsv;
	cv::cvtColor(roi, hsv, cv::COLOR_BGR2HSV);

	// マスクの作成
	cv::Mat mask;
	cv::inRange(hsv, cv::Scalar(0, smin_, vmin_), cv::Scalar(180, 256, vmax_), mask);

	// Hue成分の抽出
	cv::Mat hue(hsv.size(), hsv.depth());
	int fromTo[2] = { 0, 0 };
	cv::mixChannels({ hsv }, { hue }, fromTo, 1);

	// ヒストグラムの計算
	int histSize = 64;
	float range[] = { 0, 180 };
	calcHist(hue, mask, hist_, histSize, range);
}

ヒストグラムの計算では値がrangeの範囲内にある画素を
0≦x<1となるよう正規化し、histSizeをかけることでビンを算出しています

void calcHist(const cv::Mat& img, const cv::Mat& mask, cv::Mat& hist, int histSize, const float *range)
{
	CV_Assert(img.type() == CV_8U);
	CV_Assert(mask.type() == CV_8U);
	CV_Assert(mask.size() == img.size());

	hist.create(cv::Size(1, histSize), CV_32F);
	hist = cv::Scalar::all(0);

	float minv = range[0];
	float maxv = range[1];

	for (int i = 0; i < img.rows; ++i)
	{
		for (int j = 0; j < img.cols; ++j)
		{
			float v = img.at<uchar>(i, j);
			if (v < minv || v >= maxv)
				continue;

			float nv = (v - minv) / (maxv - minv);
			int bin = static_cast<int>(histSize * nv);
			CV_Assert(bin < histSize);
			if (mask.at<uchar>(i, j))
				hist.at<float>(bin) += 1.0f;
		}
	}
}

追跡ウィンドウの更新

全体の流れはこんな感じ
前半部分は開始処理とほとんど同じですね

int MeanShiftTracker::update(const cv::Mat& img, cv::Rect& window)
{
	// HSVに変換
	cv::Mat hsv;
	cv::cvtColor(img, hsv, cv::COLOR_BGR2HSV);

	// マスクの作成
	cv::Mat mask;
	cv::inRange(hsv, cv::Scalar(0, smin_, vmin_), cv::Scalar(180, 256, vmax_), mask);

	// Hue成分の抽出
	cv::Mat hue(hsv.size(), hsv.depth());
	int fromTo[2] = { 0, 0 };
	cv::mixChannels({ hsv }, { hue }, fromTo, 1);

	// ヒストグラムの逆投影
	float range[] = { 0, 180 };
	calcBackProject(hue, hist_, backProject_, range);

	// 逆投影画像のマスキング
	cv::bitwise_and(backProject_, mask, backProject_);

	// ウィンドウの更新
	return meanShift(backProject_, window);
}

追跡対象の2次元確率分布を求めるため、ヒストグラムの逆投影を行います
各画素に対しヒストグラムを計算した時と同じ方法でビンを計算し
ビンに対応するヒストグラムの値(度数)を逆投影画像に格納します
追跡対象に近い色を持つならば、大きな値になるはずです
見やすいように値を0～255に正規化してあります

void calcBackProject(const cv::Mat& img, const cv::Mat& hist, cv::Mat& backProject, const float *range)
{
	CV_Assert(img.type() == CV_8U);
	CV_Assert(hist.type() == CV_32F);

	backProject.create(img.size(), CV_8U);
	backProject = cv::Scalar::all(0);

	cv::Mat _hist = hist;
	cv::normalize(_hist, _hist, 0, 255, cv::NORM_MINMAX);

	int histSize = hist.rows;
	float minv = range[0];
	float maxv = range[1];

	for (int i = 0; i < img.rows; ++i)
	{
		for (int j = 0; j < img.cols; ++j)
		{
			float v = img.at<uchar>(i, j);
			if (v < minv || v >= maxv)
				continue;

			float nv = (v - minv) / (maxv - minv);
			int bin = static_cast<int>(histSize * nv);
			CV_Assert(bin < histSize);
			backProject.at<uchar>(i, j) = cv::saturate_cast<uchar>(_hist.at<float>(bin));
		}
	}
}

追跡ウィンドウ内の確率分布の重心を求めます
分布の0次モーメントを

1次モーメントを

とすると、重心は以下のように求まります

ウィンドウの中心が重心の位置に来るようウィンドウ位置を更新します
反復が既定回数に達するか、ウィンドウの移動量が小さくなるまで繰り返します

int meanShift(const cv::Mat& probImage, cv::Rect& window)
{
	CV_Assert(probImage.type() == CV_8U);

	int maxiter = 20;
	for (int iter = 0; iter < maxiter; ++iter)
	{
		// 重心の計算
		unsigned int mz = 0, mx = 0, my = 0;
		for (int i = 0; i < window.height; ++i)
		{
			for (int j = 0; j < window.width; ++j)
			{
				int y = i + window.y;
				int x = j + window.x;

				mz += probImage.at<uchar>(y, x);
				mx += x * probImage.at<uchar>(y, x);
				my += y * probImage.at<uchar>(y, x);
			}
		}

		if (mz == 0)
			return 0;

		int cx = mx / mz;
		int cy = my / mz;

		// ウィンドウの位置を更新
		int winx = cx - window.width/2;
		int winy = cy - window.height/2;

		winx = std::min(probImage.cols - 1 - window.width, std::max(0, winx));
		winy = std::min(probImage.rows - 1 - window.height, std::max(0, winy));

		// 移動量が小さい場合は終了
		if (abs(winx - window.x) < 1 && abs(winy - window.y) < 1)
			break;

		window.x = winx;
		window.y = winy;
	}

	return 1;
}

サンプル

入力データはこちらを利用させていただきました
David Ross - Incremental Visual Tracking

OpenCVのCamShiftのデモと同様に
マウスのドラッグで追跡対象を指定できるようになってます
キーボードの'b'を押すと逆投影画像に表示が切り替わります

MeanShiftTracker Demo
最初は追跡に成功していますが
途中でウィンドウが背景の方に外れてしまいました

おわりに

以前の記事ではOpenCVのサンプルの紹介で終わってしまったので
今回は理解のために自分で中身を実装してみました