おぺんcv

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

OpenCVのサンプルコードでEnetを動かしてみた

はじめに

OpenCVのサンプルコードを使って、Enet(Semantic Segmentationのモデル)を試してみました

f:id:mizunashi:20181210195823p:plain

Enetとは

[1]で提案されている、Semantic Segmentationのネットワークです
リアルタイム向けに設計されており、SegNet等の従来のネットワークよりも演算量やパラメータ数が少なく、高速なのが特徴です
既に色々なフレームワークで実装されていますが、OpenCVではバージョン3.3から、dnnモジュールが追加されたのに伴い、 Enetの学習済みモデルとサンプルコードが提供されたようです

[1] ENet: A Deep Neural Network Architecture for Real-Time Semantic Segmentation

ビルド

BUILD_EXAMPLES=ONを指定してOpenCVをビルドすれば使用することができます
opencv/samples/dnnからsegmentation.cppとcommon.hppをもってきて自前でビルドするも良し

学習済みモデルのダウンロード

opencv_extra/testdata/dnn/download_models.pyを叩くとダウンロードできます
ただしそのまま叩くとdnnサンプルに含まれるすべてのモデルがダウンロードされるので、Enetだけ欲しい場合はスクリプトを修正すると良いでしょう
ちなみに、Cityscapes Dataset+Torchで学習したもののようです

実行

実行コマンドの例を示します

./example_dnn_segmentation enet --input=input.avi --zoo=models.yml --model=Enet-model-best.net --classes=enet-classes.txt --colors=colors.txt --target=1

以下、オプションと設定時の動作について解説します

  • enet

    • モデル名を指定します
    • モデル名に対応する設定をmodels.ymlから読み取ります
  • input=input.avi

    • 入力ファイルを指定します
    • cv::VideoCaptureを使用しており、--input=image_%03d.pngのように連番画像の指定も可能です(cv::VideoCaptureが連番対応してることを書いてて知った!)
  • --zoo=models.yml

    • models.ymlのパスを指定します
    • サンプルコードと同じopencv/samples/dnnにあります
    • 指定しない場合はカレントディレクトリのmodels.ymlを探します
  • --model=Enet-model-best.net

    • 学習済みモデルのパスを指定します
    • 指定しない場合は以下の環境変数からEnet-model-best.netを探します
      • OPENCV_DNN_TEST_DATA_PATH
      • OPENCV_TEST_DATA_PATH
  • --classes=enet-classes.txt

    • 分類クラスの定義ファイルを指定します
    • opencv/samples/data/dnn/enet-classes.txtから入手可能です
    • 指定しなくても動きますが、指定すると以下のようにクラス名と色付けの対応を表示してくれます

f:id:mizunashi:20181210094517p:plain
legend

  • --colors=colors.txt
    • 各クラスの色付けを指定します
    • 指定しない場合はランダムで色付けされます
    • TimoSaemann/ENetのものと同じ色設定を作成したのでよかったらお使い下さい
0 0 0
128 64 128
244 35 232
70 70 70
102 102 156
190 153 153
153 153 153
250 170 30
220 220 0
107 142 35
152 251 152
70 130 180
220 20 60
255 0 0
0 0 142
0 0 70
0 60 100
0 80 100
0 0 230
119 11 32
  • --target=1
    • 実行するデバイスを指定します
    • デフォルト値は0で、ターゲットはCPUです
    • --target=1ではOpenCLが使用されるので、GPUを積んでる人はこちらを指定したほうが高速に実行できると思います

結果

KITTIデータセットで実行した結果を動画にしました
上半分がネットワークへの入力解像度512x256の結果、下半分が1024x256の結果です
1024x256の方が若干精度が良いように見えます

処理時間は入力解像度512x256の場合、--target=0(CPU)で約80[msec]、--target=1(OpenCL)で約20[msec]でした…速い!
1024x256にするとOpenCLで2倍の約40[msec]かかりましたが、それでも十分速い印象です

おわりに

OpenCVでEnetを動かしてみました
趣味で作りたいプログラムにSemantic Segmentation(特に車載向け)を使うものがあり、以前からEnetには注目してたのですが、慣れ親しんだOpenCVで使えるようになって感激です!

継承を用いたPimplのようなこと

はじめに

