realsense T265是一款很不错的双目+IMU传感器。体积非常小巧,而且IMU精度表现还挺不错的。
阅读本篇内容你将学会使用realsense T265传感器,而且还能学会使用它进行简单的程序开发。
而且以上数据的出厂时全都进行了标定,并且都保存在传感器中,你可以通过传感器的输出很容易获得内部的标定数据。
安装方法有两种,一种是源码安装,另一种是使用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-viewer
你可以左右移动移动它,带着它走一圈,看看它的轨迹准不准。你可以疯狂的抖它,我相信结果肯定出乎你的意料,自己去试试吧。
当你按照上述2中的方法进行了必要程序安装之后,你就已经成功的把realsense T265传感器所要用的库文件和头文件都安装到系统中了。所以我们直接编程使用就行了。
下面我向你介绍一下如何通过传感器输出的IMU信息,计算传感器运动的轨迹。其实就是通过IMU的速度和加速度信息积分产生位姿(位置和角度)信息。不废话,直接上代码。
// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.
#include
#include
#include
#include
#include
#include
#include
#include
//欧拉角转四元数,两种不同的旋转表示方法之间转换
//(如果你不懂的话,不必深究,这里慢慢来,你一时半会估计很难理解啥是四元数)
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.1的基础,这里我就直接上代码了。
#include
#include
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}
)
编译成功之后,连接上摄像头你就能看到实时的双目图像了。
上面这部分的代码来源于官方文档,我稍作修改。官方提供了很多有趣例子,你可以在下面这个网站中看到更多有趣的玩法,包括测距,简单的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
温馨提示,哈哈,上面的一些例子不是每一个都能正常运行的,因为有些还依赖于别的传感器,所以如果有些不能运行也不要感到奇怪。
如果还有问题,可以评论区给我留言,我会很快给你回复。