Cartographer with pointcloud from rs camera (ROS1)

[Splinter1984]

---

Required Info
Camera Model	D435i / T265
Operating System & Version	Linux (Ubuntu 18)
Platform	Intel NUC
Segment	Robot

Issue Description

Hi, I want to use t265 camera as odometry source for cartographer and d435i camera as point cloud source. I am facing a global SLAM problem. As far as I understand, the distance of the points perceived by the algorithm, set by me, is small for the successful compilation of a submap, therefore, the map is stitched incorrectly. But the fact is that when I set the distance too large, I get a lot of noise, which does not make the situation better. Has anyone had any experience with the d435i camera as a point cloud source for a cartographer?

my cartographer launch file

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" 
        doc="model type [burger, waffle, waffle_pi, test_07]"/>
  <arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
  <arg name="optical_rotate" value="0 0.11 1.01 0 0 -1.57" /> 
  <arg name="optical_rotate2" value="0 0 0 0 0 0"/>

  <!-- move_base -->
  <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  <!-- cartographer_node -->
  <node pkg="cartographer_ros" type="cartographer_node" name="cartographer_node" 
        args="-configuration_directory $(find turtlebot3_slam)/config
              -configuration_basename $(arg configuration_basename)"
        output="screen">
    <remap from="imu"     to="/camera_stereo/imu"/>
    <remap from="points2" to="/camera/depth/color/points"/>
    <!-- <remap from="scan"    to="/scan"/> -->
    <remap from="odom"    to="/camera_stereo/odom/sample"/>
  </node>

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

  <node pkg="tf" type="static_transform_publisher" name="camera_stereo_odom_link"
     args="$(arg optical_rotate2) camera_stereo_pose_frame base_footprint 100" />

  <node pkg="tf" type="static_transform_publisher" name="camera_stereo_base_link"
     args="$(arg optical_rotate2) base_link camera_stereo_link 100" />

</launch>

my config lua file

include "map_builder.lua"
include "trajectory_builder.lua"
-- some helpful stuff
-- https://medium.com/robotics-weekends/2d-mapping-using-google-cartographer-and-rplidar-with-raspberry-pi-a94ce11e44c
-- https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "camera_stereo_imu_optical_frame", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug)
  published_frame = "camera_stereo_odom_frame",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  
  use_odometry = true,
  use_nav_sat = false, -- forget about that --
  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.3,
  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 = 0.1,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 0.1,
  landmarks_sampling_ratio = 1.,
}

-- tunning guide
-- https://google-cartographer-ros.readthedocs.io/en/latest/tuning.html

-- Cartographer configuration options:
-- https://google-cartographer.readthedocs.io/en/latest/configuration.html


MAP_BUILDER.use_trajectory_builder_2d = true
MAP_BUILDER.num_background_threads = 7

--Local Slam
--there are more parameters to tune, but this ones are the ones I found more impactful

TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.07
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 160
-- TRAJECTORY_BUILDER_2D.min_num_points = 1e3
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1

--bandpass filter for source distance measurements
TRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range = 4.0
TRAJECTORY_BUILDER_2D.max_z = 3.0
TRAJECTORY_BUILDER_2D.min_z = 0.2
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 2.5

--use or not use IMU, if used, the tracking_frame should be set to the one that the IMU is on
TRAJECTORY_BUILDER_2D.use_imu_data = true
--this one tries to match two laser scans together to estimate the position,
--I think if not on it will rely more on wheel odometry
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false  -- preferably use if laser is the only data

TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1

--This is the scan matcher and the weights to different assumptions
--occupied_space gives more weight to the 'previous' features detected.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.

POSE_GRAPH.constraint_builder.min_score = 0.6
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.4
POSE_GRAPH.constraint_builder.max_constraint_distance = 2.5
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 2.5

--this will help continue making the map while the robot is static
--default time is 5 seconds
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.1

--map output parameters

--Global Slam
--Setting POSE_GRAPH.optimize_every_n_nodes to 0 is a handy way
--to disable global SLAM and concentrate on the behavior of local SLAM.
--This is usually one of the first thing to do to tune Cartographer.
POSE_GRAPH.optimize_every_n_nodes = 3000. --90 default
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1.
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1000.
POSE_GRAPH.optimization_problem.huber_scale = 1e1
POSE_GRAPH.optimization_problem.fixed_frame_pose_translation_weight = 1e-1
POSE_GRAPH.optimization_problem.fixed_frame_pose_rotation_weight = 1e-1

TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.2

return options

---