読者です 読者をやめる 読者になる 読者になる

おぺんcv

画像処理エンジニアのブログ

3次元のcv::Matを作る

OpenCV

はじめに

小ネタです
cv::Matが多次元に対応しているのを知ったのでメモ

作り方

多次元Matのコンストラクタがこちら(mat.hppより抜粋)
ndimsで次元数を、sizesで各次元の長さを、typeで要素の型を指定します

    /** @overload
    @param ndims Array dimensionality.
    @param sizes Array of integers specifying an n-dimensional array shape.
    @param type Array type. Use CV_8UC1, ..., CV_64FC4 to create 1-4 channel matrices, or
    CV_8UC(n), ..., CV_64FC(n) to create multi-channel (up to CV_CN_MAX channels) matrices.
    */
    Mat(int ndims, const int* sizes, int type);

サンプル

2×3×4のint行列を作ってみました

#include <opencv2/opencv.hpp>
#include <iostream>

int main()
{
	const int ni = 2, nj = 3, nk = 4;
	const int ndims = 3;
	const int sizes[ndims] = { ni, nj, nk };

	cv::Mat m(ndims, sizes, CV_32S);
	
	for (int i = 0; i < ni; i++)
		for (int j = 0; j < nj; j++)
			for (int k = 0; k < nk; k++)
				m.at<int>(i, j, k) = i + j + k;

	for (int i = 0; i < ni; i++)
		for (int j = 0; j < nj; j++)
			for (int k = 0; k < nk; k++)
				std::cout << i << "+" << j << "+" << k << " = " << m.at<int>(i, j, k) << std::endl;

	// std::cout << m << std::endl; // OpenCV Error: Assertion failed (m.dims <= 2)

	return 0;
}

(i, j, k)で要素にアクセスできるのが素晴らしいです
ちなみに、coutで表示しようとしたらassertに引っかかりました
dims <= 2までしか表示できないようです

おわりに

今までcv::Matは2次元しか対応してないと勝手に思っていて
3次元のデータを扱う際はcv::Matではなく生ポインタつかってました
早く気付いていれば良かったと思う今日この頃…

kd-treeを実装してみた

画像処理

はじめに

kd-treeを実装してみました
最近仕事でよく使うので勉強がてら

kd-treeとは

最近傍探索を効率的にできるデータ構造です
kd木 - Wikipedia

kd-treeが使えるライブラリとしてはFLANNPCLが有名どころでしょうか

ソースコード

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

kd-treeの構築と、以下の探索機能を実装してます

  • 最近傍探索 (Nearest neighbor search)
  • k-最近傍探索 (K-nearest neighbor search)
  • 半径内に含まれる近傍の探索 (Radius search)

あと、ヘッダ1個includeするだけで使えるのでお手軽です

アルゴリズム

kd-treeの構築

wikipediaに載ってる疑似コードが分かりやすかったので引用させていただきます

function kdtree (list of points pointList, int depth)
{
    if pointList is empty
        return nil;
    else
    {
        // 深さに応じて軸を選択し、軸が順次選択されるようにする
        var int axis := depth mod k;

        // 点のリストをソートし、中央値の点を選択する
        select median from pointList;

        // ノードを作成し、部分木を構築する
        var tree_node node;
        node.location := median;
        node.leftChild := kdtree(points in pointList before median, depth+1);
        node.rightChild := kdtree(points in pointList after median, depth+1);
        return node;
    }
}

一方私の書いたコードはこんな↓感じです

Node* buildRecursive(int* indices, int npoints, int depth)
{
	if (npoints <= 0)
		return nullptr;

	const int axis = depth % PointT::DIM;
	const int mid = (npoints - 1) / 2;

	std::nth_element(indices, indices + mid, indices + npoints, [&](int lhs, int rhs)
	{
		return points_[lhs][axis] < points_[rhs][axis];
	});

	Node* node = new Node();
	node->idx = indices[mid];
	node->axis = axis;

	node->next[0] = buildRecursive(indices, mid, depth + 1);
	node->next[1] = buildRecursive(indices + mid + 1, npoints - mid - 1, depth + 1);

	return node;
}

