Azure-Kinect-DK跑通ORB_SLAM2

安装好Azure-Kinect的ROS版本，方法可参考：https://blog.csdn.net/u010497704/article/details/102583779

安装好ORB_SLAM的ROS版本。

下面，介绍如何使用Azure-Kinect-DK实时跑ORB_SLAM：

1.更改一下driver.launch里发布的image类型，默认值图像太大，帧速太慢

 打开/Azure_Kinect_ROS_Driver/launch/driver.launch直接更改

我把

depth_mode：NFOV_UNBINNED

color_resolution:720P

fps:30

具体硬件规格可参考：https://docs.microsoft.com/zh-cn/azure/Kinect-dk/hardware-specification

2.更改Orb slam接收的topic名称

需要注意！！！

orb slam默认的图像size是640×480，因此yaml里的ORB Parameters是仅符合640×480大小的图像的。如果你输入的图像分辨率过高，需要自己去调节ORB Parameters，否则跟踪效果很差，经常跟丢。

因此，这里我们选择的深度图大小为640×576，彩色图像大小为1280x720，将彩色图像对齐到深度图像，这样输入图像的大小就为640×576了，不需要自己再去调节ORB Parameters。

在/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc中进行更改

topic名称为：

/rgb_to_depth/image_raw

/depth/image_raw

3.更改ORB Slam读取的相机畸变参数个数

orb slam原本只能读取5个相机畸变参数：k1，k2，p1，p2，k3

但是 azure-kinect有八个相机畸变参数：k1，k2，k3，k4，k5，k6，p1，p2

因此需要修改以下orb slam的读入。

在Tracking.cc中：

（1）在

const float k3 = fSettings["Camera.k3"];
if(k3!=0)
    {
        DistCoef.resize(5);
        DistCoef.at(4) = k3;
    }

下面增加

    const float k4 = fSettings["Camera.k4"];
    if(k4!=0)
    {
        DistCoef.resize(6);
        DistCoef.at(5) = k4;
    }
    const float k5 = fSettings["Camera.k5"];
    if(k5!=0)
    {
        DistCoef.resize(7);
        DistCoef.at(6) = k5;
    }
    const float k6 = fSettings["Camera.k6"];
    if(k6!=0)
    {
        DistCoef.resize(8);
        DistCoef.at(7) = k6;
    }

（2）在

if(DistCoef.rows==5)
        cout << "- k3: " << DistCoef.at(4) << endl;

下面增加

if(DistCoef.rows==8)
    {
        cout << "- k3: " << DistCoef.at(4) << endl;
        cout << "- k4: " << DistCoef.at(5) << endl;
        cout << "- k5: " << DistCoef.at(6) << endl;
        cout << "- k6: " << DistCoef.at(7) << endl;
    }

4.更改yaml里面的相机参数

在运行 roslaunch azure_kinect_ros_driver driver.launch 时，会把相机的参数都打印出来，我们将其中深度相机的参数写入到yaml中（因为前面说过是对齐到深度相机）

我用到的参数是：

Camera.fx: 505.076
Camera.fy: 505.239
Camera.cx: 319.893
Camera.cy: 328.057

Camera.k1: 1.12383
Camera.k2: 0.629452
Camera.p1: 6.53274e-05
Camera.p2: -0.000118945
Camera.k3: 0.035496
Camera.k4: 1.46064
Camera.k5: 0.950368
Camera.k6: 0.180062

Camera.width: 640
Camera.height: 576

Camera.fps: 30.0
DepthMapFactor: 1.0

注意！DepthMapFactor: 1.0，因为kinect发送的depth图像是CV_32FC1的，已经除以1000了。

5.最后，插上kinect，开启roslaunch，就可以成功运行orbslam了！