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中发布图像消息时使用的选项的说明：
-
raw：使用
raw选项时，图像以原始格式（未经过编码或压缩）发布。这意味着图像以原始的像素值形式传输，没有进行任何额外处理。这种选项适用于不需要进行图像编码或压缩的应用场景，或者处理图像的过程已在其他地方完成。 -
theora：
theora是一种开源视频编码格式，适用于图像压缩。使用theora选项时，图像将被编码为theora格式，并以视频流的形式发布。这种选项适用于需要更高压缩率的图像，但可能会导致一些视觉质量损失。image_transport::ImageTransport it(nh); image_transport::Publisher theora_publisher = it.advertise("theora_image", 1, image_transport::TransportHints("theora")); -
compressed：使用
compressed选项时，图像将使用ROS中的压缩图像消息格式进行压缩。压缩图像消息使用不同的压缩算法，如JPEG、PNG等。发布图像时，可以指定使用的压缩算法和压缩质量。这种选项适用于需要较高压缩率但仍保持较好视觉质量的图像。image_transport::ImageTransport it(nh); image_transport::Publisher jpeg_publisher = it.advertise("compressed_image", 1, image_transport::TransportHints("jpeg")); -
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;
加快立体匹配速度
要加快立体匹配计算的速度，可以尝试以下几种方法：
- 降低图像分辨率：将输入图像的分辨率降低，可以减少立体匹配算法的计算量。可以使用OpenCV的
cv::resize函数来缩小图像尺寸。 - 使用更快的匹配算法：尝试使用更快速的立体匹配算法，如ELAS（Efficient Large-scale Stereo）算法、SGBM（Semi-Global Block Matching）算法等，这些算法在速度和效果上都有一定的优化。
- 调整匹配算法参数：对于特定的立体图像对，可以尝试调整匹配算法的参数以提高计算速度。例如，减小搜索窗口的大小或调整匹配代价计算的参数。
- 使用GPU加速：一些立体匹配算法支持在GPU上进行加速，可以利用图形处理单元（GPU）的并行计算能力来加速立体匹配的计算。例如，OpenCV的SGBM算法可以通过设置
cv::StereoSGBM::MODE_HH模式来启用GPU加速。 - 并行计算：如果你的系统具有多个核心或多个计算单元，可以考虑使用多线程或并行计算库（如OpenMP）来并行化立体匹配的计算过程。
- 硬件加速：如果速度仍然不够满意，你可以考虑使用专门的硬件加速卡（如NVIDIA的CUDA加速卡）来进行立体匹配的计算，以获得更高的计算性能。
这里将图像resize为原来的一半
将双目相机的图像进行调整大小（resize）时，相机的一些参数可能需要相应地进行调整，以保持其对应关系。以下是一些常见的相机参数，以及它们在调整图像大小时应如何变化：
内参
-
图像分辨率（Image Resolution）：调整图像大小会改变图像的宽度和高度，因此相机的图像分辨率需要相应地更新。分辨率应根据调整后的图像大小进行更新。
-
焦距（Focal Length）：焦距是相机的一个重要参数，它与图像的视角和深度感知相关。当调整图像大小时，焦距通常需要按比例进行缩放。可以使用以下公式计算新的焦距：
新焦距 = 原焦距 * (新图像宽度 / 原图像宽度)
这将保持视角不变。
-
主点（Principal Point）：主点是相机坐标系原点在图像上的投影点。当调整图像大小时，主点的位置可能会发生变化。主点的新位置可以通过以下公式计算：
新主点坐标 = 原主点坐标 * (新图像宽度 / 原图像宽度)
这将保持主点在图像上的相对位置不变。
-
畸变系数（Distortion Coefficients）：畸变系数描述了相机镜头的畸变特性。在调整图像大小时，畸变系数通常不需要变化，因为它们是根据图像的几何特征计算得到的，并不直接依赖于图像的大小。
外参（Extrinsic Parameters）：相机的外参描述了相机坐标系与世界坐标系之间的变换关系，包括相机的位置和姿态。当调整图像大小时，外参也需要相应地进行更新。具体地，可以通过以下步骤更新外参：
- 平移向量（Translation Vector）：平移向量表示相机的位置，它的值通常以相机坐标系为单位。当调整图像大小时，平移向量需要按比例进行缩放，与图像大小的变化保持一致。
- 旋转矩阵（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′+cxv=β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′=fZXY′=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=fxZX+cxv=fyZY+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
=
fx000fy0cxcy1
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+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)ydistorted=y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy
（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=fxxdistorted+cxv=fyydistoned+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帧