疑似コードとほとんど同じです
私の場合は点そのものではなく、点へのインデックスをノードに保持しています
(中央値を取得するためにnth_elementを初めて使った…)

最近傍探索

k-d treeが構築できたらlet's最近傍探索
ググってヒットしたこちらの資料をもとに実装しました
(URLを見るとスタンフォード大の宿題らしい)
https://web.stanford.edu/class/cs106l/handouts/assignment-3-kdtree.pdf

原理は2分探索と同じで、「探している値は木の半分のこっち側にあるよね」
ってのを繰り返しながら左か右の子ノードに降りていきます
その過程で現在ノードとクエリとの距離を計算し最近傍の値を更新します
葉までたどり着いたら、自分の兄弟ノード(親から見て自分が左だったら、右のノード)の方に
最近傍の可能性がないかチェックして、ある場合は兄弟ノードの方も探索します

void nnSearchRecursive(const PointT& query, const Node* node, int *guess, double *minDist) const
{
	if (node == nullptr)
		return;

	const PointT& train = points_[node->idx];

	const double dist = distance(query, train);
	if (dist < *minDist)
	{
		*minDist = dist;
		*guess = node->idx;
	}

	const int axis = node->axis;
	const int dir = query[axis] < train[axis] ? 0 : 1;
	nnSearchRecursive(query, node->next[dir], guess, minDist);

	const double diff = fabs(query[axis] - train[axis]);
	if (diff < *minDist)
		nnSearchRecursive(query, node->next[!dir], guess, minDist);
}

K-近傍探索や半径探索も探索の流れは同じです

デモ

最近傍探索/k-最近傍探索/半径探索を実行した結果です
赤い点がクエリ(画像の中心座標)、青い点が近傍点になります

おわりに

kd-treeを実装してみました
実装に当たり、再帰関数をいっぱい書きました
こういう場合は再帰を使うとシンプルに書けるんだなーって思いました

ARM NEONの使い方 除算編

ARM

四則演算ラスト!
今回は除算編です

アーキテクチャによる違い

32bit ARM

32bit ARMには除算を直接行うSIMD命令がないため
a/bという除算を行う場合、まず逆数を計算するSIMD命令によってbの逆数を計算し
これをaと掛け算することによって除算を表現します

64bit ARM

64bit ARMには除算を行うSIMD命令が追加されています

逆数の計算

vrecpe[q]_f32(va)

32bit浮動小数ベクタvaの各要素の逆数の近似値を計算します
近似値なのでCPU(IEEE754)よりも精度が低いです

サンプル

32bit浮動小数ベクタva, vbの除算をva*(vbの逆数)という形でやってみます
vaとvbの各レーンを同じ値に設定しているので、除算後の値が1になるのが望ましい結果です

#include <stdio.h>
#include <stdint.h>
#include <arm_neon.h>

static void print_vector(float32x4_t value)
{
	float lane[4];
	vst1q_f32(lane, value);
	for (int i = 0; i < 4; i++)
		printf("lane[%d]: %f\n", i, lane[i]);
}

int main()
{
	float a[4] = { 1, 2, 3, 4 };
	float b[4] = { 1, 2, 3, 4 };

	float32x4_t va = vld1q_f32(a);
	float32x4_t vb = vld1q_f32(b);

	float32x4_t vrecipb = vrecpeq_f32(vb); // approximate reciprocal of vb
	float32x4_t vc = vmulq_f32(va, vrecipb);

	printf("va:\n");
	print_vector(va);

	printf("\nvb:\n");
	print_vector(vb);

	printf("\n1/vb:\n");
	print_vector(vrecipb);

	printf("\nva * (1/vb):\n");
	print_vector(vc);

	return 0;
}
実行結果
va:
lane[0]: 1.000000
lane[1]: 2.000000
lane[2]: 3.000000
lane[3]: 4.000000

vb:
lane[0]: 1.000000
lane[1]: 2.000000
lane[2]: 3.000000
lane[3]: 4.000000

