Is possible to use a 3D lidar with a 2D lidar to get more accuracy?

[Cristian-wp]

Hello, I am trying to use a 3D Velodyne VLP-16 wirth a 2D lidar (mounted in vertical position) in order to get more accuracy on the z axis. Actually I have manage to use Cartographer only with an IMU and the above 3D lidar, but sometimes the Z axis get a wrong value (with a big error). I think the problem is that I have few features detected from the 3D lidar, so I think that use a distance sensor (with message converted from Range to LaserScan data) or maybe directly a laserscan like the LDS-01(https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver#hlds-hls-lfcd-lds-lds-01).

When I try to use both sensors I get the following error:

[ WARN] [1639070542.328478953]: W1209 18:22:22.000000 13627 sensor_bridge.cc:211] Ignored subdivision of a LaserScan message from sensor scan because previous subdivision time 637746673337557396 is not before current subdivision time 637746673337281184

This is my lua:

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "local_origin",
  tracking_frame = "vectornav",
  published_frame = "base_link", --base_link
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  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.,
  landmarks_sampling_ratio = 1.,
  publish_tracked_pose= true,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_3D.pose_extrapolator.use_imu_based=true
--aggiunti per utilizzare il terabee
--TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching=true
--TRAJECTORY_BUILDER_3D.real_time_correlative_scan_matcher.linear_search_window=0.15

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 10

POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 0 --320 --0 disabilita il loop closure
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66
--POSE_GRAPH.optimization_problem.log_solver_summary = true

I have already try to synchronize the two output topics with the ros filters, like reported in this code:

import rospy

import message_filters
from sensor_msgs.msg import LaserScan, PointCloud2

last_laserscan=LaserScan()
last_pointcloud=PointCloud2()

def talker(last_laserscan, last_pointcloud):
  # Publishers
  laserscan_pub = rospy.Publisher("laserscan_sync", LaserScan, queue_size=1) 
  velodyne_pub = rospy.Publisher("velodyne_points_sync", PointCloud2, queue_size=1) 
  last_laserscan.header.stamp=rospy.Time.now()
  last_pointcloud.header.stamp=rospy.Time.now()
  laserscan_pub.publish(last_laserscan)
  velodyne_pub.publish(last_pointcloud)

if __name__ == "__main__":
    rospy.init_node("synchronizer", anonymous=True)
    laserscan_sub=LaserScan()
    velodyne_sub=PointCloud2()
    # Subscribers
    laserscan_sub = message_filters.Subscriber('scan', LaserScan)
    velodyne_sub = message_filters.Subscriber('velodyne_points', PointCloud2)
    ts = message_filters.ApproximateTimeSynchronizer([laserscan_sub,velodyne_sub], 1,0.1)
    #ts=message_filters.TimeSynchronizer([laserscan_sub,velodyne_sub], 2)
    ts.registerCallback(talker)
    #rate.sleep()
    rospy.spin()

The velodyne output is the /velodyne_points topic and have a frequency of 10Hz, the laserscan output is /scan and have 20Hz of frequency.

Can someone give me some advice? Thanks in advance.

[eithwa]

Can you provide bag and urdf files?

[Cristian-wp]

For the moment I stop that route, now I am trying to tuning better the slam, in order to reduce the Z error when it spin.