SLAM算法与工程实践——相机篇：RealSense T265相机使用（2）

yangtsejin

已于 2024-02-22 21:43:09 修改

阅读量1.9k

点赞数 15

分类专栏： SLAM算法与工程实践系列文章标签：算法 opencv 计算机视觉

于 2023-12-19 01:38:40 首次发布

本文链接：https://blog.csdn.net/qq_37648371/article/details/135005855

版权

SLAM算法与工程实践系列专栏收录该内容

23 篇文章 16 订阅

订阅专栏

本文详细介绍了SLAM算法在工程实践中的应用，特别是针对RealSenseT265相机的畸变矫正技术，包括校正方法如棋盘标定法、横向展开法和经纬度法，以及在ROS中的图像同步和处理。

摘要由CSDN通过智能技术生成

SLAM算法与工程实践系列文章

下面是SLAM算法与工程实践系列文章的总链接，本人发表这个系列的文章链接均收录于此

SLAM算法与工程实践系列文章链接

下面是专栏地址：

SLAM算法与工程实践系列专栏

前言

这个系列的文章是分享SLAM相关技术算法的学习和工程实践

SLAM算法与工程实践——相机篇：RealSense T265相机使用（2）

校正畸变

参考：

一文讲透鱼眼相机畸变矫正，及目标检测项目应用

AVM环视系统——鱼眼相机去畸变算法

无论是单目相机还是双目相机，拍摄的图像都会存在畸变。

它们和鱼眼相机的畸变矫正原理也是一样的：核心是求解一个“好”的重映射矩阵（remap matrix）。

从而将原图中的部分像素点（或插值点）进行重新排列，“拼”成一张矩形图。

“好”是跟最终需求挂钩的，不同任务往往采用不同的矫正/变形方案。

比如：

（1）单目相机的畸变矫正

对于单目相机，为了得到相机像素坐标系和三维世界坐标系的对应关系，我们需要对相机的桶形畸变和枕形畸变进行矫正。

（2）双目相机的畸变矫正

而对于双目相机，为了做极线对齐，实现深度估计。

我们需要将两个相机，输出变换到同一个坐标系下。

张正友的棋盘标定法，通过标志物的位置坐标，估计出相机的内外参数和畸变系数，从而计算出remap matrix。该方法是目前上述两类相机，矫正效果最好的方法。

（3）鱼眼相机的矫正变形

对于鱼眼相机，主要有三种方法：棋盘标定法、横向展开法、经纬度法。

下图是某款鱼眼相机的采集图像，而真正有效的监控区域，是内部的圆形区域。

在这里插入图片描述

棋盘标定法

棋盘矫正法的目的，是将鱼眼图“天生”的桶形畸变进行矫正。

具体效果类似于“用手对着圆形中心做挤压，把它压平”，使得真实世界中的直线，在矫正后依然是直线。

在这里插入图片描述

采用棋盘标定法进行矫正后：

在这里插入图片描述

我们发现：

① 现实世界中的直线，在鱼眼图中发生了扭曲（如鱼眼图中的蓝色和绿色曲线），矫正后变成了直线（如正方形图中的蓝色和绿色直线）;

② 矫正图只占据了鱼眼图中间的一部分（如鱼眼图中的红色曲线）。

从这个矫正效果中，可以看出：棋盘标定法的缺点，是靠近圆周（外围区域）的区域，会被拉伸的很严重，视觉效果变差。

所以一般会进行切除，导致矫正后的图片只保留了原图的中间区域。

基于以上特点，在实际使用中，我会把棋盘标定法，作为简单测量的前置任务（矫正图中的两点距离和真实世界中的两点距离，存在一一对应的关系）。

也可以作为鱼眼图像拼接的前置任务（真实世界中的三点共线，在拼接图中依然共线）。

横向展开法

横向展开法，主要是利用鱼眼相机的大FOV和俯视拍摄的特点，来进行变形。

在这里插入图片描述

比如我们把上图中的红点，想象成一个观察者，当他身体旋转360度，看到的什么样的画面呢？

在这里插入图片描述

上图是经过横向展开法，变形后的画面。

可以看到，从原先的俯视视角变为了正视视角。

因此可以根据区域功能，进行切片，再用普通视角的检测模型，做后续任务。

但是缺点也一目了然，比如展开图的左右两侧，在真实世界中应该是连通的。

所以当有目标在鱼眼图中穿过分界线时，在展开图中该目标会从左侧消失，右侧出现（或者倒过来），看起来不是很自然。

基于以上特点，在实际使用中，我会利用鱼眼相机，覆盖面积大的特点（比如3米层高的情况下，至少覆盖100平米），在“某些场景”中取代枪机或半球机，画面展开后用正常的检测器去完成后续任务。

