RealSenseからDepth, RGB,RGBD画像をMatlabを使って取得

#はじめに
Indelの提供しているRealSenseというカメラを利用した画像認識をする機会があり、画像の取得がかなり厄介だったのでメモしときます。

#動作環境
Windows 10以降？
Matlab 2018a
RealSense d435

#プログラム

## Depth, Colorの画像を取得
カメラからDepthとColorの情報を取得し、Depth画像とColor画像に変換して表示してみます。

```.m
pipe = realsense.pipeline(); 
profile = pipe.start(); % カメラから情報取得開始

% RealSenseのRGBカメラは、立ち上げた直後だと少し赤みがかった色を取得してきてしまうので、
% しばらくしてRGBがきちんと取得できるようになるまでforループを回しています。
for i=1:30
   fs = pipe.wait_for_frames();
end

pipe.stop(); %　カメラからの情報取得を停止

depth = fs.get_depth_frame(); 
color = fs.get_color_frame();

depthData = depth.get_data();
colorData = color.get_data();

depthImage = reshape(depthData',[3,depth.get_width(),depth.get_height()]),[3 2 1]);
colorImage = reshape(colorData',[2,color.get_width(),color.get_height()]),[2 1]);

imshow(depthImage); % Depth画像の表示
figure, imshow(colorImage); % RGB画像の表示
```

## 連続したDepth,Color画像の取得
先ほどのはあくまでも一枚の画像の取得でした。連続して取得をする(まあ動画みたいになります)場合は、取得の部分だけを動作させればいいです。

```.m
pipe = realsense.pipeline(); 
profile = pipe.start(); % カメラから情報取得開始

while 1
  fs = pipe.wait_for_frames();
  depth = fs.get_depth_frame(); 
  color = fs.get_color_frame();

  depth_data = depth.get_data();
  color_data = color.get_data();

  depth_image = reshape(depth_data',[3,depth.get_width(),depth.get_height()]),[3 2 1]);
  color_image = reshape(color_data',[2,color.get_width(),color.get_height()]),[2 1]);

  figure(1); % 表示するfigureを指定すると、指定されたfigureの画像を更新
  imshow(depth_image); % Depth画像の表示
  figure(2);
  imshow(color_image); % RGB画像の表示
end

pipe.stop(); %　カメラからの情報取得を停止
```

ここでのキーポイントは、`pipe.start()`と`pipe.stop()`がwhileのループの外にあるということです。
これは、カメラからの情報取得の開始と終了をする動作なので、連続して取得する場合ははじめに一度取得開始をすればいいだけということです。仮に`pipe.start()`と`pipe.stop()`をそれぞれループに入れた場合、一回の画像を取得するのに一秒半ほどかかります。

## おまけ、RGBD画像の作成
Depth画像とRGB画像を取得できるとなると、RGBD画像が作れます。
ここでは先ほどの連続取得のコードでRGBD画像を作成したいと思います。
プログラムの流れを簡単に説明すると、
1. Depth, RGBの取得
2. Depthの解像度をRGBの解像度に合わせる
3. 距離の閾値の設定(400[mm]より距離が長いときは白色を表示
4. Depthでは距離が取得できていない範囲が0となるので、それも白色
です。

```.m
pipe = realsense.pipeline(); 
profile = pipe.start(); % カメラから情報取得開始

while 1
  fs = pipe.wait_for_frames();
  depth = fs.get_depth_frame(); 
  color = fs.get_color_frame();

  depth_data = depth.get_data();
  color_data = color.get_data();

  depth_image = reshape(depthData',[3,depth.get_width(),depth.get_height()]),[3 2 1]);
  color_image = reshape(colorData',[2,color.get_width(),color.get_height()]),[2 1]);

  % Depthの解像度をRGBの解像度に合わせる
  align_to = realsense.stream.color;
  align = realsense.align(align_to);
  aligned_frames = align.process(fs);
  aligned_depth_frame = aligned_frames.get_depth_frame();
  aligned_depth_data = aligned_depth_frame.get_data();
  aligend_depthImage = permute(reshape(aligned_depth_data',[aligned_depth_frame.get_width(), aligned_depth_frame.get_height()]),[2 1]);

% 今回のRGB画像は480 x 640のサイズなので、RGBDもそのサイズで作成
  rgbd_image = color_image;
  for i = 0:480
      for j = 0:640
        d = depth_image(i,j);
        if (d > 400) || (d == 0)
          rgbd_image(i,j,:) = 255;
        else
          rgbd_image(i,j,:) = color_image(i,j,:);
        end
      end
  end
  imshow(rgbd_image);
end
pipe.stop(); %　カメラからの情報取得を停止
```

#参考文献
https://github.com/IntelRealSense/librealsense/tree/master/wrappers/matlab