C++における実装の隠蔽やインクルード依存削減についての話です
おそらく過去に何度も議論されていて何番煎じのネタかわかりませんが、自分にとっての気づきだったので書いておきます

Pimplイディオムといえば…

クラスに「Impl」という実装担当クラスのポインタを保持し、諸々の処理をImplクラスに移譲することにより、

  • 内部実装の隠蔽
  • ヘッダ側への余計なインクルード回避

を実現するテクニックとして知られています

////////////////////////////////////////////////
// Hoge.h
////////////////////////////////////////////////
class Hoge
{
public:
  Hoge();
  ~Hoge();
  void method1();
  void method2();
private:
  class Impl;
  std::unique_ptr<Impl> impl_;
};

////////////////////////////////////////////////
// Hoge.cpp
////////////////////////////////////////////////

// 実装クラス
class Hoge::Impl
{
  void method1()
  {
    ...
  }

  void method2()
  {
    ...
  }
};

Hoge::Hoge()
{
  impl_ = std::make_unique<Hoge::Impl>();
}

Hoge::~Hoge()
{
}

//  Hoge::Implに処理を移譲
void Hoge::method1()
{
  impl_->method1();
}

void Hoge::method2()
{
  impl_->method2();
}

この方法を知った当初は「実装隠蔽だなんてそんな潔癖にならなくても…」と思ってたのですが、最近はようやく意義をわかりかけてきました

クラスの規模が開発につれ段々と大きくなってくると、メンバ変数を増やしたり、それに伴う新たなインクルードを追加する必要が出てくると思いますが、それはつまりヘッダを修正することになるので、そのヘッダをインクルードしているほかのファイルも再コンパイルしなければなりません

Pimplを使えばヘッダ側を修正しなくて済むのでこのような問題を回避できますね!

ただし、面倒な点があるとすれば、上記のようなImplへのたらい回しコードをmethodが増えるたびに書かなければいけないことです

継承を用いた方法

これもPimplと同じ効果が得られ、アリかな思う方法です

////////////////////////////////////////////////
// Hoge.h
////////////////////////////////////////////////
class Hoge
{
public:
  std::unique_ptr<Hoge> create();
  virtual void method1() = 0;
  virtual void method2() = 0;
};

////////////////////////////////////////////////
// Hoge.cpp
////////////////////////////////////////////////

class HogeImpl : public Hoge
{
  void method1() override
  {
    ...
  }

  void method2() override
  {
    ...
  }
};

std::unique_ptr<Hoge> Hoge::create()
{
  return std::make_unique<HogeImpl>();
}

Hogeは抽象クラスになっているのでインスタンス化はできませんが、ユーザはcreate()を通じてHogeのポインタを受け取ることができます

このときcreate()側ではHogeを継承したHogeImplのポインタを返してしまおうという手です
メリットとしては

  • たらい回しコードを書かなくてよい
  • ヘッダ側にImplを宣言する必要もない

といったところでしょうか、一方デメリットとしては

  • 本来の継承の使い方ではない(is-a関係を表すものではない)
  • 仮想関数を使用することによるコスト
    • まあクリティカルな部分でこんな方法使わないと思うけど…

などが考えられます

おわりに

継承を使ったやり方を知ったのはOpenCVを使ってたのがきっかけで、OpenCVのクラスにはインスタンスをポインタ経由でしか生成できないものがあります
例えばこんな感じです

cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create();
sgbm->compute(...);

このクラスのヘッダを見るとpublic関数の宣言だけで実装に関する記述が無く、実装を見ると前述のコードのようになっていて「これってPimplと似てるな~」と思った次第です

お断りしておくと、OpenCVのStereoSGBMクラスはStereoMatcherという抽象的なステレオマッチングクラスを継承しており、create()はFactory Method的に使っているのであって、本来の継承の使い方をしています

以上、Pimplについて思うことを書いてみました

近況

はじめに

この1年は仕事に没頭してました
去年の4月に初めてPLを務めた案件があって、ようやく一区切りついたところです
ブログを書く気力はなかったのですが、趣味でコードは書いてました(笑)
そこで今回はこの1年で更新or新規に書いたコードを簡単に紹介します
余力があれば個別に記事にしたいと思います…

目次

作ったもの紹介

stixel-world

