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

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

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

---

下面是专栏地址：

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

---

文章目录

• SLAM算法与工程实践系列文章
• • SLAM算法与工程实践系列文章链接
  • SLAM算法与工程实践系列专栏
• 前言
• SLAM算法与工程实践——相机篇：传统相机使用（3）
• • 发布相机图像
  • • 发送图像时编码格式选择
    • 发布对象初始化
    • 打上时间戳
    • topic设置
    • 出现的错误
  • 立体匹配
  • • 焦距
    • 基线长度
    • 加快立体匹配速度
    • 计算公式总结
  • 生成点云

---

前言

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

---

SLAM算法与工程实践——相机篇：传统相机使用（3）

发布相机图像

参考：

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

ros图像处理相关

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

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

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

2. theora：theora是一种开源视频编码格式，适用于图像压缩。使用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的矩阵。当调整图像大小时，旋转矩阵不需要变化，因为它只描述了相机的方向，而不涉及图像大小。

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

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

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

计算公式总结

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

成像平面到像素坐标
$$\{\begin{array}{l}u=\alpha X^{\prime }+c_x\\ v=\beta Y^{\prime }+c_y\end{array}.$$

代入 $\{\begin{array}{l}{X}^{\prime }=f\frac{X}{Z}\\ Y^{\prime }=f\frac{Y}{Z}\end{array}$

得展开形式：
$$\{\begin{array}{l}u=f_x\frac{X}{Z}+c_x\\ v=f_y\frac{Y}{Z}+c_y\end{array}.$$
其中，$f$ 的单位为米，$\alpha ,\beta$ 的单位为像素/米，所以 $f_x,f_y$ 和 $c_x,c_y$ 的单位为像素。

像素坐标和真实坐标的关系
$$Z\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)\triangleq KP.$$
该式中，我们把中间的量组成的矩阵称为相机的内参数(Camera Intrinsics)矩阵K。

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

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

（2）对归一化平面上的点计算径向畸变和切向畸变。
$$\{\begin{array}{l}{x}_{\mathrm{distorted}}=x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{1}xy+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}xy\end{array}$$
（3）将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。
$$\{\begin{array}{l}u=f_x{x}_{\text{distorted}}+c_x\\ v=f_y{y}_{\text{distoned}}+c_y\end{array}.$$
在上面的纠正畸变的过程中，我们使用了5个畸变项。实际应用中，可以灵活选择纠正模型比如只选择 $k_1,k_2,p_1,p_2$ 这4项，或者只选择 $k_1,p_1,p_2$ 这3项。

深度 $z$ 和焦距的关系
$$\frac{z-f}{z}=\frac{b-u_{\mathrm{L}}+u_{\mathrm{R}}}{b}.$$
稍加整理，得
$$z=\frac{fb}{d},d\stackrel{\text{def}}{=}{u}_L-u_R.$$
其中 $d$ 定义为左右图的横坐标之差，称为视差。

视差与距离成反比：视差越大，距离越近。同时，由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由 $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帧