这里还要补充两点：

① COCO数据集上训练的人体检测器，在鱼眼图中直接使用是不会work的;

② 与棋盘标定法不同，横向展开不会损失像素，所以展开图也可以再remap回鱼眼原图

经纬度法

经纬度法主要分为两个方面：

① 经度

在这里插入图片描述

下图是鱼眼图沿着经度对齐矫正后的画面。

在这里插入图片描述

该方法与棋盘矫正法相比，没有像素损失，也不需要标定（人为设计规则求解remap matrix）。

但是缺点也很明显，它只对竖直方向（图中的蓝色线和绿色线）进行了矫正，而水平方向（红色线）依然是扭曲的。

② 纬度

在这里插入图片描述

下图是鱼眼图沿着纬度对齐矫正后的画面。

在这里插入图片描述

可以看到，只对水平方向（图中的蓝色线和绿色线）进行了矫正，而竖直方向（红色线）依然是扭曲的。

基于以上特点，在实际落地中，我没有采用经纬度矫正法。

更多的是在学习和研究阶段，把它当作设计和计算remap matrix的一个作业。

接收和发布图像

参考：

ROS中C++ boost编程，类内回调函数

关于ROS在一个回调函数中处理两个订阅话题消息（多话题回调、多参数回调问题）

ROS之订阅多个话题并对其进行同步处理(多传感器融合)

同时订阅双目图像

订阅多个话题并对其进行同步处理，我这里订阅两个图像后再对图像做处理，使用同一个回调函数

在主函数中实现

利用全局变量 TimeSynchronizer

opencv官方方法：http://wiki.ros.org/action/fullsearch/message_filters?action=fullsearch&context=180&value=linkto%3A%22message_filters%22#Subscriber

#include <message_filters/subscriber.h>
   2 #include <message_filters/synchronizer.h>
   3 #include <message_filters/sync_policies/exact_time.h>
   4 #include <sensor_msgs/Image.h>
   5 #include <sensor_msgs/CameraInfo.h>
   6 
   7 using namespace sensor_msgs;
   8 using namespace message_filters;
   9 
  10 void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
  11 {
  12   // Solve all of perception here...
  13 }
  14 
  15 int main(int argc, char** argv)
  16 {
  17   ros::init(argc, argv, "vision_node");
  18 
  19   ros::NodeHandle nh;
  20   message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  21   message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
  22 
  23   typedef sync_policies::ExactTime<Image, CameraInfo> MySyncPolicy;
  24   // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  25   Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, info_sub);
  26   sync.registerCallback(boost::bind(&callback, _1, _2));
  27 
  28   ros::spin();
  29 
  30   return 0;
  31 }

我的实现

#include "UseT265Cam.h"
#include <iostream>
#include <opencv2/opencv.hpp>
#include <pcl/point_cloud.h>
#include <ros/ros.h>
#include <boost/bind.hpp>
#include <cv_bridge/cv_bridge.h>

void StereoImageCallback(const sensor_msgs::ImageConstPtr &img1_msg, const sensor_msgs::ImageConstPtr &img2_msg) {
    // 将ROS图像消息转换为OpenCV图像
    cv_bridge::CvImagePtr cv_image_left = cv_bridge::toCvCopy(img1_msg, sensor_msgs::image_encodings::MONO8);
    cv_bridge::CvImagePtr cv_image_right = cv_bridge::toCvCopy(img2_msg, sensor_msgs::image_encodings::MONO8);
    cv::Mat image_left = cv_image_left->image.clone();
    cv::Mat image_right = cv_image_right->image.clone();

    std::cout << "xxx" << std::endl;
    // 创建窗口
    cv::namedWindow("Left Raw View", cv::WINDOW_AUTOSIZE);
    cv::namedWindow("Right Raw View", cv::WINDOW_AUTOSIZE);
    // 显示图像
    cv::imshow("Left Raw View", image_left);
    cv::imshow("Right Raw View", image_right);
    cv::waitKey(1);
}


int main(int argc, char **argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "t265_viewer");

    // 创建RealsenseViewer T265对象
    UseT265Cam *t265_cam = new UseT265Cam();
ros::NodeHandle nh;

message_filters::Subscriber<sensor_msgs::Image> image_sub1(nh, "/camera/fisheye1/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> image_sub2(nh, "/camera/fisheye2/image_raw", 1);
    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> sync(image_sub1, image_sub2, 10);
    sync.registerCallback(boost::bind(&StereoImageCallback, _1, _2));
}

在类的成员函数中实现

利用类成员 message_filters::Synchronizer

在类中写函数实现一个回调函数处理多个话题信息时，需要用指针变量，如下所示，