1/vb:
lane[0]: 0.998047
lane[1]: 0.499023
lane[2]: 0.333008
lane[3]: 0.249512

va * (1/vb):
lane[0]: 0.998047
lane[1]: 0.998047
lane[2]: 0.999023
lane[3]: 0.998047

近似値なので結構精度低いなーという印象
まあ用途次第でしょうけど

ベクタ逆数のステップ

よくわからないタイトルですが
先ほどの逆数の精度を良くする方法です

vrecps[q]_f32(va)

ARM Information Centerの説明が以下です

VRECPS (ベクタ逆数のステップ)は、あるベクタの要素を対応する別のベクタの要素で乗算し、その結果を 2 から減算して、最終的な結果をデスティネーションベクタの要素に返します。

最初この説明を見たときはなんのこっちゃと思いましたが
ニュートン法で逆数の近似を求める際に使うようです

ある数 d とその逆数の現在の近似値 x_nがあるとき、ニュートン法によって
 x_{n+1} = x_n(2 - dx_n)
という反復を繰り返すことで近似値が 1/dに収束します
この (2 - dx_n)を計算してくれるのがvrecpsというわけ

サンプル

先ほどの逆数のサンプルにニュートン法の反復計算を追加してみました

#include <stdio.h>
#include <stdint.h>
#include <arm_neon.h>

static void print_vector(float32x4_t value)
{
	float lane[4];
	vst1q_f32(lane, value);
	for (int i = 0; i < 4; i++)
		printf("lane[%d]: %f\n", i, lane[i]);
}

int main()
{
	float a[4] = { 1, 2, 3, 4 };
	float b[4] = { 1, 2, 3, 4 };

	float32x4_t va = vld1q_f32(a);
	float32x4_t vb = vld1q_f32(b);

	float32x4_t vrecipb = vrecpeq_f32(vb); // approximate reciprocal of vb
	float32x4_t vc = vmulq_f32(va, vrecipb);

	printf("va:\n");
	print_vector(va);

	printf("\nvb:\n");
	print_vector(vb);

	printf("\n1/vb:\n");
	print_vector(vrecipb);

	printf("\nva * (1/vb):\n");
	print_vector(vc);

        //  Newton-Raphson iteration
	int steps = 3;
	for (int i = 0; i < steps; i++)
	{
		printf("\nNewton step: %d\n", i + 1);

		float32x4_t vtmp = vrecpsq_f32(vb, vrecipb);
		vrecipb = vmulq_f32(vrecipb, vtmp);
		vc = vmulq_f32(va, vrecipb);

		printf("\n1/vb:\n");
		print_vector(vrecipb);

		printf("\nva * (1/vb):\n");
		print_vector(vc);
	}

	return 0;
}
実行結果
va:
lane[0]: 1.000000
lane[1]: 2.000000
lane[2]: 3.000000
lane[3]: 4.000000

vb:
lane[0]: 1.000000
lane[1]: 2.000000
lane[2]: 3.000000
lane[3]: 4.000000

1/vb:
lane[0]: 0.998047
lane[1]: 0.499023
lane[2]: 0.333008
lane[3]: 0.249512

va * (1/vb):
lane[0]: 0.998047
lane[1]: 0.998047
lane[2]: 0.999023
lane[3]: 0.998047

Newton step: 1

1/vb:
lane[0]: 0.999996
lane[1]: 0.499998
lane[2]: 0.333333
lane[3]: 0.249999

va * (1/vb):
lane[0]: 0.999996
lane[1]: 0.999996
lane[2]: 0.999999
lane[3]: 0.999996

Newton step: 2

1/vb:
lane[0]: 1.000000
lane[1]: 0.500000
lane[2]: 0.333333
lane[3]: 0.250000

va * (1/vb):
lane[0]: 1.000000
lane[1]: 1.000000
lane[2]: 1.000000
lane[3]: 1.000000

Newton step: 3

1/vb:
lane[0]: 1.000000
lane[1]: 0.500000
lane[2]: 0.333333
lane[3]: 0.250000

