Go to Qiita Advent Calendar 2024 Top
LoginSignup
46
35

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@idev_jp(koki ogura)

RealSense予備実験色々

Last updated at Posted at 2018-10-15

#How to set capture size with pipeline.

.cpp
rs2::config cfg;
cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_RGB8, 30);
cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 30);
rs2::pipeline pipe;
pipe.start(cfg);

上記はGLで使う場合(ピクセルの色情報がRGBの順番)。CV(BGRの順番)ならば

.cpp
cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_BGR8, 30);

とする。

#OpenCVを使って表示する例
opencv_cap2.png

opencv_cap2.cpp
#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>

int main()
{
  rs2::colorizer color_map;

  rs2::config cfg;
  cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_BGR8, 30);
  cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 30);

  rs2::pipeline pipe;
  pipe.start(cfg);

  while(cv::waitKey(1) == -1) {
    rs2::frameset frames = pipe.wait_for_frames();
    rs2::frame color_frame = frames.get_color_frame();
    rs2::frame depth_frame = color_map(frames.get_depth_frame());

    cv::Mat color(cv::Size(1280, 720), CV_8UC3, (void*)color_frame.get_data(), cv::Mat::AUTO_STEP);
    cv::Mat depth(cv::Size(1280, 720), CV_8UC3, (void*)depth_frame.get_data(), cv::Mat::AUTO_STEP);

    cv::namedWindow("color", cv::WINDOW_AUTOSIZE);
    cv::imshow("color", color);
    cv::namedWindow("depth", cv::WINDOW_AUTOSIZE);
    cv::imshow("depth", depth);
  }
  return 0;
}

上記プログラムはcolorとdepthをそれぞれ別ウィンドウで表示する。
これをdispという名前の1つの行列にまとめて表示するには以下のようにすれば良い。
（縦に結合してから全体を65%に縮めて表示）

.cpp
    cv::Mat disp(color.rows + depth.rows, std::max(color.cols, depth.cols), CV_8UC3);
    color.copyTo(cv::Mat(disp, cv::Rect(0, 0, color.cols, color.rows)));
    depth.copyTo(cv::Mat(disp, cv::Rect(0, color.rows, depth.cols, depth.rows)));

    cv::resize(disp, disp, cv::Size(), 0.65, 0.65);

    cv::namedWindow("disp", cv::WINDOW_AUTOSIZE);
    cv::imshow("disp", disp);

Mat行列を作る部分のサイズ指定では縦横の順にする必要がある。
行列のサイズ指定は伝統的に行数列数の順番なので。

#ビルドするコマンド（例えばopencv_cap2.cpp）

.sh
$ g++ -std=c++11 -o opencv_cap2 opencv_cap2.cpp `pkg-config --cflags opencv` `pkg-config --cflags realsense2` `pkg-config --libs realsense2` `pkg-config --libs opencv`

#cmakeファイル (例えばopencv_cap2.cpp)

CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(opencv_cap2 CXX)
set(CMAKE_CXX_FLAGS "-std=c++11 -O2 -Wall")

find_package(PkgConfig)
pkg_check_modules(RealSense2 REQUIRED realsense2)
pkg_check_modules(OpenCV REQUIRED opencv)
include_directories(${RealSense2_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
link_directories(${RealSense2_LIBRARY_DIRS} ${OpenCV_LIBRARY_DIRS})

add_executable(opencv_cap2 opencv_cap2.cpp)
target_link_libraries(opencv_cap2 ${RealSense2_LIBRARIES} ${OpenCV_LIBRARIES})

のように記述する。link_directoriesの後にadd_executable、その後にtarget_link_libraries等の順番が大切。

#cmakeを使ったビルドと実行
適当なディレクトリに上記のopencv_cap2.cppとCMakeLists.txtを作る。その状態から以下のコマンドでビルドして実行できる。

.sh
$ mkdir build
$ cd build
$ cmake ..
$ make
$ ./opencv_cap2

#colorとdepth
それぞれの情報を同じ画素数(例えば1280x720)で取得しても、色情報と深度情報ではカメラの画角が違うので色情報の任意のピクセル位置に対応する深度を得たいと思って深度情報の同じピクセル位置のものを取っても間違った情報となる。
こうならないために色情報と深度情報を画角を考えて一旦結合させてから色情報と深度情報を得る必要である。D435の場合は色情報より深度情報の方の画角が広いので深度情報を色情報の画角に合わせることになる。これは次のようにプログラムで簡単にできる。

.cpp
    rs2::frameset frames = pipe.wait_for_frames();
    rs2::align align(RS2_STREAM_COLOR);
    auto aligned_frames = align.process(frames);
    rs2::video_frame color_frame = aligned_frames.first(RS2_STREAM_COLOR);
    rs2::depth_frame depth_frame = aligned_frames.get_depth_frame();

このようにして得られた色情報と深度情報のピクセルの位置は一致している。
##注意
ただし、一番下の行、1280x720であれば719の行に対する深度はところどころ取得できないみたいだ。従ってこのような場合は0から718までのものを利用するのが良いだろう。

#色情報(カラー画像)の任意のピクセルに対応する3次元上の点の位置
これも用意されている関数をincludeして使えば簡単に出来る。
pipe.start直後に画角情報(depth_intr)を取得しておく。
後ほどalignで画角をカラーカメラの方に合わせるので取得は色情報の方だ。
色画像の(x,y)の位置のピクセルに対する3次元上で位置(point)は次のように取得する。

.cpp
#include <librealsense2/rs.hpp>
#include <librealsense2/rsutil.h>
#include <opencv2/opencv.hpp>

  // fps: 6, 15, 30
  rs2::config cfg;
  cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_BGR8, 6);
  cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 6);

  // start and get depth intrinsics
  rs2::pipeline pipe;
  auto profile = pipe.start(cfg);
  auto depth_stream = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
  auto depth_intr = depth_stream.get_intrinsics();

  while (true) {
    // get frames
    rs2::frameset frames = pipe.wait_for_frames();

    // align
    rs2::align align(RS2_STREAM_COLOR);
    auto aligned_frames = align.process(frames);
    rs2::video_frame color_frame = aligned_frames.first(RS2_STREAM_COLOR);
    rs2::depth_frame depth_frame = aligned_frames.get_depth_frame();

    ....
    // get 3d-coordinate info from specified pixel position.
    float pixel[2] = {float(x), float(y)};
    float depth = depth_frame.get_distance(pixel[0], pixel[1]);
    float point[3];
    rs2_deproject_pixel_to_point(point, &depth_intr, pixel, depth);
    ...
  }

