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

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

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

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

下面是专栏地址：

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

文章目录

• SLAM算法与工程实践系列文章
• • SLAM算法与工程实践系列文章链接
  • SLAM算法与工程实践系列专栏
  • 前言
  • SLAM算法与工程实践——相机篇：RealSense T265相机使用（2）
  • • 校正畸变
    • • 棋盘标定法
      • 横向展开法
      • 经纬度法
      • • ① 经度
        • ② 纬度
        • 接收和发布图像
        • • 同时订阅双目图像
          • • 在主函数中实现
            • 在类的成员函数中实现
            • 使用Opencv库订阅T265图像

              ---

              前言

              这个系列的文章是分享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    2 #include    3 #include    4 #include    5 #include    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_sub(nh, "image", 1);  21   message_filters::Subscriber info_sub(nh, "camera_info", 1);  22   23   typedef sync_policies::ExactTime MySyncPolicy;  24   // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)  25   Synchronizer 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 #include #include #include #include #include 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     // 初始化ROS节点    ros::init(argc, argv, "t265_viewer");    // 创建RealsenseViewer T265对象    UseT265Cam *t265_cam = new UseT265Cam();ros::NodeHandle nh;message_filters::Subscriber    sub_1 = new message_filters::SubscriberregisterCallback(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#include#include #include #include#includeusing 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 streamauto f = data.first_or_default(RS2_STREAM_POSE);auto pose = f.as().get_pose_data();cout