github.com
視差画像から障害物を検出するアルゴリズム
以前は路面の視差モデルをカメラのピッチとカメラの高さを用いて計算していたのですが
直線フィッティングを用いて視差画像から自動で推定する機能を追加しました
これによって

  • カメラのピッチとカメラの高さがわからない
  • カメラのピッチが移動中に変動する

という状況にも対応できるようになりました
ただ、依然として路面モデルは平面を仮定してるので、曲面には対応できてません
今後曲面モデルにも対応したいですね
自動運転への注目が高まっているのか、外国の研究者の方からの質問がちょくちょくきます
まあ私は論文を読んで「こうかな?」と思って実装しただけなので、
答えられることにも限界があるのですが…

multilayer-stixel-world

github.com
画像の1列から複数の障害物を検出できるstixel-worldの進化版
こちらも視差モデルの自動推定機能を追加しました

semi-global-matching

github.com
有名な視差計算のアルゴリズム
OpenCVではcv::stereo::StereoBinarySGBMで提供されてます
OpenCV版より速いものを作ることをモチベにして実装してたのですが
かなり巧みに実装されていて、シングルスレッドでは勝てない、というか
OpenCVと同じ実装になってしまうので諦めました
そこで本作はマルチスレッドでOpenCVに勝てるように実装されています(笑)
例えば1024x333サイズの画像だと100msecくらい速いです

coarse-to-fine-patchmatch

github.com Denseなオプティカルフローを計算するアルゴリズムです
KITTIのベンチマークで40位前後の精度、かつCPU1コアで3秒くらいなので
高速化すれば実用的になるのでは、と思い興味を持ってます
実装は著者によって公開されていて、本作はOpenCVから呼び出しやすいように
再実装したものになります
patchmatch自体が面白い手法なのでどこかで紹介したいなあ…

CU-DAISY

github.com DAISYという特徴量計算をCUDAで実装したものです
前述のcoarse-to-fine-patchmatchがマッチングの特徴量としてこのDIASYを使用しており
そこそこ処理時間を食っていたのでCUDA化するに至りました
OpenCVのCPU実装より20倍くらい速いです

おわりに

とりあえず今回はこんなところで
見ての通り(?)論文読んで実装したり高速化したりするのが趣味なので
何か面白そうなネタがあったらコメントやメール下さい

Visual Studio 2015でHalideを始める

Halideという画像処理向けのプログラミング言語を始めるべく
Visual Studio 2015でHalideの環境構築をしてみました
今回はその手順について紹介します

はじめに

HalideとはMITが開発している画像処理向けの言語です
公式ページやブログなどの紹介によると以下の特徴があるようです

  • C++内で使用するDSL(domain-specific language)である
  • 画像処理を簡単に記述できる
  • 色んなターゲット(x86、ARM、CUDA等)向けに高速化できる

簡単に処理を記述できて、尚且つ一度書いたコードを色んなターゲットで
高速に動かせるとしたら、とても魅力的ですね
リポジトリのREADMEによるとWindows(VS2015)にも対応しているようなので、
試しに導入してみることにしました

参考:

やること

  • Visual Studio 2015でHalideをビルドする
  • Halidを使った簡単なプログラムをビルド・実行する

事前に必要なもの

  • Visual Studio 2015 Update 3 以降
  • CMake
  • libjpegとlibpng (画像ファイルを読み書きするtutorialを動かす場合)

また、コマンドラインでの作業が主体になるので
お好きなコンソールエミュレータを立ち上げておきましょう

ソースの入手

Halid

GitHubからHalidのリポジトリをクローンします
ここではD:\halideというディレクトリにソースを置くことにします

mkdir D:\halide
cd D:\halide
git clone https://github.com/halide/Halide.git
LLVMとClang

HalidのビルドにはLLVMとClangが必要になります
LLVM Download PageからLLVM source codeとClang source codeとダウンロードします
2017年4月22日の時点で最新バージョンは4.0.0でした
解凍してHalidのソースと同じディレクトリに置きます

$pwd
D:\halide

$ls
Halide/  cfe-4.0.0.src/  llvm-4.0.0.src/

ビルド

LLVMのビルド

ソースと同じ場所にllvm-buildというディレクトリを作成しビルドを行います
まずはcmake