#Post Process
深度情報を表示してみると解るんだけど、カメラを固定して同じところの情報を取得してもフレーム毎に値が変わる。これを抑える為に深度情報を取得した後に深度情報に対してかけるフィルターが用意されている。実際にやってみると結構な効果があるので、このフィルタリングは必須だと思う。参考資料へのリンクは
https://www.mouser.com/pdfdocs/Intel-RealSense-Depth-PostProcess.pdf
ヘッダーの在処は
/usr/local/include/librealsense2/rs_processing.hpp
それぞれのフィルターの意味は上記リンク先をあたって欲しい。
実際のプログラミングは次のようにする。
フィルターの初期化(例)は

.cpp
  rs2::decimation_filter dec_filter;
  dec_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 3);
  rs2::disparity_transform depth_to_disparity(true);
  rs2::disparity_transform disparity_to_depth(false);
  rs2::spatial_filter spat_filter;
  spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 2);
  spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, 0.5);
  spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, 20);
  spat_filter.set_option(RS2_OPTION_HOLES_FILL, 0);
  rs2::temporal_filter temp_filter;
  temp_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, 0.4);
  temp_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, 20);
  temp_filter.set_option(RS2_OPTION_HOLES_FILL, 3);
  rs2::hole_filling_filter hf_filter;
  hf_filter.set_option(RS2_OPTION_HOLES_FILL, 1);

そして、深度情報depth_frameを得た後に順番にフィルターをかける。
この時、次の順番にするのが大切。

.cpp
    // filter
    depth_frame = dec_filter.process(depth_frame);
    depth_frame = depth_to_disparity.process(depth_frame);
    depth_frame = spat_filter.process(depth_frame);
    depth_frame = temp_filter.process(depth_frame);
    depth_frame = disparity_to_depth.process(depth_frame);
    depth_frame = hf_filter.process(depth_frame);

これでフィルタリングされた深度情報を得ることが出来るようになった。
注意しなければならないのは初期化の時に

.cpp
  rs2::decimation_filter dec_filter;
  dec_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 3);

のように3x3のサブピクセル化を指定しているので、アクセスに関してはpixel(x,y)に対する深度情報はx,yそれぞれ3で割って、

.cpp
      float pixel[2] = {float(x), float(y)};
      float depth = depth_frame.get_distance(pixel[0]/3.0, pixel[1]/3.0);
      float point[3];
      rs2_deproject_pixel_to_point(point, &depth_intr, pixel, depth);