va * (1/vb):
lane[0]: 1.000000
lane[1]: 1.000000
lane[2]: 1.000000
lane[3]: 1.000000

(定量的に評価したわけではありませんが)
ステップを進めると精度が良くなっていくのがわかります

除算 (64bit ARMのみ)

vdiv[q]_f32(va, vb)

32bit浮動小数ベクタvaとvbの除算を行います

サンプル

vdivq_f32を使用して32bit浮動小数ベクタva, vbの除算をやってみます
コンパイルする際は64bit用のコンパイラ(aarch64-linux-gnu-g++など)を使ってね

#include <stdio.h>
#include <stdint.h>
#include <arm_neon.h>

static void print_vector(float32x4_t value)
{
	float lane[4];
	vst1q_f32(lane, value);
	for (int i = 0; i < 4; i++)
		printf("lane[%d]: %f\n", i, lane[i]);
}

int main()
{
	float a[4] = { 1, 2, 3, 4 };
	float b[4] = { 1, 2, 3, 4 };

	float32x4_t va = vld1q_f32(a);
	float32x4_t vb = vld1q_f32(b);

	float32x4_t vc = vdivq_f32(va, vb);

	printf("va:\n");
	print_vector(va);

	printf("\nvb:\n");
	print_vector(vb);

	printf("\nva / vb:\n");
	print_vector(vc);

	return 0;
}
実行結果
va:
lane[0]: 1.000000
lane[1]: 2.000000
lane[2]: 3.000000
lane[3]: 4.000000

vb:
lane[0]: 1.000000
lane[1]: 2.000000
lane[2]: 3.000000
lane[3]: 4.000000

va / vb:
lane[0]: 1.000000
lane[1]: 1.000000
lane[2]: 1.000000
lane[3]: 1.000000

そのまま除算ができるとありがたいですね
また64bit ARM(ARMv8)では浮動小数SIMD演算がIEEE754準拠になったため
結果もCPUと変わらないはずです

参考

NEONの32bitと64bitの違いが解説されてます
今回の記事を書くにあたりとても参考になりました
community.arm.com

次回

次回は初期化編です
ARM NEONの使い方 初期化編

Stixel Computationを実装してみた

画像処理 車載

6D-Visionについて調べてみたで紹介したStixelですが
ちまちまと実装を重ね、ようやくひと段落したので公開することにしました

デモ

物体上に出てる棒のようなものがStixelです
入力の視差はOpenCVのStereo SGBMで計算しています

youtu.be


youtu.be

使い方

README.mdに記載しましたので
興味があれば使ってやって下さい

アルゴリズム解説

執筆中…!

参考文献

  • [1] D. Pfeiffer, U. Franke: “Efficient Representation of Traffic Scenes by means of Dynamic Stixels”, IEEE Intelligent Vehicles Symposium IV 2010, San Diego, CA, Juni 2010
  • [2] H. Badino, U. Franke, and D. Pfeiffer, “The stixel world - a compact medium level representation of the 3d-world,” in DAGM Symposium, (Jena, Germany), September 2009.

ARM NEONの使い方 乗算編

ARM

今回は乗算編です
特に変わった内容はありません

乗算

vmul[q]_<type>(va, vb)

64bit(qが付く場合は128bit)のベクタvaとvbの乗算

サンプル

符号付き16bit整数ベクタの乗算をしてみます

#include <stdio.h>
#include <stdint.h>
#include <arm_neon.h>

int main()
{
	int16_t a[4] = { 1, 2, 3, 4 };
	int16_t b[4] = { 5, 6, 7, 8 };

	int16x4_t va = vld1_s16(a);
	int16x4_t vb = vld1_s16(b);

	int16x4_t vc = vmul_s16(va, vb);

	int16_t c[4];
	vst1_s16(c, vc);
	for (int i = 0; i < 4; i++)
		printf("c[%d]: %d\n", i, c[i]);

	return 0;
}
実行結果
c[0]: 5
c[1]: 12
c[2]: 21
c[3]: 32

整数を使っていて桁あふれを考慮する場合はvmull(Vector long multiply)を使いましょう