$mkdir D:\halide\llvm-build
$cd D:\halide\llvm-build
$cmake -DCMAKE_INSTALL_PREFIX=../llvm-install -DLLVM_ENABLE_TERMINFO=OFF -DLLVM_TARGETS_TO_BUILD=X86;ARM;NVPTX;AArch64;Mips;Hexagon -DLLVM_ENABLE_ASSERTIONS=ON -DLLVM_BUILD_32_BITS=OFF -DCMAKE_BUILD_TYPE=Release ../llvm-4.0.0.src -G "Visual Studio 14 Win64"

次にビルド

MSBuild.exe /m /t:Build /p:Configuration=Release .\INSTALL.vcxproj

私の場合、MSBuild.exeへのパスが通ってなかったので環境変数の設定から
以下のディレクトリをPathに追加しました

C:Program Files (x86)\MSBuild\14.0\Bin

ビルドが終わるまでしばらく待ちます
CPU使用率が100%になってイイ感じ(?)です

Clangのビルド

LLVM同様、cmakeとビルドを行います

$mkdir D:\halide\clang-build
$cd D:\halide\clang-build
$cmake -DCMAKE_INSTALL_PREFIX=../llvm-install -DLLVM_ENABLE_TERMINFO=OFF -DLLVM_TARGETS_TO_BUILD=X86;ARM;NVPTX;AArch64;Mips;Hexagon -DLLVM_ENABLE_ASSERTIONS=ON -DLLVM_BUILD_32_BITS=OFF -DCMAKE_BUILD_TYPE=Release ../cfe-4.0.0.src -G "Visual Studio 14 Win64"
MSBuild.exe /m /t:Build /p:Configuration=Release .\INSTALL.vcxproj
Halideのビルド

最後にHalideをビルドします

$mkdir D:\halide\halide-build
$cd D:\halide\halide-build
$cmake -DLLVM_DIR=../llvm-install/lib/cmake/llvm -DLLVM_VERSION=37 -DCMAKE_BUILD_TYPE=Release -G "Visual Studio 14 Win64" ../halide
MSBuild.exe /m /t:Build /p:Configuration=Release .\ALL_BUILD.vcxproj

ビルドが完了するとhalide-build以下にHalideがインストールされています
私の場合、無事ビルド完了…と思いきやtutorialコードのビルドで失敗してしまいました
それについては余談で触れたいと思います

Halideのプロジェクトを作成してみる

ビルドできたらtutorialを実行するも良し、ですが
ここでは自分でHalideのプロジェクトを作成してみましょう
プロジェクト用ディレクトリを作成し、CMakeLists.txtとソースコードを置きます

halide_sample
   |- CMakeLists.txt
   |- main.cpp
CMake

CMakeLists.txtの中身はこんな感じです
Halide_DIRにHalideのインストール先を設定し
そこからヘッダとライブラリのパスを設定します
Halide.libもリンクしておきます

cmake_minimum_required(VERSION 2.8)

set(Halide_DIR D:/halide/halide-build)

include_directories(${Halide_DIR}/include)
link_directories(${Halide_DIR}/lib/Release)

