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

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

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

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

---

下面是专栏地址：

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

---

---

前言

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

---

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

相机标定

参考：

realsense相机两种获取相机内外参的方式

使用Realsense D435i运行VINS-Fusion

kalibr标定realsenseD435i --多相机标定

kalibr标定realsenseD435i（二）–多相机标定

realsense相机内参如何获得+python pipeline+如何通过python script获取realsense相机内参（windows下可用）

将.bag包解析为图片

创建extract.py，修改相应信息，然后：python extract.py

#coding:utf-8

import roslib;  
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError

path_left='/home/lk/Pictures/left/' #存放图片的位置
path_right='/home/lk/Pictures/right/' #存放图片的位置
class ImageCreator():

    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('_2023-05-29-19-42-07.bag', 'r') as bag:   #要读取的bag文件；
            for topic,msg,t in bag.read_messages():
                if topic == "/camera/infra1/image_rect_raw":  #图像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print (e)
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        image_name = timestr+ ".jpg" #图像命名：时间戳.jpg
                        cv2.imwrite(path_left+image_name, cv_image)  #保存；
                if topic == "/camera/infra2/image_rect_raw":  #图像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print (e)
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        image_name = timestr+ ".jpg" #图像命名：时间戳.jpg
                        cv2.imwrite(path_right+image_name, cv_image)  #保存；


if __name__ == '__main__':

    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

在解析的left、right目录里选取相互对应的图片

注：图片要清晰，棋盘格要在视野内。附棋盘格生成网站：Camera Calibration Pattern Generator

打开标定软件，输入图片、内角点、尺寸

直接获取相机参数

将相机的roslaunch停止，然后使用命令读取参数

rs-enumerate-devices -c

黑白相机参数

左相机内参

Intrinsic of "Infrared 1" / 424x240 / {Y8}
  Width:        424
  Height:       240
  PPX:          216.466430664062
  PPY:          118.884384155273
  Fx:           214.962127685547
  Fy:           214.962127685547
  Distortion:   Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    89.19 x 58.34

右相机内参

 Intrinsic of "Infrared 2" / 424x240 / {Y8}
  Width:        424
  Height:       240
  PPX:          216.466430664062
  PPY:          118.884384155273
  Fx:           214.962127685547
  Fy:           214.962127685547
  Distortion:   Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    89.19 x 58.34

1. 相机内参（Intrinsic Parameters）：
   • 焦距（Focal Length）：单位为像素（pixel）。
   • 光学中心（Optical Center）：单位为像素（pixel）。
2. 相机畸变参数（Distortion Parameters）：
   • 畸变系数（Distortion Coefficients）：无单位。

相机内参和畸变参数可以通过Realsense相机的SDK或工具获取。请注意，Realsense D435相机的深度图像默认以毫米（mm）作为单位。如果您要将深度图像转换为米（m）作为单位，可以将深度值除以1000。

关于 fx，fy, ppx, ppy的详细说明见：https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0#intrinsic-camera-parameters

外参，from infrared 1 to infrared 2 指的就是T

平移向量的单位是米