積和演算

みんな大好き(?)積和演算
内積とか計算するときに重宝しますね

vmla[q]_<type>(va, vb, vc)

64bit(qが付く場合は128bit)のベクタva、vb、vcについて

va + vb * vc

を計算します

サンプル

符号付き16bit整数のベクタの積和演算です

#include <stdio.h>
#include <stdint.h>
#include <arm_neon.h>

int main()
{
	int16_t a[4] = { 1, 1, 1, 1 };
	int16_t b[4] = { 2, 2, 2, 2 };
	int16_t c[4] = { 3, 3, 3, 3 };

	int16x4_t va = vld1_s16(a);
	int16x4_t vb = vld1_s16(b);
	int16x4_t vc = vld1_s16(c);

	int16x4_t vd = vmla_s16(va, vb, vc);

	int16_t d[4];
	vst1_s16(d, vd);
	for (int i = 0; i < 4; i++)
		printf("d[%d]: %d\n", i, d[i]);

	return 0;
}
実行結果
d[0]: 7
d[1]: 7
d[2]: 7
d[3]: 7

doubling multiplyってなんぞや

乗算に関しては先ほどのmulとmlaを使うことがほとんどだと思いますが
命令一覧を見ると doubling multiply というのがあったので使ってみました

サンプル

vqdmull(Vector saturating doubling long multiply)を使って乗算をしてみると…

#include <stdio.h>
#include <stdint.h>
#include <arm_neon.h>

int main()
{
	int16_t a[4] = { 1, 2, 3, 4 };
	int16_t b[4] = { 5, 6, 7, 8 };

	int16x4_t va = vld1_s16(a);
	int16x4_t vb = vld1_s16(b);

	int32x4_t vc = vqdmull_s16(va, vb);

	int32_t c[4];
	vst1q_s32(c, vc);
	for (int i = 0; i < 4; i++)
		printf("c[%d]: %d\n", i, c[i]);

	return 0;
}
実行結果
c[0]: 10
c[1]: 24
c[2]: 42
c[3]: 64

結果が2倍になった
どうやら乗算してさらに2倍する演算のようです
使い道はあるのかな…

次回

次回は除算編です
ARM NEONの使い方 除算編

ARM NEONの使い方 減算編

ARM

年内に終わるかな?
今回は減算編です

減算 (通常の減算、符号拡張付き減算、飽和付き減算)

加算編で紹介したものとほぼ変わらないので、まとめて紹介

vsub[q]_<type>(va, vb)

64bit(qが付く場合は128bit)のベクタvaとvbの引き算

vsubl_<type>(va, vb)

64bitのベクタvaとvbの各レーンのbit幅を倍に拡張して引き算

vsubq[q]_<type>(va, vb)

64bit(qが付く場合は128bit)のベクタvaとvbの引き算
演算結果がオーバーフローする場合は最大値/最小値で飽和させます

サンプル

符号付き16bit整数のベクタvaとvbの引き算を、先ほど紹介した3つの方法でやってみます

#include <stdio.h>
#include <stdint.h>
#include <arm_neon.h>

int main()
{
	int16_t a[4] = { 1, -1, 1, -2 };
	int16_t b[4] = { 1, -1, -32767, 32767 };

	int16x4_t va = vld1_s16(a);
	int16x4_t vb = vld1_s16(b);

	int16x4_t vc = vsub_s16(va, vb);
	int32x4_t vcl = vsubl_s16(va, vb);
	int16x4_t vcq = vqsub_s16(va, vb);

	printf("vsub_s16\n");
	int16_t c[4];
	vst1_s16(c, vc);
	for (int i = 0; i < 4; i++)
		printf("c[%d]: %d\n", i, c[i]);

	printf("\nvsubl_s16\n");
	int32_t cl[4];
	vst1q_s32(cl, vcl);
	for (int i = 0; i < 4; i++)
		printf("cl[%d]: %d\n", i, cl[i]);

	printf("\nvqsub_s16\n");
	int16_t cq[4];
	vst1_s16(cq, vcq);
	for (int i = 0; i < 4; i++)
		printf("cq[%d]: %d\n", i, cq[i]);


	return 0;
}
実行結果
vsub_s16
c[0]: 0
c[1]: 0
c[2]: -32768
c[3]: 32767

