SLAM算法与工程实践——相机篇:RealSense D435使用(2)

发布时间:2023年12月28日

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

在这里插入图片描述
在这里插入图片描述

平移向量的单位是米

相机的内参矩阵是
[ f x s c x 0 f y c y 0 0 1 ] \left[\begin{matrix} f_x&s&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{matrix}\right] ?fx?00?sfy?0?cx?cy?1? ?
f x , f y f_x,f_y fx?,fy? 为焦距,一般情况下,二者相等。

x 0 , y 0 x_0,y_0 x0?,y0? 为主坐标(相对于成像平面)。

s s 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_8UC1CV_32FC1
  • dst:输出的伪彩色图像,必须是多通道图像,如CV_8UC3CV_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,在填充点云的颜色时要注意通道的顺序

彩色点云效果如下

在这里插入图片描述

文章来源:https://blog.csdn.net/qq_37648371/article/details/135005259
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。