3D Sensor Advent Calendar 2019の6日目の記事です。
はじめに
この記事では、六本木ヒルズの森美術館で2020年3月末まで開催中の「未来と芸術展」に出展中の「2025年大阪•関西万博誘致計画案」展示のために実装した、KinectV2+PCLによるヘッドトラッキングを、実際のコードを交えて紹介します。
展示内容と展示の技術構成について詳しくは以下の記事をご覧ください。（ステマじゃないよ！）
「2025年大阪•関西万博誘致計画案」森美術館展示を実現した技術〜センサー編｜PARTY｜note
C++初心者向けのTips
今回の実装を始めるまで、３DセンサープログラミングがおろかC++もやったことないような、ソフトウェアエンジニアです。いつもは、KotlinでAndroid開発したり、Pythonでバックエンドを開発したりしています。
そんなC++初心者が続いたポイントを、簡単に紹介しておきます。
CMakeGUIを使ってVisualStudioのソリューションファイルを生成する
KinectV2を制御するアプリとPCLの実装とにあたっては、3D Sensor Advent Calendar 2019を主催する@UnaNancyOwenさんのサンプルコードは欠かせません。マジで足を向けて寝られません。ありがとうございます。
UnaNancyOwen/Kinect2Sample: Kinect for Windows SDK v2 Sample Program
Drawing Point Cloud retrieve from Kinect v2 using Point Cloud Library – part.1 without Grabber – Summary?Blog
これらをVisualStudioでプロジェクトとして読み込んめる用にソリューションファイルを生成してくれるのがCMakeGUIです。
CMakeを使う方法は調べると色々出てきますが、GUIでやってしまうのが最も簡単です。
リンクするライブラリのDebug用とRelease用に注意
DebugでビルドできてたのにRealeaseでビルドできない、、みたいな初心者によくあるパターン。ここで初めてライブラリにはDebug用とRelease用があることを知る。
OpenCVのopencv_world411.lib（Release用）とopencv_world411d.lib（Debug用）は一緒にリンクしてても怒られないのに、PCLに必要なライブラリ群はRelease構成にRelease用と一緒にDebug用ライブラリも混ぜてリンクしてるとビルドが通らないことがある。その逆も然り。
PCLはRelease用には_release、Debug用には_debugが、libboostとVTKのDebug用には_gdが、ついているので、それぞれの構成でそれぞれのライブラリをリンク設定しましょう。
KinectV2 + PCL でヘッドトラッキング
前置きが長くなりました。ここからが本題です。まずは、今回の実装したプログラムの概要を簡単に紹介します。
- KinectSDKのBodyTrackingのJointで頭の位置を推定
- 現実の座標をAffine変換で仮想空間の座標に変換
- 設定した点からの距離でトラッキングする対象を決定
- 座標の移動をローパスフィルターで滑らかにする
- PCLで対象の取り逃しを検出する
- OSCで座標を送信
ひとつずつ、実際のコードを交えて紹介していきます。
KinectSDKのBodyTrackingのJointで頭の位置を推定
これはSDKについてくる機能をそのまま使いました。見切れたりすると首から下の精度は、あまり良くないですが、頭の位置だけであれば、一回捉えてしまうと意外と安定します。
Jointの取得は@UnaNancyOwenさんのサンプルコードをベースにしています。
Kinect2Sample/sample/JointSmooth at master · UnaNancyOwen/Kinect2Sample
Jointsから頭のJoint（JointType::JointType_Head）だけを取得しています。Trackingしなくても良い位置にいる場合は、それを無視します。
ここでは誰をトラッキングすべきかは決めず、設定した点からの距離だけを計算しています。後述する処理で、この距離をつかって一番距離が短い人をトラッキングの対象にします。
ERROR_CHECK(bodyFrame->GetAndRefreshBodyData(BODY_COUNT, &bodies[0]));
Concurrency::parallel_for(0, BODY_COUNT, [&](const int count) {
const Microsoft::WRL::ComPtr<IBody> body = bodies[count];
if (body == nullptr) {
return;
}
UINT64 id = -1;
ERROR_CHECK(body->get_TrackingId(&id));
BOOLEAN tracked = FALSE;
ERROR_CHECK(body->get_IsTracked(&tracked));
if (!tracked) {
return;
}
//リアルな頭位置を保持
std::array<Joint, JointType::JointType_Count> joints;
ERROR_CHECK(body->GetJoints(JointType::JointType_Count, &joints[0]));
realPositions[count] = sensor::Vector3::ZERO();
if (joints.size() > 0) {
Joint joint = joints[JointType::JointType_Head];
if (joint.TrackingState == TrackingState::TrackingState_Tracked) {
//座標を仮想位置に変換（後述）
sensor::Vector3 pos = transform(joint.Position.X, joint.Position.Y, joint.Position.Z);
//範囲を超えている人は除外
if (pos.x >= ranges.getMinWidth() && pos.x <= ranges.getMaxWidth() &&
pos.y >= ranges.getMinHeight() && pos.y <= ranges.getMaxHeight() &&
pos.z >= ranges.getMinDepth() && pos.z <= ranges.getMaxDepth()) {
realPositions[count] = pos;
}
}
}
//設定した点からの距離を配列に詰める(後述)
distances[count] = sensor::Distance(static_cast<int>(id), getDistance(realPositions[count]));
});
現実の座標をAffine変換で仮想空間の座標に変換
今回の展示では、トラッキングした座標を仮想空間上の座標に変換してあげる必要がありました。特に現実のセンサーは設置の都合上、上に見上げる形で設置してあるため、センサーから遠ざかるほど頭のY座標の値が小さくなってしまいます。
それをEigenを使ったAffine変換で調整し、仮想空間上の座標に変換します。
でらうま倶楽部 : Eigen - C++で使える線形代数ライブラリ
Eigen::Vector3f pre_pos(x, y, z);
//仮想空間上のKinect位置に平行移動
Eigen::Translation<float, 3> translation = Eigen::Translation<float, 3>(settings.getX(), settings.getY(), settings.getZ());
//現実のKinectの傾きを真っ直ぐに戻す
//getPitch(), getYaw(), getRoll()はKinectのそれぞれの角度（float）
float pitchRad = -settings.getPitch() / 180.0 * M_PI;
Eigen::Quaternionf rotateX(Eigen::AngleAxisf(pitchRad, Eigen::Vector3f::UnitX()));
float yawRad = -settings.getYaw() / 180.0 * M_PI;
Eigen::Quaternionf rotateY(Eigen::AngleAxisf(yawRad, Eigen::Vector3f::UnitY()));
float rollRad = -settings.getRoll() / 180.0 * M_PI;
Eigen::Quaternionf rotateZ(Eigen::AngleAxisf(rollRad, Eigen::Vector3f::UnitZ()));
//行列の計算なので計算順に注意
Eigen::Affine3f affine = translation * (rotateX * rotateY * rotateZ);
Eigen::Vector3f pos = affine * pre_pos;
Yaw, Roll, Pitchのどれがどの値かは以下画像がわかりやすいです。
設定した点からの距離でトラッキングする対象を決定
先程求めた距離をもとに、トラッキングする対象を決めます。ただし、設定した点近くに二人並んだ状態になると、人の微妙な揺れなどで対象が高速に切り替わり続けることがあります。
それを防止するために、対象が一定フレーム数を維持しないと実際には切り替わらない仕組みをいれました。
{
//スクリーン位置に一番近い人を検出
sensor::Distance nearestDist = sensor::Distance::NONE();
for (auto& dist : distances) {
if (nearestDist.isZero() && !dist.isZero()) {
nearestDist = dist;
} else if (!nearestDist.isZero() && !dist.isZero() && dist.val < nearestDist.val) {
nearestDist = dist;
}
};
//チラつき防止のため、同じ対象が一定フレーム数を維持した場合のみ、対象を変更する
if (trackingId == -1 || trackingId == nearestDist.id) {
//今誰もトラッキングしてないか、対象が変わってないので
//トラッキングを開始/継続する
trackingId = nearestDist.id;
nextId = -1;
delayCounter = 0;
}
else {
if (nextId == -1 && nearestDist.isZero()) {
//次の対象がいないので何もしない
delayCounter = 0;
}
else if (nextId != -1 && nextId != nearestDist.id) {
//次の対象が前フレームから更に変わったのでカウンターリセット
nextId = nearestDist.id;
delayCounter = 0;
}
else {
//次の対象を待機させる
nextId = nearestDist.id;
delayCounter++;
}
}
}
if (nextId != -1 && nextId != trackingId && delayCounter >= (settings.getTargetDelayFrameCount())) {
//次の対象が一定フレーム数を維持したので、新しくトラッキングを開始する
trackingId = nextId;
nextId = -1;
delayCounter = 0;
}
座標の移動をローパスフィルターで滑らかにする
Kinectのトラッキング精度や人の微妙な揺れによって、座標が移動がカクついたり、静止しているつもりでも座標が微妙に動いたりすることがあります。
そういったセンサーや人からくるブレやノイズをなくして滑らかな値の変化にするためには、デジタルフィルターを通します。今回はローパスフィルターを使っています。
ローパスフィルターの簡単な式に対する、絶大な効果は興奮に値します。
センサの入力などに使うディジタルフィルタ | なんでも独り言
float x = currentPos.isZero() ? settings.getZeroX() : currentPos.x;
float y = currentPos.isZero() ? settings.getZeroY() : currentPos.y;
float z = currentPos.isZero() ? settings.getZeroZ() : currentPos.z;
//ローパスフィルター
if (!positions.empty()) {
sensor::Vector3 pastPos = positions.front();
x = filterX * x + (1.0 - filterX) * pastPos.x;
y = filterY * y + (1.0 - filterY) * pastPos.y;
z = filterZ * z + (1.0 - filterZ) * pastPos.z;
}
sensor::Vector3 result(x, y, z);
positions.push_front(result);
if (positions.size() > 2) {
positions.pop_back();
}
PCLで対象の取り逃しを検出する
KinectSDKのBodyTrakingでは最大人数を超えると、新しい人が来ても（たとえ最前列でも！）新たに捉えてはくれません。今回は特に規制線などがない展示なので、センサーの前に多くの人が並ぶ可能性があります。そのため、トラッキングすべき人がKinectSDKのBodyTrackingでは捉えられていないことが発生します。
それをPCLの点群クラスタリングによる人の推定との比較で検知し、取り逃しをしている場合はKinectを再起動する方法を取りました。
PCLは必要なライブラリが多く、初期設定がややこしいですが、All-in-one Installerがあるので、それを使うと楽です。
Point Cloud Library 1.9.1 has been released – Summary?Blog
PCL部分の概要を簡単に説明すると以下のような感じです。
- PCLにKinectのDepthBufferを渡す
- 位置を調整と必要な点群以外の除去
- 点群のクラスタリングと人の推定
- KinectSDKが検出した座標がPCLで検出した人に含まれていることを検証
ちなみにこれらの処理は別スレッドで動かしています。これは、メインの座標検出のフレームレートを落とさないようにするためです。
PCLにKinectのDepthBufferを渡す
このあたりはやはり@UnaNancyOwenさんのサンプルコードを参考にしました。
Drawing the Point Cloud retrieved from Kinect v2 using Point Cloud Library without Grabber
std::vector<CameraSpacePoint> cameraSpacePoints(depthBuffer.size());
coordinateMapper->MapDepthFrameToCameraSpace(depthBuffer.size(), &depthBuffer[0], cameraSpacePoints.size(), &cameraSpacePoints[0]);
points->clear();
for (size_t i = 0; i < cameraSpacePoints.size(); i++)
{
pcl::PointXYZ point;
if (cameraSpacePoints[i].Z < 0)
{
continue;
}
point.x = cameraSpacePoints[i].X;
point.y = cameraSpacePoints[i].Y;
point.z = cameraSpacePoints[i].Z;
points->points.push_back(point);
}
位置を調整と必要な点群以外の除去
まずは位置の調整です。これは座標の変換でもやったAffine変換とほぼ同じです。
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>());
float pitchRad = settings.getPitch() / 180.0 * M_PI;
Eigen::Affine3f transformRotateX = Eigen::Affine3f::Identity();
transformRotateX.rotate(Eigen::AngleAxisf(pitchRad, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*points, *output, transformRotateX);
float yawRad = settings.getYaw() / 180.0 * M_PI;
Eigen::Affine3f transformRotateY = Eigen::Affine3f::Identity();
transformRotateY.rotate(Eigen::AngleAxisf(yawRad, Eigen::Vector3f::UnitY()));
pcl::transformPointCloud(*output, *output, transformRotateY);
float rollRad = settings.getRoll() / 180.0 * M_PI;
Eigen::Affine3f transformRotateZ = Eigen::Affine3f::Identity();
transformRotateZ.rotate(Eigen::AngleAxisf(rollRad, Eigen::Vector3f::UnitZ()));
pcl::transformPointCloud(*output, *output, transformRotateZ);
Eigen::Affine3f transformMove = Eigen::Affine3f::Identity();
transformMove.translation() << settings.getX(), settings.getY(), settings.getZ();
pcl::transformPointCloud(*output, *output, transformMove);
次に必要な点群以外の除去です。最初にPassTroughFilterでざっくり必要な範囲の点群だけを取り出します。
//有効範囲以外を除去
passThroughFilter(output, "x", ranges.getMinWidth(), ranges.getMaxWidth());
passThroughFilter(output, "y", ranges.getMinHeight(), ranges.getMaxHeight());
passThroughFilter(output, "z", ranges.getMinDepth(), ranges.getMaxDepth());
void PclManager::passThroughFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr input, const std::string& field, float min, float max)
{
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(input);
pass.setFilterFieldName(field);
pass.setFilterLimits(min, max);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>());
pass.filter(*filtered);
*input = *filtered;
}
クラスタリング処理を軽くするため、voxelGirdFilterで間引いて点群の数をさらに絞って、ノイズも除去します。
//間引く
voxelGridFilter(output, 0.02f);
//ノイズ除去
std::vector<int> index;
pcl::removeNaNFromPointCloud(*output, *output, index);
void PclManager::voxelGridFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr input, float leaf)
{
pcl::VoxelGrid<pcl::PointXYZ> grid;
grid.setLeafSize(leaf, leaf, leaf);
grid.setInputCloud(input);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>());
grid.filter(*filtered);
*input = *filtered;
}
点群のクラスタリングと人の推定
点群が整ったとところで、それをクラスタリングして点群の塊を判別します。今回の展示では障害物などはセンサーの範囲にはないので、一定の大きさの塊を人と推定します。
for (int i = 0; i < clusteredPoints.size(); i++)
{
clusteredPoints[i].reset();
}
clusteredPoints.clear();
if (points->size() <= 0) { return; }
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(points);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.07); // 2cm
ec.setMinClusterSize(50);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(points);
ec.extract(cluster_indices);
//分割されたインデックスを元に、クラウドを分割して返す
for (int i = 0; i < cluster_indices.size(); i++)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr p(new pcl::PointCloud<pcl::PointXYZ>());
for (int j = 0; j < cluster_indices[i].indices.size(); j++)
{
int index = cluster_indices[i].indices[j];
p->push_back(points->points[index]);
}
//小さい塊はKinectSDKでも捉えてない可能性が高いので無視
if (p->size() > settings.getMinPointCloud()) {
clusteredPoints.push_back(p);
}
}
KinectSDKが検出した座標がPCLで検出した人に含まれていることを検証
KinectSDKの取り逃しは、クラスタ化した点群の塊を囲む立方体（BoundingBox）を計算し、その中にKinectSDKで検出した座標があるかどうかで検証するようにしました。
また、１フレームだけの検出では取り逃しとは言えないので、ここでも一定フレーム取り逃しが推測されたらKinectの再起動フラグを立てる仕組みにしています。
pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud = nullptr;
float minDistance = 0.0;
//この点に最も近い人を検出する
Eigen::Vector3f screen(settings.getScreenX(), settings.getScreenY(), settings.getScreenZ());
for (auto& p : clusteredPoints) {
//クラスタ化した点群の重心点を取得
Eigen::Vector4f xyz_centroid;
pcl::compute3DCentroid(*p, xyz_centroid);
Eigen::Vector3f centroid(xyz_centroid.x(), xyz_centroid.y(), xyz_centroid.z());
float distance = (centroid - screen).norm();
//設定した点に最も近い点群を取得
if (minDistance <= 0.0 || distance < minDistance) {
minDistance = distance;
pointCloud = p;
}
}
if (pointCloud != nullptr) {
//クラスタ化した点群のBoundingBoxを取得
pcl::MomentOfInertiaEstimation<pcl::PointXYZ> moment;
moment.setInputCloud(pointCloud);
moment.compute();
pcl::PointXYZ min;
pcl::PointXYZ max;
bool detected = false;
if (moment.getAABB(min, max)) {
//BoundingBox内に、ひとつでも点が含まれていればOK
for (auto& pos : positions) {
if (pos.x >= min.x && pos.x <= max.x
&& pos.y >= min.y && pos.y <= max.y
&& pos.z >= min.z && pos.z <= max.z) {
detected = true;
break;
}
}
//BoundingBox内にひとつも点がないのでボーンが取れてない可能性がある
//再起動カウンターを進める
if (!detected) {
mutex.lock();
counter++;
mutex.unlock();
return;
}
}
}
//すべてのBoundingBox内に点があるので再起動カウンター初期化
mutex.lock();
counter = 0;
mutex.unlock();
//一定フレーム以上取り逃している可能性があるので再起動フラグを立てる
if (counter >= settings.getLimitFrameCount()) {
mutex.lock();
isNeededToRestart = true;
mutex.unlock();
std::cerr << "Bone missing was detected." << std::endl;
}
OSCで座標を送信
最後に、対象の座標はOSCでUE4に配信しています。UE4では実寸の展示空間が仮想空間上に再現されています。その仮想空間に受信した座標を配置し、そこに人がいる状態を再現しています。
C++のOSCによるデータの配信は弊社エンジニアが書いた以下記事が詳しいです。
Visual Studio C++プロジェクトでOSCを送受信する手順｜PARTY｜note
繰り返しになりますが、この辺の技術構成に興味ある方は是非、以下記事の方も御覧ください！
「2025年大阪•関西万博誘致計画案」森美術館展示を実現した技術〜センサー編｜PARTY｜note
おわりに
実際のコードを抜粋して紹介したので、そのままコピペでは動かない部分も多いと思いますが、ロジックや実装を考える参考になれば嬉しいです。
質問や、よりよい方法があったら是非コメントしてください！
次は一日飛ばして、12/8(日)の @HoloMon さんの「BlenderとAzureKinectを組み合わせて何かする」です！