Intel realsense T265双目+IMU传感器使用方法介绍，以及如何快速上手编写realsense的代码，进行简单地开发，积分出位姿，使用opencv显示双目图像

realsense T265是一款很不错的双目+IMU传感器。体积非常小巧，而且IMU精度表现还挺不错的。

阅读本篇内容你将学会使用realsense T265传感器，而且还能学会使用它进行简单的程序开发。

1.realsense T265传感器输出的数据

• 双目摄像头可以采集到848X800分辨率下30FPS的鱼眼图像数据；
• IMU数据（包括6DoF的Pose，3DoF的加速度计，3DoF的陀螺仪）

而且以上数据的出厂时全都进行了标定，并且都保存在传感器中,你可以通过传感器的输出很容易获得内部的标定数据。

2.安装和使用方法

安装方法有两种，一种是源码安装，另一种是使用DKMS包进行安装。

如果英语还不错的话，也可以参考官网上的方法：Installing the packages. 我下面的方法就是从官方文档中提取出来的。

这里我强烈推荐第二种，因为这种方法就像你在电脑上安装软件一样简单。我下面介绍的也就是这种方法。

• 第一步注册服务器的公钥

sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

• 第二步将服务器添加到存储库列表（根据你的电脑版本选择）

（1）Ubuntu 16 LTS:

sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u

（2）Ubuntu 18 LTS:

sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u

• 第三步安装库文件（两个都要安装）

sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils

• 第四步选择安装开发人员和调试软件包（推荐安装第一个）

sudo apt-get install librealsense2-dev

sudo apt-get install librealsense2-dbg

• 第五步安装成功了，将你的realsense T265连接到电脑USB（至少USB3.0）口上，然后终端中输入以下命令就能看到传感器输出的数据了。

realsense-viewer

你可以左右移动移动它，带着它走一圈，看看它的轨迹准不准。你可以疯狂的抖它，我相信结果肯定出乎你的意料，自己去试试吧。

3. 编程使用realsense T265

3.1 通过IMU积分出位姿

当你按照上述2中的方法进行了必要程序安装之后，你就已经成功的把realsense T265传感器所要用的库文件和头文件都安装到系统中了。所以我们直接编程使用就行了。

下面我向你介绍一下如何通过传感器输出的IMU信息，计算传感器运动的轨迹。其实就是通过IMU的速度和加速度信息积分产生位姿（位置和角度）信息。不废话，直接上代码。

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.
#include <librealsense2/rs.hpp>
#include <iostream>
#include <iomanip>
#include <chrono>
#include <thread>
#include <mutex>

#include <math.h>
#include <float.h>

//欧拉角转四元数，两种不同的旋转表示方法之间转换
//（如果你不懂的话，不必深究，这里慢慢来，你一时半会估计很难理解啥是四元数）
inline rs2_quaternion quaternion_exp(rs2_vector v)
{
    float x = v.x/2, y = v.y/2, z = v.z/2, th2, th = sqrtf(th2 = x*x + y*y + z*z);
    float c = cosf(th), s = th2 < sqrtf(120*FLT_EPSILON) ? 1-th2/6 : sinf(th)/th;
    rs2_quaternion Q = { s*x, s*y, s*z, c };
    return Q;
}

//两个四元素之间进行“乘法”,相当于是旋转之后再旋转
inline rs2_quaternion quaternion_multiply(rs2_quaternion a, rs2_quaternion b)
{
    rs2_quaternion Q = {
        a.x * b.w + a.w * b.x - a.z * b.y + a.y * b.z,
        a.y * b.w + a.z * b.x + a.w * b.y - a.x * b.z,
        a.z * b.w - a.y * b.x + a.x * b.y + a.w * b.z,
        a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
    };
    return Q;
}

//通过离散欧拉积分计算出位姿（旋转和位置）
rs2_pose predict_pose(rs2_pose & pose, float dt_s)
{
    rs2_pose P = pose;
    P.translation.x = dt_s * (dt_s/2 * pose.acceleration.x + pose.velocity.x) + pose.translation.x;
    P.translation.y = dt_s * (dt_s/2 * pose.acceleration.y + pose.velocity.y) + pose.translation.y;
    P.translation.z = dt_s * (dt_s/2 * pose.acceleration.z + pose.velocity.z) + pose.translation.z;
    rs2_vector W = {
            dt_s * (dt_s/2 * pose.angular_acceleration.x + pose.angular_velocity.x),
            dt_s * (dt_s/2 * pose.angular_acceleration.y + pose.angular_velocity.y),
            dt_s * (dt_s/2 * pose.angular_acceleration.z + pose.angular_velocity.z),
    };
    P.rotation = quaternion_multiply(quaternion_exp(W), pose.rotation);
    return P;
}

