Azure Kinectで点群を取得し，OpenRCFに表示する方法

本稿では，Azure Kinectから点群データを取得して，点群をOpenRCFのシミュレータに表示する方法を解説します．

OpenRCF自体は下記のサイトからダウンロードできます．
https://mase.openrcf.site

Azure Kinect SDK のインストール

１．「ツール」→「NuGetパッケージマネージャ」→「ソリューションのNuGetパッケージの管理」を開く．
２．参照から「Microsoft.Azure.Kinect.Sensor」を検索し，インストールする．
３．「AnyCPU」→「構成マネージャー」→「x64」に変える．

AzureKinect.csの追加

１．プロジェクト（OpenRCF）を右クリックして，「追加」→「新しい項目」→「クラス」を選び，「AzureKinect.cs」という名前のファイルを追加する．
２．「AzureKinect.cs」に以下のコードを記述する．

C#

using System;
using OpenRCF;
using Microsoft.Azure.Kinect.Sensor;

namespace RobotController
{
    class AzureKinect
    {
        private Device device;
        private Transformation transform;
        private DeviceConfiguration deviceConfiguration = new DeviceConfiguration();
        private Capture capture;
        private Image colorImage;
        private Image depthImage;

        public AzureKinect()
        {
            deviceConfiguration.DepthDelayOffColor = TimeSpan.Zero;
            deviceConfiguration.DisableStreamingIndicator = false;
            deviceConfiguration.ColorResolution = ColorResolution.R720p;
            deviceConfiguration.DepthMode = DepthMode.NFOV_2x2Binned;
            deviceConfiguration.CameraFPS = FPS.FPS15;
            deviceConfiguration.ColorFormat = ImageFormat.ColorBGRA32;            
            deviceConfiguration.SynchronizedImagesOnly = true;
            
            PrickleBall.Position = Position;
            PrickleBall.Rotate = Rotate;
     
            Polygon.LineWidth = 0;
            Polygon.Rotate = Rotate;
        }

        private bool isConnected = false;

        public void Connect(int deviceNum = 0)
        {  
            if (!isConnected)
            {
                if (deviceNum < Device.GetInstalledCount())
                {
                    if (Device.GetInstalledCount() < 2) deviceConfiguration.WiredSyncMode = WiredSyncMode.Standalone;
                    else
                    {
                        if (deviceNum == 0) deviceConfiguration.WiredSyncMode = WiredSyncMode.Master;
                        else deviceConfiguration.WiredSyncMode = WiredSyncMode.Subordinate;
                    }

                    try
                    {
                        device = Device.Open(deviceNum);
                        device.StartCameras(deviceConfiguration);
                        transform = device.GetCalibration().CreateTransformation();                        
                        isConnected = true;
                        Console.WriteLine("Connected to Azure Kinect.");
                    }
                    catch (AzureKinectOpenDeviceException ex)
                    {
                        Console.WriteLine("Failed to connect Azure Kinect.");
                        Console.WriteLine(ex.Message);
                        Console.WriteLine(ex.StackTrace.ToString());
                    }
                }
                else
                {
                    Console.WriteLine("Azure Kinect is not found in serial devices.");
                }
            }
            else
            {
                Console.WriteLine("Azure Kinect is already connected.");
            }
        }
        
        private BGRA[] pointColors = new BGRA[0];
        private Short3[] pointPositions = new Short3[0];

        public void UpdatePointCloud()
        {
            if (isConnected)
            {                               
                capture = device.GetCapture();
                colorImage = transform.ColorImageToDepthCamera(capture);
                depthImage = transform.DepthImageToPointCloud(capture.Depth);

                pointColors = colorImage.GetPixels<BGRA>().ToArray();
                pointPositions = depthImage.GetPixels<Short3>().ToArray();

                capture.Dispose();
                colorImage.Dispose();
                depthImage.Dispose();
            }
        }
        
        private PrickleBall PrickleBall = new PrickleBall(0.035f);
        private Cuboid Camera = new Cuboid(0.103f, 0.04f, 0.1254f);
        private Rectangle Polygon = new Rectangle(0.01f, 0.01f);

        public Vector Position
        {
            get { return Camera.Position; }
            set { Camera.Position = value; }
        }

        public RotationMatrix Rotate
        {
            get { return Camera.Rotate; }
            set { Camera.Rotate = value; }
        }

        public bool IsDrawEnabled = true;

        public void Draw()
        {
            PrickleBall.Draw();
            Camera.Draw();

            if (IsDrawEnabled)
            {                                
                float[] vector = new float[3];
               
                for (int i = 0; i < pointPositions.Length; i++)
                {
                    if (250 < pointPositions[i].Z && pointPositions[i].Z < 6000)
                    {                 
                        vector[0] = 0.001f * pointPositions[i].X;
                        vector[1] = 0.001f * pointPositions[i].Y;
                        vector[2] = 0.001f * pointPositions[i].Z;
                        Polygon.Position.Set = Position.Plus(Rotate.Times(vector));
                        Polygon.Color.SetRGBA(pointColors[i].R, pointColors[i].G, pointColors[i].B, 0);
                        Polygon.Draw();
                    }
                }
            }   
        }

    }
}

AzureKinect.csの使い方

例えば，MainWindow.xaml.cs内に次のように記述する．

C#

AzureKinect AzureKinect = new AzureKinect();

void Draw()
{
    AzureKinect.Draw();
}
        
void Button1_Click(object sender, RoutedEventArgs e)
{
    AzureKinect.Connect();
}
        
void Button2_Click(object sender, RoutedEventArgs e)
{
    Parallel.RunEndless(AzureKinect.UpdatePointCloud, 100);
}

void Button3_Click(object sender, RoutedEventArgs e)
{
    AzureKinect.IsDrawEnabled = !AzureKinect.IsDrawEnabled;
}

void Button4_Click(object sender, RoutedEventArgs e)
{
    AzureKinect.Position[2] = 1;
    AzureKinect.Rotate.SetRollPitchYaw(-0.5 * Math.PI, 0, -0.5 * Math.PI);
}

ボタン1を押すと Azure Kinect と接続し，ボタン2を押すと100msごとに点群データが更新される．ボタン3を押すと描画のオン-オフが切り替わる．全ての点群を描画するのは非常に重い処理なので，描画をする必要がない場合はオフにしておいた方がよい（常にオンだとPCが発熱する）．ボタン4を押すと Azure Kinect の位置・姿勢が設定され，それに応じて点群データの表示位置も更新される．実行すると，以下のように点群データ（色付き）がシミュレータ上に表示される．