SLAM算法与工程实践——相机篇:传统相机使用(3)

发布时间:2023年12月17日

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

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

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


下面是专栏地址:

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



前言

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


SLAM算法与工程实践——相机篇:传统相机使用(3)

发布相机图像

参考:

ROS传输图像带宽不够用的解决方法(realsenseD415压缩图像)

ros图像处理相关

ROS工作空间中,可执行文件会生成到 devel/lib 中,如果是clion开发,则会生成到 cmake-build-debug/devel/lib

以下是关于在ROS中发布图像消息时使用的选项的说明:

  1. raw:使用raw选项时,图像以原始格式(未经过编码或压缩)发布。这意味着图像以原始的像素值形式传输,没有进行任何额外处理。这种选项适用于不需要进行图像编码或压缩的应用场景,或者处理图像的过程已在其他地方完成。

  2. theoratheora是一种开源视频编码格式,适用于图像压缩。使用theora选项时,图像将被编码为theora格式,并以视频流的形式发布。这种选项适用于需要更高压缩率的图像,但可能会导致一些视觉质量损失。

    image_transport::ImageTransport it(nh);
    image_transport::Publisher theora_publisher = it.advertise("theora_image", 1, image_transport::TransportHints("theora"));
    
  3. compressed:使用compressed选项时,图像将使用ROS中的压缩图像消息格式进行压缩。压缩图像消息使用不同的压缩算法,如JPEG、PNG等。发布图像时,可以指定使用的压缩算法和压缩质量。这种选项适用于需要较高压缩率但仍保持较好视觉质量的图像。

    image_transport::ImageTransport it(nh);
    image_transport::Publisher jpeg_publisher = it.advertise("compressed_image", 1, image_transport::TransportHints("jpeg"));
    
  4. compressedDepth:使用compressedDepth选项时,适用于深度图像的压缩版本。深度图像通常包含浮点数值,因此将其压缩为标准图像消息可能会导致较高的数据量。使用compressedDepth选项,深度图像将被转换为16位整数形式,并进行压缩。这种选项适用于需要传输深度信息但对数据量有限制的应用场景。

    image_transport::ImageTransport it(nh);
    image_transport::Publisher compressed_depth_publisher = it.advertise("compressed_depth_image", 1, image_transport::TransportHints("compressedDepth"));
    

这些选项可通过ROS中的image_transport包和相应的图像传输插件来实现。你可以根据你的需求选择适当的选项,并根据发布者和订阅者之间的带宽和延迟要求进行权衡。

发送图像时编码格式选择

注意:

在发布相机图像时,编码方式不要选择 mono8,不然发不出来的是压缩过的图像,如下所示

sensor_msgs::ImagePtr left_image_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", undisFrameL).toImageMsg();

在这里插入图片描述

编码方式要选择 bgr8,正确的结果如下所示

sensor_msgs::ImagePtr left_image_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", undisFrameL).toImageMsg();

在这里插入图片描述

在发送视差图和深度图,编码的格式要做选择

发布原始的视差图和深度图,应该是 32FC1,因为其数据给是为浮点数类型

由于原始的视差图和深度图不方便显示,所以我这里将其像素值归一化到了[0,255]区间

cv::normalize(disparity, disparity_tmp, 0, 255, cv::NORM_MINMAX, CV_8UC1);

cv::normalize(depth_map_tmp, depth_map_tmp, 0, 255, cv::NORM_MINMAX, CV_8UC1);

cv::applyColorMap(depth_map_tmp, depthColor, cv::COLORMAP_JET);

cv::normalize(depthColor, depthColor, 0, 255, cv::NORM_MINMAX, CV_8UC1);

所以在传输归一化之后的图像,应该设为 8UC1 的格式

而由于depthColor是由depth_map_tmp生成的伪彩色图,所以应该用 bgr8 格式

disparity_map_msg = cv_bridge::CvImage(std_msgs::Header(), "8UC1", disparity_tmp).toImageMsg();

disparity_map_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", disparity).toImageMsg();