int main(int argc, char * argv[]) try
{
  	//声明一个realsense传感器设备
    rs2::pipeline pipe;
    // 创建一个配置信息
    rs2::config cfg;
    //告诉配置信息，我需要传感器的POSE和6DOF IMU数据
    cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);

    
    std::mutex mutex;
    //回调函数
    auto callback = [&](const rs2::frame& frame)
    {
        std::lock_guard<std::mutex> lock(mutex);
        if (rs2::pose_frame fp = frame.as<rs2::pose_frame>()) {
            rs2_pose pose_data = fp.get_pose_data();
            auto now = std::chrono::system_clock::now().time_since_epoch();
            double now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now).count();
            double pose_time_ms = fp.get_timestamp();
            float dt_s = static_cast<float>(std::max(0., (now_ms - pose_time_ms)/1000.));
            rs2_pose predicted_pose = predict_pose(pose_data, dt_s);
            std::cout << "Predicted " << std::fixed << std::setprecision(3) << dt_s*1000 << "ms " <<
                    "Confidence: " << pose_data.tracker_confidence << " T: " <<
                    predicted_pose.translation.x << " " <<
                    predicted_pose.translation.y << " " <<
                    predicted_pose.translation.z << " (meters)   \r";
        }
    };

    //开始接收数据，接收数据之后进入回调函数进行处理
    rs2::pipeline_profile profiles = pipe.start(cfg, callback);
    std::cout << "started thread\n";
    while(true) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

源文件中牵扯到一些数学，不必深究看过就算过。有了源文件我们还需要CMakeLists.txt文件，这里你直接复制我写的就可以运行。

cmake_minimum_required(VERSION 3.1.0)
project(test)
set(CMAKE_BUILD_TYPE "release")

add_executable(test main.cpp) #通过main.cpp编译生成可执行文件test
target_link_libraries(test realsense2) #将realsense2的库文件链接给test

将源文件main.cpp和CMakeLists.txt放在同一个文件夹下，然后顺序执行以下命令就可以编译生成可执行文件了。

mkdir build
cd build
cmake ..
make 

最后将realsense T265连接上电脑（至少USB3.0），然后运行刚刚生成的程序test，移动移动传感器，就可以得到位姿信息了。

如果你运行的时候报错：error while loading shared libraries: librealsense2.so.2.34: cannot open shared object file: no such file or directory，那你需要将CMakeLists.txt中的librealsense2的链接方式改为如下方式：

find_package(realsense2 REQUIRED)
include_directories(  ${realsense2_INCLUDE_DIR} )
target_link_libraries(test ${realsense2_LIBRARY} )

3.2 用opencv显示双目摄像头的数据

有了3.1的基础，这里我就直接上代码了。

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

int main(){

    rs2::config cfg;
    cfg.enable_stream(RS2_STREAM_FISHEYE,1, RS2_FORMAT_Y8);
    cfg.enable_stream(RS2_STREAM_FISHEYE,2, RS2_FORMAT_Y8);
    rs2::pipeline pipe;
    pipe.start(cfg);

    rs2::frameset data;

    while (1){
        data = pipe.wait_for_frames();
        rs2::frame image_left = data.get_fisheye_frame(1);
        rs2::frame image_right = data.get_fisheye_frame(2);

        if (!image_left || !image_right)
            break;

        cv::Mat cv_image_left(cv::Size(848, 800), CV_8U, (void*)image_left.get_data(), cv::Mat::AUTO_STEP);
        cv::Mat cv_image_right(cv::Size(848, 800), CV_8U, (void*)image_right.get_data(), cv::Mat::AUTO_STEP);

        cv::imshow("left", cv_image_left);
        cv::imshow("right", cv_image_right);
        cv::waitKey(1);
    }

    return 0;
}

对应的CMakeLists.txt文件内容如下：

cmake_minimum_required(VERSION 3.1.0)
project(test)
set(CMAKE_BUILD_TYPE "release")

find_package(OpenCV)

include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(test main.cpp)
target_link_libraries(test
        realsense2
        ${OpenCV_LIBS}
)

编译成功之后，连接上摄像头你就能看到实时的双目图像了。

4.更多有趣的玩法

上面这部分的代码来源于官方文档，我稍作修改。官方提供了很多有趣例子，你可以在下面这个网站中看到更多有趣的玩法，包括测距，简单的AR等等。

T265 Tracking Camera
https://github.com/IntelRealSense/librealsense/blob/master/doc/t265.md

实际上这些例子，都已经在上文中的2步骤中全部成功安装到我们的电脑里面了，我们只需要打开一个终端输入rs-，然后按一下Tab键所有的例子都出来了，尽情享用吧！

如果你想知道他们是怎么实现的，你只需要看下面这个网址就可以。
https://github.com/IntelRealSense/librealsense/tree/master/examples

温馨提示，哈哈，上面的一些例子不是每一个都能正常运行的，因为有些还依赖于别的传感器，所以如果有些不能运行也不要感到奇怪。

如果还有问题，可以评论区给我留言，我会很快给你回复。