下面是SLAM算法与工程实践系列文章的总链接,本人发表这个系列的文章链接均收录于此
下面是专栏地址:
这个系列的文章是分享SLAM相关技术算法的学习和工程实践
参考:
ROS传输图像带宽不够用的解决方法(realsenseD415压缩图像)
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和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;
要加快立体匹配计算的速度,可以尝试以下几种方法:
cv::resize
函数来缩小图像尺寸。cv::StereoSGBM::MODE_HH
模式来启用GPU加速。这里将图像resize为原来的一半
将双目相机的图像进行调整大小(resize)时,相机的一些参数可能需要相应地进行调整,以保持其对应关系。以下是一些常见的相机参数,以及它们在调整图像大小时应如何变化:
内参
图像分辨率(Image Resolution):调整图像大小会改变图像的宽度和高度,因此相机的图像分辨率需要相应地更新。分辨率应根据调整后的图像大小进行更新。
焦距(Focal Length):焦距是相机的一个重要参数,它与图像的视角和深度感知相关。当调整图像大小时,焦距通常需要按比例进行缩放。可以使用以下公式计算新的焦距:
新焦距 = 原焦距 * (新图像宽度 / 原图像宽度)
这将保持视角不变。
主点(Principal Point):主点是相机坐标系原点在图像上的投影点。当调整图像大小时,主点的位置可能会发生变化。主点的新位置可以通过以下公式计算:
新主点坐标 = 原主点坐标 * (新图像宽度 / 原图像宽度)
这将保持主点在图像上的相对位置不变。
畸变系数(Distortion Coefficients):畸变系数描述了相机镜头的畸变特性。在调整图像大小时,畸变系数通常不需要变化,因为它们是根据图像的几何特征计算得到的,并不直接依赖于图像的大小。
外参(Extrinsic Parameters):相机的外参描述了相机坐标系与世界坐标系之间的变换关系,包括相机的位置和姿态。当调整图像大小时,外参也需要相应地进行更新。具体地,可以通过以下步骤更新外参:
需要注意的是,以上公式假设图像的宽高比保持不变。如果调整图像大小后的宽高比与原始图像不同,那么需要根据实际情况进行适当的调整。
此外,还应注意,调整图像大小可能会影响立体匹配算法的性能和准确性。一些立体匹配算法可能对图像分辨率和焦距变化敏感,因此在调整图像大小时应谨慎评估和验证立体匹配的效果。
在代码中体现为将双目相机的内参矩阵、生成点云计算需要的焦距乘以缩放系数
归一化平面坐标和像素坐标的关系:
成像平面到像素坐标
{
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帧