相机的内参矩阵是
$$\left[\begin{array}{ccc}{f}_x& s& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$
$f_x,f_y$ 为焦距，一般情况下，二者相等。

$x_0,y_0$ 为主坐标（相对于成像平面）。

$s$ 为坐标轴倾斜参数，理想情况下为0

彩色相机参数

 Intrinsic of "Color" / 640x480 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        640
  Height:       480
  PPX:          326.332061767578
  PPY:          241.928192138672
  Fx:           610.158020019531
  Fy:           610.118896484375
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    55.35 x 42.95

 Intrinsic of "Color" / 848x480 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        848
  Height:       480
  PPX:          430.33203125
  PPY:          241.928192138672
  Fx:           610.157958984375
  Fy:           610.118896484375
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    69.59 x 42.95

 Intrinsic of "Color" / 960x540 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        960
  Height:       540
  PPX:          487.12353515625
  PPY:          272.169219970703
  Fx:           686.427734375
  Fy:           686.3837890625
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    69.92 x 42.95

 Intrinsic of "Color" / 1280x720 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        1280
  Height:       720
  PPX:          649.498046875
  PPY:          362.892272949219
  Fx:           915.236938476562
  Fy:           915.178405761719
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    69.92 x 42.95

 Intrinsic of "Color" / 1920x1080 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        1920
  Height:       1080
  PPX:          974.2470703125
  PPY:          544.338439941406
  Fx:           1372.85546875
  Fy:           1372.767578125
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    69.92 x 42.95

订阅话题

参考：

Robotics - Publish and Subscribe to RGB Image from Intel Realsense

使用ros从realsence相机中获取图像

realsense在ros中的订阅处理

[ROS调用cv_bridge]undefined reference to `cv_bridge::toCvCopy(boost::shared_ptr＜sensor_msgs::Image_

订阅RGB相机

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "cv_bridge/cv_bridge.h"
#include "sensor_msgs/CameraInfo.h"
#include "sensor_msgs/Image.h"
#include "image_transport/image_transport.h"
#include "opencv2/highgui/highgui.hpp"
using namespace std;


void imgRGBCallback(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    cv::Mat color_mat;
    cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
    color_mat = cv_ptr->image;
    cv::imshow("RGB", color_mat);
    cv::waitKey(1);
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "realsense_sub");
    ros::NodeHandle n;
    ros::Subscriber subRGBImg;
    subRGBImg = n.subscribe("/camera/color/image_raw", 1,  imgRGBCallback);
    ros::spin();
    return 0;
}

CMakeLists.txt如下

cmake_minimum_required(VERSION 3.0.2)
project(depth_rs)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  cv_bridge	# 记得包含cv_bridge，不然可能会报错
)

add_executable(depth_rs_node src/depth_rs_node.cpp)

target_link_libraries(depth_rs_node
  ${catkin_LIBRARIES}
)

订阅双目

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

class RealsenseViewer
{
public:
  RealsenseViewer()
  {
    // 初始化ROS节点
    nh_ = ros::NodeHandle("~");

    // 创建左右视图的图像订阅器
    left_image_sub_ = nh_.subscribe("/camera/infra1/image_rect_raw", 1, &RealsenseViewer::leftImageCallback, this);
    right_image_sub_ = nh_.subscribe("/camera/infra2/image_rect_raw", 1, &RealsenseViewer::rightImageCallback, this);

    // 创建窗口
    cv::namedWindow("Left View", cv::WINDOW_AUTOSIZE);
    cv::namedWindow("Right View", cv::WINDOW_AUTOSIZE);
  }

  void leftImageCallback(const sensor_msgs::ImageConstPtr& msg)
  {
    // 将ROS图像消息转换为OpenCV图像
    cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);

    // 显示左视图图像
    cv::imshow("Left View", cv_image->image);
    cv::waitKey(1);
  }

  void rightImageCallback(const sensor_msgs::ImageConstPtr& msg)
  {
    // 将ROS图像消息转换为OpenCV图像
    cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);

    // 显示右视图图像
    cv::imshow("Right View", cv_image->image);
    cv::waitKey(1);
  }

private:
  ros::NodeHandle nh_;
  ros::Subscriber left_image_sub_;
  ros::Subscriber right_image_sub_;
};

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

  // 创建RealsenseViewer对象
  RealsenseViewer viewer;

  // 循环运行ROS节点
  ros::spin();

  return 0;
}

立体匹配

参考：

awesome-sgbm

https://blog.csdn.net/wwp2016/article/details/86080722

SGBM算法使用

在SGBM（Semi-Global Block Matching）算法中，有多个参数可以调整以获得最佳的立体匹配效果。以下是SGBM算法中常用参数的说明：

1. minDisparity：最小视差值。它表示视差图中最小的视差值，通常为0，默认为0。。
2. numDisparities：视差范围的数量。它表示图像中可能的最大视差值与最小视差值之间的差异范围。通常，它是16的倍数。
3. blockSize：匹配块的大小。它定义了在计算代价体积时要使用的像素块的大小。较大的块大小可以提供更好的噪声抑制，但可能会导致更大的计算开销。它必须是奇数，并且通常在3到11之间，默认为3。
4. P1：控制视差平滑度的第一个参数，是一个整数值，默认为0。它是一个正整数，表示相邻像素之间的视差差异所引起的惩罚。较大的P1值会导致更平滑的视差图像，但可能会丢失一些细节。
5. P2：控制视差平滑度的第二个参数，是一个整数值，默认为0。进一步控制视差平滑性的参数。它通常比P1大，可以用来控制边缘处的视差平滑程度。
6. disp12MaxDiff：左右视图之间一致性检查的最大差异。它是一个正整数，用于限制左右视差图之间的最大差异。较大的值可以提高一致性，但可能会导致视差图的不连续性。表示左右视差图像之间的最大差异，默认为0。较大的值可以用于去除不可靠的匹配。
7. preFilterCap：预处理滤波器的截断值。它用于控制预处理阶段中代价体积的截断范围。预处理滤波器的容量，用于去除噪声，默认为0。较大的值可以提高匹配的鲁棒性，但也可能导致某些细节丢失。
8. uniquenessRatio：视差唯一性比率，表示最佳匹配和次佳匹配之间的差异阈值，默认为0。较大的值会增加匹配的准确性，但也可能导致视差图像的稀疏性增加（可能会导致视差图的不连续性）。它定义了最佳匹配与次佳匹配之间的视差差异。
9. speckleWindowSize：用于去除孤立的视差点的窗口大小，默认为0，表示不执行孤立点过滤。
10. speckleRange：用于去除孤立的视差点的阈值范围，默认为0，表示不执行孤立点过滤。

这些参数可以通过调用cv::StereoSGBM对象的相应方法进行设置。例如：

cv::StereoSGBM sgbm;
sgbm.setMinDisparity(minDisparity);
sgbm.setNumDisparities(numDisparities);
sgbm.setBlockSize(blockSize);
sgbm.setP1(P1);
sgbm.setP2(P2);
sgbm.setDisp12MaxDiff(disp12MaxDiff);
sgbm.setPreFilterCap(preFilterCap);
sgbm.setUniquenessRatio(uniquenessRatio);

例如

#include <opencv2/opencv.hpp>

int main() {
    cv::Mat imageLeft = cv::imread("left_image.png", cv::IMREAD_GRAYSCALE);
    cv::Mat imageRight = cv::imread("right_image.png", cv::IMREAD_GRAYSCALE);

    // 创建StereoSGBM对象
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create();

    // 设置SGBM算法的参数
    sgbm->setMinDisparity(0);
    sgbm->setNumDisparities(64);
    sgbm->setBlockSize(5);
    sgbm->setP1(8 * imageLeft.channels() * sgbm->getBlockSize() * sgbm->getBlockSize());
    sgbm->setP2(32 * imageLeft.channels() * sgbm->getBlockSize() * sgbm->getBlockSize());
    sgbm->setDisp12MaxDiff(1);
    sgbm->setPreFilterCap(63);
    sgbm->setUniquenessRatio(10);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(32);

    // 应用SGBM算法进行立体匹配
    cv::Mat disparity;
    sgbm->compute(imageLeft, imageRight, disparity);

    cv::imshow("Disparity", disparity);
    cv::waitKey(0);

    return 0;
}

后处理方式

要显示层次更分明的深度图，您可以对深度图像进行一些后处理操作，例如调整对比度、应用颜色映射或进行阈值化等。以下是几种常见的方法：

1. 调整对比度和亮度：通过调整深度图像的对比度和亮度，可以增强深度图像中不同深度层次的可见性。您可以使用线性或非线性的像素值映射函数来实现这一点。

2. 颜色映射：将深度值映射到不同的颜色，可以更清晰地显示深度图像。例如，您可以使用热度图（从蓝色到红色）或彩虹色映射来表示不同深度值。

   cv::applyColorMap(depth, depthColor, cv::COLORMAP_JET);
   ```
   

3. 阈值化：根据具体的应用需求，您可以对深度图像进行阈值化，并将不同深度范围内的像素设置为不同的值或颜色。这可以帮助您分割出特定深度层次的目标。

   cv::Mat thresholdedDepth;
   cv::threshold(depth, thresholdedDepth, thresholdValue, maxValue, cv::THRESH_BINARY);
   ```
   

4. 深度图像平滑化：通过应用平滑化操作（如高斯滤波或中值滤波），可以去除深度图像中的噪声，使层次结构更加清晰。

   cv::GaussianBlur(depth, smoothedDepth, cv::Size(5, 5), 0);
   ```
   

便于计算点云

具体的代码为

// 将视差图转换为深度图
    double baseline = 5.01066446304321;  // 基线长度（单位：cm）
    double focal_length = 389.365356445312;  // 焦距（单位：像素）
    // cv::Mat depth_map = baseline * focal_length / disparity_map;


	  auto start = std::chrono::high_resolution_clock::now();

    // 定义SGBM参数
    int minDisparity = 0;  // 最小视差
    int numDisparities = 16 *6 ;  // 视差范围的数量
    int blockSize = 16;  // 匹配块的大小
    int P1 = 8 * image_left.channels() * blockSize * blockSize;  // P1参数
    int P2 = 32 * image_right.channels() * blockSize * blockSize;  // P2参数
    int disp12MaxDiff = 2;  // 左右视图一致性检查的最大差异

    // 创建SGBM对象并设置参数
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(minDisparity, numDisparities, blockSize);
    sgbm->setP1(P1);
    sgbm->setP2(P2);
    sgbm->setDisp12MaxDiff(disp12MaxDiff);

    // 计算视差图像
    cv::Mat disparity;
    sgbm->compute(image_left, image_right, disparity);

    // 将视差图像归一化到0-255范围内
    cv::normalize(disparity, disparity, 0, 255, cv::NORM_MINMAX, CV_8U);

    cv::Mat depth_map = baseline * focal_length /(disparity);
    cv::Mat depthColor;
    cv::applyColorMap(depth_map, depthColor, cv::COLORMAP_JET);

    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> duration = end - start;
    double elapsedTime = duration.count();

    std::cout << "compute disparty map time cost = " << elapsedTime << " seconds. " << std::endl;

    // 显示视差图
    cv::imshow("Disparity Map", disparity);
    cv::imshow("Depth Map", depthColor);

    cv::waitKey(1);

示例如下

第二排的左图为视差图，右图为深度的热度图

计算时间为

applyColorMap函数是OpenCV中用于应用颜色映射的函数。它将灰度图像（单通道图像）映射到伪彩色图像（多通道图像），以提高图像的可视化效果。以下是applyColorMap函数的用法说明：

cv::applyColorMap(src, dst, colormap);

参数说明：

• src：输入的灰度图像，必须是单通道图像，如CV_8UC1或CV_32FC1。
• dst：输出的伪彩色图像，必须是多通道图像，如CV_8UC3或CV_32FC3。
• colormap：颜色映射类型，可以是以下常用选项之一：
  • cv::COLORMAP_AUTUMN：秋季色调映射
  • cv::COLORMAP_BONE：骨骼色调映射
  • cv::COLORMAP_JET：喷射色调映射
  • cv::COLORMAP_WINTER：冬季色调映射
  • cv::COLORMAP_RAINBOW：彩虹色调映射
  • cv::COLORMAP_OCEAN：海洋色调映射
  • cv::COLORMAP_SUMMER：夏季色调映射
  • cv::COLORMAP_SPRING：春季色调映射
  • cv::COLORMAP_COOL：凉爽色调映射
  • cv::COLORMAP_HSV：HSV色调映射

根据源图像的像素值，applyColorMap函数将适当的颜色映射应用到目标图像中的像素上，从而生成伪彩色图像。映射的结果会保存在目标图像dst中。

显示点云

点云PCL库学习-双目图像转化为点云PCD并显示

点云PCL库学习-双目图像转化为点云PCD并显示

生成的点云图通过ROS发布，用rviz打开

但是生成的点云看起来很奇怪，经常会有一道线

这是由于计算得到的视差图提前归一化了，导致后续计算时视差图的值不是真值，而是归一化之后的值

// 显示视差图
// 将视差图像归一化到0-255范围内
cv::normalize(disparity, disparity, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::imshow("Disparity Map", disparity);

只有在显示前才应该将视差图或者深度图归一化到其便于显示的值，否则就保持原值，用于计算

这里传出的坐标系为图像坐标系，在 rviz 中，红绿蓝分别代表 x,y,z 轴

要将其改为世界坐标系后再输出

世界坐标系与图像坐标系的关系如下：

图像坐标系：

但是实际上应该是如下的样子

世界坐标系：

或者是如下的形式，两者等价

代码中应该写为

//设为图像坐标系
pcl::PointXYZ point(camera_x, camera_y, camera_z);

//设为世界坐标系
pcl::PointXYZ point(camera_z, -camera_x, -camera_y);

注意这里不是

pcl::PointXYZ point(camera_z, camera_x, -camera_y);

不然这里点云和图像是相反的

步长为3时生成的点云

这里的基线（baseline）单位设置为分米，显示比较合适

如果设置为m，点云会更接近坐标轴

如果设置为cm，点云会更远，稀疏到很难看见

彩色点云

前面生成点云之后，还可以进一步生成彩色点云，用彩色相机给点云上色

 // 生成点云
        // 创建点云对象
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
        // 逐像素计算点云
        for (int y = 0; y < depth_map.rows; y += 1) {
            for (int x = 0; x < depth_map.cols; x += 1) {
                // 获取当前像素的深度值
                float depth = depth_map.at<float>(y, x);

                // 如果深度值为0，表示无效点，跳过
                if (depth == 0)
                    continue;

                // 计算相机坐标系下的点坐标
                float camera_x = (x - cx_424) * depth / fx_424;
                float camera_y = (y - cy_424) * depth / fy_424;
                float camera_z = depth;

                pcl::PointXYZRGB point;
                point.x = camera_z;
                point.y = -camera_x;
                point.z = -camera_y;
                point.r = image_color.at<cv::Vec3b>(y, x)[0];;
                point.g = image_color.at<cv::Vec3b>(y, x)[1];
                point.b = image_color.at<cv::Vec3b>(y, x)[2];

                point_cloud_tmp->push_back(point);
            }
        }

由于这里的彩色图是接收D435发出的，其格式为 rgb8，

所以这里在填充点云的 RGB 分量时，对应的通道为0，1，2，在填充点云的颜色时要注意通道的顺序

彩色点云效果如下