cartographer 3d 初试，运行速腾聚创data

Posted 2021-07-26 Hill_LAI

不黑不吹，这个文章不错
谷歌cartographer使用速腾聚创3d激光雷达数据进行三维建图

结合文章，先上图

再上数据
链接: https://pan.baidu.com/s/17P_NhPinVU6zlNG5p01PZw 提取码: 63gv

再上配置文件urdf

<!--
  Copyright 2016 The Cartographer Authors

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->

<robot name="cartographer_backpack_3d">
  <material name="orange">
    <color rgba="1.0 0.5 0.2 1" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 1" />
  </material>

  <link name="imu">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="rslidar">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="vertical_vlp16_link">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="base_link" />

  <joint name="imu_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

  <joint name="rslidar_joint" type="fixed">
    <parent link="base_link" />
    <child link="rslidar" />
    <origin xyz="-0.25 0 0.4" rpy="0. 0 0" />
  </joint>

  <joint name="vertical_vlp16_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="vertical_vlp16_link" />
    <origin xyz="0.19 0. 0.04" rpy="0. 1.3963 0." />
  </joint>
</robot>

再上launch文件

<!--
  Copyright 2016 The Cartographer Authors

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->

<launch>

  <arg name="bag_filename" default="/home/your_name/Downloads/ros_bag/Bluewhale/2018-08-11-13-20-34.bag" />
 
  <param name="/use_sim_time" value="true" />

  <include file="$(find cartographer_ros)/launch/backpack_3d.launch" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="-r 3 --clock $(arg bag_filename)" />
</launch>

再上backpack_3d.launch文件

<!--
  Copyright 2016 The Cartographer Authors

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_3d_peak_bluewhale.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_3d_peak.lua"
      output="screen">
    <!-- remap from="points2" to="points_raw" /-->
    <!-- remap from="imu" to="imu_correct" /-->

    <!-- remap from="points2" to="horizontal_laser_3d" /-->
    <!-- remap from="points2_2" to="vertical_laser_3d" /-->

    <!-- remap from="points2" to="kitti/velo/pointcloud" /-->
    <!-- remap from="imu" to="kitti/oxts/imu" /-->

    <!-- remap from="points2" to="rslidar_points" /-->
    <!-- remap from="imu" to="imu_data" /-->

    <remap from="points2" to="rslidar_points" />
    <remap from="imu" to="xqserial_server/IMU" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

再上配置文件lua，这个很重要，影响到是否能建好图

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = true, --false
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1., --1.
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1 --160
TRAJECTORY_BUILDER_3D.min_range = 0.2
TRAJECTORY_BUILDER_3D.max_range = 20.
TRAJECTORY_BUILDER_2D.min_z = 0.1
TRAJECTORY_BUILDER_2D.max_z = 1.0
TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = false

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 4 --7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 40 --320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 20 --10
POSE_GRAPH.constraint_builder.min_score = 0.5 --0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.55 -0.66
POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d = true

return options

最后运行第一个launch文件就可以了，要在launch文件中配置好你的urdf、lua文件，可以啦