不用指针变量我在写代码时不会进入回调函数，不知道是因为我代码写的有问题还是其他的原因，

在类的成员函数中实现接收多个话题，一定要用指针来实现

//定义变量
public:
    message_filters::Subscriber<sensor_msgs::Image> *sub_1;
    message_filters::Subscriber<sensor_msgs::Image> *sub_2;
    message_filters::Synchronizer<stereoSyncPolicy> *sync;

//类中的函数，将两个图像收到后一起处理
void UseT265Cam::subscribeStereoImage() {
    sub_1 = new message_filters::Subscriber<sensor_msgs::Image>(_nh, "/camera/fisheye1/image_raw", 10);
    sub_2 = new message_filters::Subscriber<sensor_msgs::Image>(_nh, "/camera/fisheye2/image_raw", 10);
    sync = new message_filters::Synchronizer<stereoSyncPolicy>(stereoSyncPolicy(10), *sub_1, *sub_2);
    sync->registerCallback(boost::bind(&UseT265Cam::StereoImageCallback, this, _1, _2));
}

//或者写为
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> *sync1=new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10),*sub_1, *sub_2);
    sync1->registerCallback(boost::bind(&UseT265Cam::StereoImageCallback, this, _1, _2));

使用Opencv库订阅T265图像

T265启动后的话题为

/camera/accel/imu_info
/camera/accel/metadata
/camera/accel/sample
/camera/fisheye1/camera_info
/camera/fisheye1/image_raw
/camera/fisheye1/image_raw/compressed
/camera/fisheye1/image_raw/compressed/parameter_descriptions
/camera/fisheye1/image_raw/compressed/parameter_updates
/camera/fisheye1/image_raw/compressedDepth
/camera/fisheye1/image_raw/compressedDepth/parameter_descriptions
/camera/fisheye1/image_raw/compressedDepth/parameter_updates
/camera/fisheye1/image_raw/theora
/camera/fisheye1/image_raw/theora/parameter_descriptions
/camera/fisheye1/image_raw/theora/parameter_updates
/camera/fisheye1/metadata
/camera/fisheye2/camera_info
/camera/fisheye2/image_raw
/camera/fisheye2/image_raw/compressed
/camera/fisheye2/image_raw/compressed/parameter_descriptions
/camera/fisheye2/image_raw/compressed/parameter_updates
/camera/fisheye2/image_raw/compressedDepth
/camera/fisheye2/image_raw/compressedDepth/parameter_descriptions
/camera/fisheye2/image_raw/compressedDepth/parameter_updates
/camera/fisheye2/image_raw/theora
/camera/fisheye2/image_raw/theora/parameter_descriptions
/camera/fisheye2/image_raw/theora/parameter_updates
/camera/fisheye2/metadata
/camera/gyro/imu_info
/camera/gyro/metadata
/camera/gyro/sample
/camera/odom/metadata
/camera/odom/sample
/camera/realsense2_camera_manager/bond
/camera/tracking_module/parameter_descriptions
/camera/tracking_module/parameter_updates

常用的有

/camera/fisheye1/image_raw
/camera/fisheye2/image_raw

/camera/accel/imu_info
/camera/accel/metadata
/camera/accel/sample

/camera/gyro/imu_info
/camera/gyro/metadata
/camera/gyro/sample

 #include<iostream>
#include<string>

#include <librealsense2/rs.hpp>

#include <opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

int main(int argc,char** argv)
{
    rs2::config cfg;

    // 使能 左右目图像数据
    cfg.enable_stream(RS2_STREAM_FISHEYE,1, RS2_FORMAT_Y8);
    cfg.enable_stream(RS2_STREAM_FISHEYE,2, RS2_FORMAT_Y8);

    // 使能 传感器的POSE和6DOF IMU数据
    cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);

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

    rs2::frameset data;

    while (1)
   {
    data = pipe.wait_for_frames();
	// Get a frame from the pose stream
	auto f = data.first_or_default(RS2_STREAM_POSE);
	auto pose = f.as<rs2::pose_frame>().get_pose_data();
	
	cout<<"px: "<<pose.translation.x<<"   py: "<<pose.translation.y<<"   pz: "<<pose.translation.z<<
	"vx: "<<pose.velocity.x<<"   vy: "<<pose.velocity.y<<"   vz: "<<pose.velocity.z<<endl;
	cout<<"ax: "<<pose.acceleration.x<<"   ay: "<<pose.acceleration.y<<"   az: "<<pose.acceleration.z<<
	"gx: "<<pose.angular_velocity.x<<"   gy: "<<pose.angular_velocity.y<<"   gz: "<<pose.angular_velocity.z<<endl;

     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;
}