のようにアクセスしなければならなくなる。以上に注意すればフィルタリングされたノイズの少ない深度情報を使って3次元のpoint情報を得ることが出来る。それでもカメラからの距離が6mを超えるような点でのノイズに依ると思われる揺らぎは大きい(5%以上か？）のだが。

#3D表示はOpen3Dを使う
RealSense自体のサンプルプログラムでPointCloudをインタラクティブに表示させるのにはOpenGLを使っている。しかし次のような理由で僕はOpen3Dを使う。

  • 設定の問題かもしれないが、OpenGLだとmacからslogin -Yで接続した時にmacサイドにXで表示できない！
  • 代わりにVizを使おうと思ってもOpenCVの再構築が必要で面倒だ！

要はロボット等での動作は外部からアクセスして表示させたいのさ。
という訳で次にOpen3Dのインストールについて書く。

##さっそく追記
OpenGLがmacから使えないのはmac側の設定の問題だった。
一旦、XQuartzを終了しmacのターミナルを開いて、

.sh
$ defaults write org.macosforge.xquartz.X11 enable_iglx -bool true

とiglxを有効にする設定をしてあげれば良い。
これでRealSenseでのOpenGLを使ったサンプルプログラムをmacから動かすことができるようになる。realsense-viewerもちゃんと動く。やったね！
しかし、ここでもっと重要な問題が発覚！
これらの設定をしても、Open3Dのサンプル(例えばpointcloud.py)がmacから動かない！

.sh
GLFW Error: X11: RandR gamma ramp support seems broken
libGL error: No matching fbConfigs or visuals found
libGL error: failed to load driver: swrast
GLFW Error: Requested OpenGL version 2.1, got version 1.4
Failed to create window
[DrawGeometries] Failed creating OpenGL window.

のようなエラーメッセージが出てくる。
OpenGLがダメだからOpen3Dを選択したのに、このままの状況では本末転倒だな。

##さっそく追記２
OpenGL使えないからOpen3D使おうってのもアホですね。
Open3DからOpenGL(GLFW)を使ってるのにね。
当初はOpen3Dは全く別系統のものだと思っていました。

##追記３
macからsshでロボットに接続してglxが使えないのは困る。
仕方がないのでロボットにはモニターをつけておいて、macから接続した時に次のコマンドを実行してウィンドウ出力はロボット側のモニターに出力するようにして対処している。
色々なデバッグはこれでいい。ロボットサイドで全てやればいいんじゃないの？と言われそうだけどね。それでもいいんだけどね（笑）

.sh
$ export DISPLAY=:0.0

これ以降のX出力はロボット側（リモート側）になる。例えば

.sh
$ glxgears

はロボット側のモニターにウィンドウ表示される。
ただしcv2.waitKeyなどは該当ウィンドウ側、つまりはロボット側（リモート側）のキー入力となる。やっぱりロボットサイドで全てやった方がいいかも（笑）

#Open3Dのインストール(python3からも使えるように)
http://www.open3d.org/docs/getting_started.html#ubuntu

python 3.7.0 を pyenv にて global に使っている前提。
~/developディレクトリー内で作業する。

.sh
$ cd ~/develop
# ソースをclone
$ git clone https://github.com/IntelVCL/Open3D
$ cd Open3D
$ util/scripts/install-deps-ubuntu.sh
$ mkdir build
$ cd build
# 使っているpythonのpathを指定してcmake
$ cmake -DPYTHON_EXECUTABLE:FILEPATH=$HOME/.pyenv/versions/3.7.0/bin/python3.7 ..
# makeしてインストール
$ make -j
$ sudo make install
# 使っているpythonから簡単にimportできるようにする。
$ cd ~/.pyenv/versions/3.7.0/lib/python3.7/site-packages
$ sudo ln -s ~/develop/Open3D/build/lib/Python/open3d.cpython-37m-x86_64-linux-gnu.so open3d.so
# 次のコマンドで必要なディレクト指定は以下のようにして調べる。
# システムに標準でインストールされているpython3を次のように起動して
# $ /usr/bin/python3
# >>> import _tkinter
# >>> _tkinter.__file__
# '/usr/lib/python3.6/lib-dynload/_tkinter.cpython-36m-x86_64-linux-gnu.so'
# >>> exit()
$ sudo ln -s /usr/lib/python3.6/lib-dynload/_tkinter.cpython-36m-x86_64-linux-gnu.so _tkinter.so
# matplotlibが必要。インストールしていなければ次のようにインストールする。
$ pip install matplotlib
# さあテストだ！
$ cd ~/develop/Open3D/examples/Python/Basic
$ python rgbd_sun.py

#他のubuntuマシンから接続して使う
そのマシンの設定を変えないとGLXが使えない。
/usr/share/lightdm/lightdm.conf.d/50-xserver-command.conf
を次のように編集（-coreの後に +iglxを追加）してindirect glxが使えるようにする。

50-xserver-command.conf
xserver-command=X -core +iglx

そしてrebootすれば良い。
これで他のubuntu 18.04マシンでOpen3Dの結果が表示できる。

#Point Cloud
別記事で書くことにした。
https://qiita.com/idev_jp/items/0f71cf831b604f8adcea

#資料リンク等
RealSense API How To
RealSense Class Reference
Projection in RealSense SDK 2.0
Depth Post-Processing for Intel® RealSenseTM D400 Depth Cameras
Open3D

46
35
3

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
46
35

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?