ORB-SLAM2 双眼例を Oak-D 双眼カメラで実現

ROSのインストール

ROS をインストールするには公式ドキュメント http://wiki.ros.org/Distributions を読んで、適切な ROS バージョンをインストールするために独自の ubuntu システムバージョンを用意することをお勧めします。私のバージョンは 16.04+kinetic (16.04 バージョンの使用は推奨されません)バージョンが古すぎるのでアップグレードが必要すぎること）

ディープハイをインストールする

公式ドキュメントは非常に詳細なので、詳しく説明するのが面倒です https://docs.oakchina.cn/en/latest/pages/HighLevel/ros/ros_test_ Depthai_cpp_library.html

ORB-SLAM2をインストールする

同様に、公式 Web サイトのドキュメントを自分で読んでください https://github.com/raulmur/ORB_SLAM2 ここでは、公式ドキュメントを直接読む能力をさらに訓練することをお勧めします

プロセスの実行

双眼カメラによって公開されたトピックを表示して、orb-slam ノードを変更します

stereo_node.launch インスタンスを実行します。

gagaga@ubuntu:~$ cd catkin_ws/
gagaga@ubuntu:~/catkin_ws$ source devel/setup.bash
gagaga@ubuntu:~/catkin_ws$ roslaunch depthai_examples stereo_node.launch

トピックを見る

gagaga@ubuntu:~/catkin_ws$ rostopic list
#我选择的是这两个
/stereo_publisher/left/image
/stereo_publisher/right/image

カメラの内部パラメータと外部パラメータを表示する

rostopic echo -n 1 /stereo_publisher/left/camera_info
rostopic echo -n 1 /stereo_publisher/right/camera_info

-n 1メッセージは 1 回だけ出力されます。
パラメータは次のとおりです
D:。カメラの歪み係数。SLAM では、最初の 5 桁 [k1、k2、p1、p2、k3] は通常、カメラ内部の
K:参照行列で使用されます。
R：カメラ回転行列
P:. カメラ投影行列行列の 4 番目の数値は、設定ファイル内のパラメータ42.741687850846795に対応しますORB-SLAM2bf

以下は参考用のカメラの内部パラメータと外部パラメータです。

//左目
header: 
  seq: 0
  stamp: 
    secs: 1681488843
    nsecs: 696887012
  frame_id: "oak_left_camera_optical_frame"
height: 720
width: 1280
distortion_model: "rational_polynomial"
D: [0.8292564749717712, -0.07580796629190445, -0.0001298815623158589, -0.00036975208786316216, -0.011737951077520847, 1.1769423484802246, 0.09024432301521301, -0.045253634452819824]
K: [563.531494140625, 0.0, 639.3436889648438, 0.0, 563.531494140625, 356.0097961425781, 0.0, 0.0, 1.0]
R: [0.9999469518661499, -0.009474759921431541, 0.0040466636419296265, 0.00948441680520773, 0.9999521970748901, -0.002373715629801154, -0.0040239798836410046, 0.002411969704553485, 0.9999889731407166]
P: [566.3302001953125, 0.0, 634.0206909179688, 42.741687850846795, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
//右目
header: 
  seq: 0
  stamp: 
    secs: 1681488882
    nsecs: 431191493
  frame_id: "oak_right_camera_optical_frame"
height: 720
width: 1280
distortion_model: "rational_polynomial"
D: [1.209637999534607, -0.026369793340563774, -0.0002514233929105103, -0.0004901287029497325, -0.016096703708171844, 1.5596858263015747, 0.2600933015346527, -0.053905777633190155]
K: [566.3302001953125, 0.0, 634.0206909179688, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 1.0]
R: [0.9998436570167542, -0.007231925614178181, 0.016135742887854576, 0.007193292956799269, 0.9999711513519287, 0.0024510070215910673, -0.0161530040204525, -0.0023345546796917915, 0.999866783618927]
P: [566.3302001953125, 0.0, 634.0206909179688, 0.0, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

設定ファイルを変更するには、主に EuRoC.yaml と ros_stereo.cc の 2 つのファイルを変更します。ソースファイルを直接変更せず、バックアップを作成することをお勧めします。

EuRoC.yaml ファイルは **/home/gagaga/catkin_ws/src/ORB_SLAM2/Examples/Stereo** ディレクトリにあります。このファイルはカメラの内部パラメータと外部パラメータを設定するためのものです。表示されるパラメータをコピーするだけです。指定された場所。

以下は参考までに私が書いたものです（ただし、実際の動作ではなぜ歪みが残るのかはわかりません）

LEFT.height: 720
LEFT.width: 1280
LEFT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data:[0.8292564749717712, -0.07580796629190445, -0.0001298815623158589, -0.00036975208786316216, -0.011737951077520847]
LEFT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [563.531494140625, 0.0, 639.3436889648438, 0.0, 563.531494140625, 356.0097961425781, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9999469518661499, -0.009474759921431541, 0.0040466636419296265, 0.00948441680520773, 0.9999521970748901, -0.002373715629801154, -0.0040239798836410046, 0.002411969704553485, 0.9999889731407166]
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [566.3302001953125, 0.0, 634.0206909179688, 42.741687850846795, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 0.0, 1.0, 0.0]

RIGHT.height: 720
RIGHT.width: 1280
RIGHT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data:[1.209637999534607, -0.026369793340563774, -0.0002514233929105103, -0.0004901287029497325, -0.016096703708171844]
RIGHT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [566.3302001953125, 0.0, 634.0206909179688, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 1.0]
RIGHT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9998436570167542, -0.007231925614178181, 0.016135742887854576, 0.007193292956799269, 0.9999711513519287, 0.0024510070215910673, -0.0161530040204525, -0.0023345546796917915, 0.999866783618927]
RIGHT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [566.3302001953125, 0.0, 634.0206909179688, 0.0, 0.0, 566.3302001953125, 352.3761291503906, 0.0, 0.0, 0.0, 1.0, 0.0]

ros_stereo.cc ファイルの場所は /home/gagaga/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src です。ここでサブスクリプショントピックを変更します。

    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/stereo_publisher/left/image", 1);
    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/stereo_publisher/right/image", 1);

変更後は再コンパイルすればOK

gagaga@ubuntu:~/catkin_ws/src/ORB_SLAM2$ ./build_ros.sh

ついに稼働開始！

ここにスクリプトを作成して、orb-slam ルートディレクトリに .sh ファイルとして保存し、直接実行します。

#!/bin/bash

gnome-terminal --command="bash -c 'roscore; exec bash'"

sleep 3

gnome-terminal --command="bash -c 'roslaunch depthai_examples stereo_node.launch; exec bash'"


gnome-terminal --command="bash -c 'rosrun ORB_SLAM2 Stereo ~/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt ~/catkin_ws/src/ORB_SLAM2/Examples/Stereo/EuRoC.yaml false; exec bash'"

参考文献

https://blog.csdn.net/guanjing_dream/article/details/127859240

https://blog.csdn.net/qq_42221707/article/details/122603455