保存.pbstream地图-.pbstream地图转pgm,yaml文件,参考此处
对于.pbstream文件生成ply及pcd文件如下:
bag包对应生成的.pbstream文件(下面代码中的*请自行改成自己的文件名),
roslaunch cartographer_ros assets_writer_my_3d.launch bag_filenames:=/home/hyper/Downloads/***.bag pose_graph_filename:=/home/hyper/Downloads/***.pbstream
执行完此句,会生成对应的ply文件,ply转pcd如下:
pcl_ply2pcd ***.bag_points.ply ***.pcd
可视化pcd点云可用如下命令:
pcl_viewer ***.pcd
对于assets_writer_my_3d.launch文件如下:
<launch>
<node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
type="cartographer_assets_writer" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename assets_writer_my_3d.lua
-urdf_filename $(find cartographer_ros)/urdf/my_backpack_3d.urdf
-bag_filenames $(arg bag_filenames)
-pose_graph_filename $(arg pose_graph_filename)"
output="screen">
</node>
</launch>
launch文件里对应的assets_writer_my_3d.lua文件如下:
VOXEL_SIZE = 5e-2
include "transform.lua"
options = {
tracking_frame = "base_link",
pipeline = {
{
action = "min_max_range_filter",
min_range = 1.,
max_range = 60.,
},
{
action = "dump_num_points",
},
{
action = "fixed_ratio_sampler",
sampling_ratio = 0.01,
},
{
action = "write_ply",
filename = "points.ply"
},
-- Gray X-Rays. These only use geometry to color pixels.
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all",
transform = YZ_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy_all",
transform = XY_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xz_all",
transform = XZ_TRANSFORM,
},
-- Now we recolor our points by frame and write another batch of X-Rays. It
-- is visible in them what was seen by the horizontal and the vertical
-- laser.
{
action = "color_points",
frame_id = "world",
color = { 255., 0., 0. },
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all_color",
transform = YZ_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy_all_color",
transform = XY_TRANSFORM,
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xz_all_color",
transform = XZ_TRANSFORM,
},
}
}
return options
最主要的就是修改lua文件中的tracking_frame和frame_id。