depth_map_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", depthColor).toImageMsg();

depth_map_msg = cv_bridge::CvImage(std_msgs::Header(), "8UC1", depth_map_tmp).toImageMsg();


depth_map_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_map).toImageMsg();

效果如下

在这里插入图片描述

发布对象初始化

在发布图像时,需要创建 ImageTransport 对象,进行图像传输相关的操作

// 创建ImageTransport对象,进行图像传输相关的操作
image_transport::ImageTransport it(cam1.nh_);

注意:不能在自定义类的构造函数中设置 image_transport::ImageTransport 类的对象的初始化,如果要初始化,代码示例如下

#include <ros/ros.h>
#include <image_transport/image_transport.h>

class MyImagePublisher
{
public:
  MyImagePublisher() : nh_(""), it_(nh_)
  {
    // 在构造函数中初始化 nh_ 和 it_
    image_pub_ = it_.advertise("image_topic", 1);
  }

  void publishImage(const sensor_msgs::ImagePtr& image_msg)
  {
    image_pub_.publish(image_msg);
  }

private:
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Publisher image_pub_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_publisher_node");
  
  MyImagePublisher image_publisher;

  // 其他逻辑...
  
  ros::spin();

  return 0;
}

在构造函数的后面初始化

在这里插入图片描述

打上时间戳

在ROS中,如果你想要在发布图像消息时添加时间戳,你可以使用ros::Time::now()函数来获取当前的时间,并将其赋值给图像消息的header.stamp字段。

示例如下:

// 设置图像消息的时间戳
image_msg->header.stamp = ros::Time::now();

将图像发布后,如何查看是否带有时间戳

rostopic echo -p -b 1 -c image_topic | grep -v "data:"

在上述命令中,我们使用了以下选项:

  • -p:以可打印的格式(plain)输出消息内容。
  • -b 1:只输出最新的一条消息。
  • -c:清除屏幕上的旧消息。

当然,可以只用下面的命令即可

rostopic echo image_topic | grep -v "data:"

即将图像信息中的 data: 关键字排除,显示其它的信息

topic设置

在设置发布对象的topic时,一定要在循环外面设置好,进入循环再设置的话会降低发送的帧率

我这里将发送函数封装成了函数,可以自己设置topic和queue_size

void UseStereoCam::setStereoTopic(image_transport::Publisher &frame_pub, std::string &topic, int queue_size) {
    // 设置发布话题
    frame_pub = it_.advertise(topic, queue_size);
}

可以在终端用命令查看和操作ROS话题,以下是几个常用的rostopic命令:

rostopic list:列出当前正在运行的ROS节点发布和订阅的所有话题。

rostopic list

rostopic echo:订阅并打印指定话题的消息内容。

rostopic echo <topic_name>

例如,要打印名为/my_topic的话题的消息内容,可以运行:

rostopic echo /my_topic

rostopic pub:发布指定话题的消息。

rostopic pub <topic_name> <message_type> <args>

例如,要向名为/my_topic的话题发布一个std_msgs/String类型的消息,可以运行:

 rostopic pub /my_topic std_msgs/String "data: 'Hello, world!'"

rostopic hz:计算指定话题的发布频率。

rostopic hz <topic_name>

例如,要计算名为/my_topic的话题的发布频率,可以运行:

rostopic hz /my_topic

rostopic info:显示指定话题的详细信息,包括消息类型和发布者/订阅者的信息。

rostopic info <topic_name>

在这里插入图片描述

rostopic bw:该命令用于计算指定话题的带宽使用情况,包括每秒传输的字节数。

要查看图像消息的大小,可以执行以下命令:

rostopic bw <topic_name>

在这里插入图片描述

出现的错误

在编译时报错 image_transport::ImageTransport::ImageTransport(ros::NodeHandle const&) 未定义