vsubl_s16
cl[0]: 0
cl[1]: 0
cl[2]: 32768
cl[3]: -32769

vqsub_s16
cq[0]: 0
cq[1]: 0
cq[2]: 32767
cq[3]: -32768

3、4番目のレーンの引き算の結果がそれぞれ

  • vsub_s16()ではオーバーフロー
  • vsubl_s16()では32bitに拡張
  • vqsub_s16()では最大値/最小値で飽和

となっています

符号なしベクタの減算について

符号なし16bit整数のベクタvaとvbの減算を考えます
このときvsub_u16()を使ってしまうと、戻り値も符号なし16bitなので
va[i] >= vb[i]の場合は正しい結果が得られるのですが
va[i] < vb[i]の場合はオーバーフローが発生してしまいます

正しい結果を得るためには、ベクタを符号付き32bitに拡張する必要がありますが、
これを1回で実現してくれるNEON命令はなさそうです

そこで1つ思いついたのが、vsubl_u16()を使用して減算結果を符号なし32bitに拡張し
vreinterpretq_s32_u32()で符号付き32bitとして解釈する方法です

サンプル

符号なし16bit整数のベクタvaとvbの引き算を、vsub_u16()を使った方法と
vsubl_u16() & vreinterpretq_s32_u32()を使った方法でやってみます

#include <stdio.h>
#include <stdint.h>
#include <arm_neon.h>

int main()
{
	uint16_t a[4] = { 1, 1, 1, 1 };
	uint16_t b[4] = { 0, 1, 2, 65535 };

	uint16x4_t va = vld1_u16(a);
	uint16x4_t vb = vld1_u16(b);
	uint16x4_t vc = vsub_u16(va, vb);

	uint32x4_t vc_u32 = vsubl_u16(va, vb);
	int32x4_t vc_s32 = vreinterpretq_s32_u32(vc_u32);

	printf("vsub_u16\n");
	uint16_t c[4];
	vst1_u16(c, vc);
	for (int i = 0; i < 4; i++)
		printf("c[%d]: %d\n", i, c[i]);

	printf("\nvsubl_u16 and vreinterpretq_s32_u32\n");
	int c_s32[4];
	vst1q_s32(c_s32, vc_s32);
	for (int i = 0; i < 4; i++)
		printf("c_s32[%d]: %d\n", i, c_s32[i]);

	return 0;
}
vsub_u16
c[0]: 1
c[1]: 0
c[2]: 65535
c[3]: 2

vsubl_u16 and vreinterpretq_s32_u32
c_s32[0]: 1
c_s32[1]: 0
c_s32[2]: -1
c_s32[3]: -65534

vsubl_u16() & vreinterpretq_s32_u32()を使った方法で、一応正しい結果が得られました
やり方として良いのかわかりませんが…

次回

次回は乗算編です
ARM NEONの使い方 乗算編

Free Space Computationを実装してみた

画像処理 車載

以前の記事で紹介したFree Space Computationですが
実装がひと段落したので公開することにしました

デモ

free space(走行可能領域)の推定結果を赤で塗ってます
まあなんとなく計算できてるような…?
なお、実際の処理時間は動画ほど速くありません

youtu.be

youtu.be

使い方

README.mdに記載しましたので
興味があれば使ってやって下さい

その他メモ

アルゴリズム

以前の記事で解説していますのでそちらをご参照ください
主に[1]の「B. Image Based Free Space Computation」を参考に実装しています

Road Disparity

[1]では路面上の視差をB-Splineでフィッティングして求めていますが
カメラパラメータから路面上の視差を計算する方法もあります([2]で知りました)
今回は計算が簡単なカメラパラメータを使う方法を採用しました

DPによる境界の計算

ここは割と推測で実装してる部分がありまして、あまり自身はありません
あと計算時間の都合上、かなり処理を端折ってる部分があります…