KinectのRGB-Dデータからポイントクラウドを生成してみた

KinectV2で取得したカラーマップとデプスマップを組み合わせて、実空間をリアルタイムでポイントクラウドにできるUnityアプリケーションを作ってみましたのでまとめます

ソースコード

なんでPCLを使わないの

• +αの処理がしたい
• 視点移動中に止まってしまう

参考プログラム

色付けしてないポイントクラウド

Kinect for Windows SDK 2.0 Unity Pro Add-in でポイントクラウド風描画を参考に作成

↑バージョンが変わったのか少々動かないところがあり、改造しました。
プログラムの内容は上のソースコードを見てください

色付けする

ソースの中からパーティクルの処理部分を抽出したものを貼り付けておきます

                    for (int y = 0; y < depthHeight; y += 2)
                    {
                        for (int x = 0; x < depthWidth; x += 2)
                        {
                            int depthIndex = (y * depthWidth) + x;
                            CameraSpacePoint p = cameraSpacePoints[depthIndex];
                            ColorSpacePoint colorPoint = colorSpacePoints[depthIndex];

                            byte r = 0;
                            byte g = 0;
                            byte b = 0;
                            byte a = 0;

                            int colorX = (int)System.Math.Floor(colorPoint.X + 0.5);
                            int colorY = (int)System.Math.Floor(colorPoint.Y + 0.5);

                            if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
                            {
                                int colorIndex = ((colorY * colorWidth) + colorX) * bytesPerPixel;
                                int displayIndex = depthIndex * bytesPerPixel;
                                b = colorFrameData[colorIndex++];
                                g = colorFrameData[colorIndex++];
                                r = colorFrameData[colorIndex++];
                                a = colorFrameData[colorIndex++];
                            }

                            if (!(double.IsInfinity(p.X)) && !(double.IsInfinity(p.Y)) && !(double.IsInfinity(p.Z)))
                            {
                                particles[particleCount].position = new Vector3(p.X*scale, p.Y*scale, p.Z*scale);
                                particles[particleCount].startColor = new Color(r/255F,g/255F,b/255F,a/255F);

                                particles[particleCount].startSize = size;
                                particleCount++;
                            }
                        }
                    }
                    _particleSystem = gameObject.GetComponent<ParticleSystem>();
                    _particleSystem.SetParticles(particles, particles.Length);
                }

• 今回はdepth領域の左上から右下までを探査するかたちで行いました

• またKinectV2 で カラー画像を映す- WPF * C#
  を参考に、RGBAが格納されている配列を、depthIndexを使って処理

• 私はparticle systemのstartColorで数日手こずってました
  (new Colorの引数が[0~1]ではなく[0~255]だと思っていた 笑)

• ちなみに色情報が格納されてある配列の要素数を調べてみると8294400という数値がでました。これは1980(色空間の幅サイズ)*1080(色空間の高さサイズ)*4(R,B,G,A)だと思っています