file(GLOB srcs ./*.cpp ./*.h*)
add_executable(sample ${srcs})

target_link_libraries(sample Halide)
ソース

main.cppの中身はこんな感じです
内容はtutorialのlesson_01_basics.cppから引っ張ってきました
Halide::FuncとHalide::Varで関数を記述し、realizeで呼び出すって感じでしょうか

#include <Halide.h>
#include <iostream>

int main()
{
	Halide::Func gradient;
	Halide::Var x, y;
	gradient(x, y) = x + y;
	Halide::Buffer<int32_t> output = gradient.realize(3, 3);
	
	for (int i = 0; i < output.height(); i++)
	{
		for (int j = 0; j < output.width(); j++)
		{
			std::cout << output(i, j) << " ";
		}
		std::cout << std::endl;
	}

	return 0;
}
プロジェクトの作成とビルド

cmakeを使ってプロジェクトの作成とビルドを行います

$mkdir build
$cd build
$cmake .. -G "Visual Studio 14 Win64"
cmake --build . --config Release
パスの設定

実行にはHalide.dllが必要になりますので
環境変数のPathにdllの場所を指定しておきましょう

D:\halide\halide-build\bin\Release
実行結果

ちゃんと (x, y) = x + y という結果になってました

0 1 2
1 2 3
2 3 4

おわりに

今回はVisual Studio 2015でHalideの環境構築するところまでを紹介しました
今後はより複雑なアルゴリズムの記述や、高速化の方法について調べてみようと思います

【余談】tutorialのビルドにハマった話

私の場合、tutorialのビルド時に以下のようなビルドエラーに遭遇してしまいました

  • jpeg.libで未定義のシンボル__iob_funcが参照されている
  • jpeg.libで未定義のシンボルsprintfが参照されている
  • INT32が再定義されている

ビルドエラーは画像ファイルを読み書きするtutorial(lesson_02,07,09,12)で起きてました
Halide/tutorialのCMakeLists.txtを見ると、これらのサンプルはlibjpegとlibpngが入ってないと
ビルドされないはずなのですが、libjpegとlibpngなんて入れたっけなあ…
と思ってたら、どうやらAnaconda2(python環境)を入れた際に一緒に入ってたみたいです

__iob_funcの未定義

これについては、ググると回避方法が見つかりました
Halide/tools/halide_image_io.hで__iob_funcを定義してやります

FILE _iob[] = { *stdin, *stdout, *stderr };
extern "C" FILE * __cdecl __iob_func(void)
{
    return _iob;
}

参考:

sprintfの未定義

なんでsprintfが未定義なの?ってのが謎ですが
どっかに定義されてるだろってことで、extern宣言したら回避できました(適当)
これもhalide_image_io.hに書いてます

extern "C" int sprintf ( char * str, const char * format, ... );
NT32の再定義

どうやらINT32がlibjpgのjmorecfg.hと、Windows Kits\8.1\Include\shared\basetsd.hで
両方定義されちゃってるようです
jmorecfg.hをみると、INT32はQGLOBAL_Hが定義されていない場合に定義されるようです
そこで、halide_image_io.hでjpeglib.hをインクルードする前にQGLOBAL_Hを定義してやります

#ifndef HALIDE_NO_JPEG
#define QGLOBAL_H
#include "jpeglib.h"
#undef QGLOBAL_H
#endif

以上でビルドエラーを回避することができましたが、かなり適当なのでご参考までに

Stixel Computationを実装してみた multi-layer編

はじめに

以前Stixel Computationを実装してみたで紹介したStixelですが
今回はその発展形である、Multi-Layered Stixelを実装してみました

概要

Stixelとは、Daimlerが研究しているステレオカメラによる障害物検出の手法です
画像を幅5~10ピクセル程度の細長い棒に区切って、そこからさらに障害物の領域を検出します
以前紹介したStixel計算手法では、区切られた1つの列からは1つのStixel(最も手前側にあるもの)
しか検出できませんでしたが、Multi-Layered Stixelでは複数のStixelを検出可能になりました

デモ

物体上に出てる棒状のものがStixelです
以前のStixelとは違い、ガードレールの奥にいる車両のStixelも検出できています

www.youtube.com

アルゴリズム解説

ざっくり説明するとMulti-Layered Stixelの計算は
画像の各列を路面(ground)、物体(object)、空(sky)のいずれかのセグメントに分類する問題です
列を3つのセグメントに分割する組み合わせは色々と考えられますが、各セグメントに対し

  • Data項:セグメントがあるクラスに属しているとき期待される視差値と実際の視差値の誤差
  • Prior項:セグメントの割り当て方に関する先験情報

からなるコスト関数を定義し、これらの和が最小となる分割を求めます
最小化問題を効率的に解くために動的計画法が用いられます

さらなる詳細は[1]の3 The Multi-Layered Stixel Worldを参照下さい
数式の展開や実装方法など、かなり細かく解説してあります

おわりに

参考文献は著者の博士論文のようで、非常に読み応えがありました
なかなか内容が理解できず、3回くらいの挫折を経て何とか実装に至りました
また実装ができても当初は処理がめちゃめちゃ重く
inline化やテーブル化などの高速化を頑張りました

Stixel検出後の処理として、Stixelの追跡やグループ化などの処理があるようです
これらもいつか実装できればと思っています

3次元のcv::Matを作る

はじめに

小ネタです
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を実装してみました
実装に当たり、再帰関数をいっぱい書きました
こういう場合は再帰を使うとシンプルに書けるんだなーって思いました