/usr/bin/ld: /home/jin/catkin_ws/devel/lib/libdepth_rs.so: undefined reference to `image_transport::ImageTransport::ImageTransport(ros::NodeHandle const&)'
/usr/bin/ld: /home/jin/catkin_ws/devel/lib/libdepth_rs.so: undefined reference to `image_transport::Publisher::publish(boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&) const'
/usr/bin/ld: /home/jin/catkin_ws/devel/lib/libdepth_rs.so: undefined reference to `image_transport::ImageTransport::advertise(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int, bool)'
/usr/bin/ld: /home/jin/catkin_ws/devel/lib/libdepth_rs.so: undefined reference to `image_transport::ImageTransport::~ImageTransport()'
collect2: error: ld returned 1 exit status
make[2]: *** [depth_rs/CMakeFiles/pub_img_node.dir/build.make:263: /home/jin/catkin_ws/devel/lib/depth_rs/pub_img_node] Error 1
make[1]: *** [CMakeFiles/Makefile2:2670: depth_rs/CMakeFiles/pub_img_node.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....

这是因为 CMakeLists.txt 配置的问题,要加上 image_transport 包

find_package(catkin REQUIRED COMPONENTS
        roscpp
        rospy
        std_msgs
        cv_bridge
        pcl_ros
        image_transport
)

立体匹配

焦距

对于双目相机,通常会有左右两个相机的内参矩阵。要计算双目相机的焦距,需要首先获取左右相机的焦距,然后对其进行处理,用其平均值即可

// 左相机内参矩阵
    cv::Mat leftCameraMatrix = (cv::Mat_<double>(3, 3) << fx_left, 0, cx_left, 0, fy_left, cy_left, 0, 0, 1);

    // 右相机内参矩阵
    cv::Mat rightCameraMatrix = (cv::Mat_<double>(3, 3) << fx_right, 0, cx_right, 0, fy_right, cy_right, 0, 0, 1);

    // 获取左相机焦距
    double leftFocalLength = (leftCameraMatrix.at<double>(0, 0) + leftCameraMatrix.at<double>(1, 1)) / 2.0;

    // 获取右相机焦距
    double rightFocalLength = (rightCameraMatrix.at<double>(0, 0) + rightCameraMatrix.at<double>(1, 1)) / 2.0;

    // 处理焦距(例如取平均值)
    double focalLength = (leftFocalLength + rightFocalLength) / 2.0;

    // 打印焦距
    std::cout << "Focal Length: " << focalLength << std::endl;

基线长度

在双目相机中,基线长度表示左右相机之间的距离。要从双目相机的外参矩阵中获取基线长度,需要提取平移向量,并计算其范数,即向量的长度

    // 双目相机外参矩阵
    cv::Mat stereoExtrinsics = (cv::Mat_<double>(4, 4) <<
        r11, r12, r13, tx,
        r21, r22, r23, ty,
        r31, r32, r33, tz,
        0,   0,   0,   1);

    // 提取平移向量
    cv::Mat translationVector = stereoExtrinsics.col(3).rowRange(0, 3);

    // 计算基线长度(平移向量的范数)
    double baselineLength = cv::norm(translationVector);

    // 打印基线长度
    std::cout << "Baseline Length: " << baselineLength << std::endl;

加快立体匹配速度

要加快立体匹配计算的速度,可以尝试以下几种方法:

  1. 降低图像分辨率:将输入图像的分辨率降低,可以减少立体匹配算法的计算量。可以使用OpenCV的cv::resize函数来缩小图像尺寸。
  2. 使用更快的匹配算法:尝试使用更快速的立体匹配算法,如ELAS(Efficient Large-scale Stereo)算法、SGBM(Semi-Global Block Matching)算法等,这些算法在速度和效果上都有一定的优化。
  3. 调整匹配算法参数:对于特定的立体图像对,可以尝试调整匹配算法的参数以提高计算速度。例如,减小搜索窗口的大小或调整匹配代价计算的参数。
  4. 使用GPU加速:一些立体匹配算法支持在GPU上进行加速,可以利用图形处理单元(GPU)的并行计算能力来加速立体匹配的计算。例如,OpenCV的SGBM算法可以通过设置cv::StereoSGBM::MODE_HH模式来启用GPU加速。
  5. 并行计算:如果你的系统具有多个核心或多个计算单元,可以考虑使用多线程或并行计算库(如OpenMP)来并行化立体匹配的计算过程。
  6. 硬件加速:如果速度仍然不够满意,你可以考虑使用专门的硬件加速卡(如NVIDIA的CUDA加速卡)来进行立体匹配的计算,以获得更高的计算性能。

这里将图像resize为原来的一半

将双目相机的图像进行调整大小(resize)时,相机的一些参数可能需要相应地进行调整,以保持其对应关系。以下是一些常见的相机参数,以及它们在调整图像大小时应如何变化:

内参

  1. 图像分辨率(Image Resolution):调整图像大小会改变图像的宽度和高度,因此相机的图像分辨率需要相应地更新。分辨率应根据调整后的图像大小进行更新。

  2. 焦距(Focal Length):焦距是相机的一个重要参数,它与图像的视角和深度感知相关。当调整图像大小时,焦距通常需要按比例进行缩放。可以使用以下公式计算新的焦距:

    新焦距 = 原焦距 * (新图像宽度 / 原图像宽度)

    这将保持视角不变。

  3. 主点(Principal Point):主点是相机坐标系原点在图像上的投影点。当调整图像大小时,主点的位置可能会发生变化。主点的新位置可以通过以下公式计算:

    新主点坐标 = 原主点坐标 * (新图像宽度 / 原图像宽度)

    这将保持主点在图像上的相对位置不变。

  4. 畸变系数(Distortion Coefficients):畸变系数描述了相机镜头的畸变特性。在调整图像大小时,畸变系数通常不需要变化,因为它们是根据图像的几何特征计算得到的,并不直接依赖于图像的大小。

外参(Extrinsic Parameters):相机的外参描述了相机坐标系与世界坐标系之间的变换关系,包括相机的位置和姿态。当调整图像大小时,外参也需要相应地进行更新。具体地,可以通过以下步骤更新外参:

  1. 平移向量(Translation Vector):平移向量表示相机的位置,它的值通常以相机坐标系为单位。当调整图像大小时,平移向量需要按比例进行缩放,与图像大小的变化保持一致。
  2. 旋转矩阵(Rotation Matrix):旋转矩阵描述相机的姿态,它通常是一个3x3的矩阵。当调整图像大小时,旋转矩阵不需要变化,因为它只描述了相机的方向,而不涉及图像大小。

需要注意的是,以上公式假设图像的宽高比保持不变。如果调整图像大小后的宽高比与原始图像不同,那么需要根据实际情况进行适当的调整。

此外,还应注意,调整图像大小可能会影响立体匹配算法的性能和准确性。一些立体匹配算法可能对图像分辨率和焦距变化敏感,因此在调整图像大小时应谨慎评估和验证立体匹配的效果。

在代码中体现为将双目相机的内参矩阵、生成点云计算需要的焦距乘以缩放系数

计算公式总结

归一化平面坐标和像素坐标的关系:

成像平面到像素坐标
{ u = α X ′ + c x v = β Y ′ + c y . \begin{cases}u=\alpha X'+c_x\\ v=\beta Y'+c_y\end{cases}. {u=αX+cx?v=βY+cy??.

代入 { X ′ = f X Z Y ′ = f Y Z \begin{cases}X^{\prime}=f{\frac{X}{Z}}\\Y^{\prime}=f{\frac{Y}{Z}}\end{cases} {X=fZX?Y=fZY??

得展开形式:
{ u = f x X Z + c x v = f y Y Z + c y . \begin{cases}u=f_x\frac{X}{Z}+c_x\\ v=f_y\frac{Y}{Z}+c_y\end{cases}. {u=fx?ZX?+cx?v=fy?ZY?+cy??.
其中, f f f 的单位为 α , β \alpha,\beta α,β 的单位为像素/米,所以 f x , f y f_x,f_y fx?,fy? c x , c y c_x,c_y cx?,cy? 的单位为像素。

像素坐标和真实坐标的关系
Z ( u v 1 ) = ( f x 0 c x 0 f y c y 0 0 1 ) ( X Y Z ) ? K P . Z\begin{pmatrix}u\\ v\\ 1\end{pmatrix}=\begin{pmatrix}f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1\end{pmatrix}\begin{pmatrix}X\\ Y\\ Z\end{pmatrix}\triangleq KP. Z ?uv1? ?= ?fx?00?0fy?0?cx?cy?1? ? ?XYZ? ??KP.
该式中,我们把中间的量组成的矩阵称为相机的内参数(Camera Intrinsics)矩阵K

对于相机坐标系中的一点 P P P,我们能够通过5个畸变系数找到这个点在像素平面上的正确位置:

(1)将三维空间点投影到归一化图像平面。设它的归一化坐标为 [ x , y ] T [x,y]^T [x,y]T

(2)对归一化平面上的点计算径向畸变和切向畸变。
{ x d i s t o r t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y d i s t o r t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y \left\{\begin{array}{l}{x_{\mathrm{distorted}}=x(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6})+2p_{1}x y+p_{2}(r^{2}+2x^{2})}\\ {y_{\mathrm{distorted}}=y(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6})+p_{1}(r^{2}+2y^{2})+2p_{2}x y}\\ \end{array}\right. {xdistorted?=x(1+k1?r2+k2?r4+k3?r6)+2p1?xy+p2?(r2+2x2)ydistorted?=y(1+k1?r2+k2?r4+k3?r6)+p1?(r2+2y2)+2p2?xy?
(3)将畸变后的点通过内参数矩阵投影到像素平面,得到该点在图像上的正确位置。
{ u = f x x distorted + c x v = f y y distoned + c y . \begin{cases}u=f_x x_{\text{distorted}}+c_x\\ v=f_y y_{\text{distoned}}+c_y\end{cases}. {u=fx?xdistorted?+cx?v=fy?ydistoned?+cy??.
在上面的纠正畸变的过程中,我们使用了5个畸变项。实际应用中,可以灵活选择纠正模型比如只选择 k 1 , k 2 , p 1 , p 2 k_1,k_2,p_1,p_2 k1?,k2?,p1?,p2? 这4项,或者只选择 k 1 , p 1 , p 2 k_1,p_1,p_2 k1?,p1?,p2? 这3项。

深度 z z z 和焦距的关系
z ? f z = b ? u L + u R b . \frac{z-f}{z}=\frac{b-u_\mathrm{L}+u_\mathrm{R}}{b}. zz?f?=bb?uL?+uR??.
稍加整理,得
z = f b d , d = def u L ? u R . z=\frac{fb}{d},\quad d\overset{\text{def}}{=}u_L-u_R. z=dfb?,d=defuL??uR?.
其中 d d d 定义为左右图的横坐标之差,称为视差。

视差与距离成反比:视差越大,距离越近。同时,由于视差最小为一个像素,于是双目的深度存在一个理论上的最大值,由 b b b 确定

生成点云

在声明pcl::PointCloud<pcl::PointXYZ>::Ptr对象后,可以使用reset方法来初始化它。

以下是一个示例:

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

int main()
{
    // 声明点云指针
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;

    // 初始化点云指针
    cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);

    // 在这里进行其他操作,如添加点、访问点云数据等

    return 0;
}

在上述示例中,首先声明了pcl::PointCloud<pcl::PointXYZ>::Ptr类型的点云指针cloud,然后使用reset方法将其初始化为指向新创建pcl::PointCloud<pcl::PointXYZ>对象的指针。

通过这种方式,你可以在需要的时候重新初始化点云指针,例如在处理不同的点云数据时或在循环中重复使用指针。

请确保在使用点云指针之前进行适当的初始化,以避免访问未分配的内存或出现其他错误。

生成的点云如下所示

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

这时计算视差图,深度图,以及点云图的时间分别如下所示

在这里插入图片描述

查看点云的帧率,将点云发布出去后,使用命令

rostopic hz /point_cloud

稳定在了30帧

在这里插入图片描述

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