Expose Multiscan Matching Coarse Fine Resolution Multiplier

[louietouie]

Basic Info

This PR has no corresponding tickets
Tested only on Jazzy, Ubuntu 24.04, small Gazebo simulations

Summary

This exposes the multiplier between the fine search and coarse search for the Multi-resolution Correlative Scan Matcher. This multiplier is only for the translational search (x and y), not the angle. So it scales up the fine resolutions loop_search_space_resolution and correlation_search_space_resolution with the new parameters loop_search_space_coarse_resolution_multiplier and correlation_search_space_coarse_resolution_multiplier. It keeps the default scale at 2.

Documentation updates

I updated the README.md file. I commented out new BOOST_SERIALIZATION_NVP parameters, as I noticed the PR for m_pMinPassThrough did.

Future work

• It needs more rigorous testing
• I think there is a decent amount of repeat code (see calculations of m_xPoses and xPoses) that could be consolidated
• fine_search_angle_offset should be renamed fine_search_angle_resolution
• in this paper, the process described in section D3 searches over multiple of the coarse steps (until no coarse score is larger than some calculated fine score). I think this would guarantee that the best fine resolution would be found, so large multipliers like 10 would have no reduction in scan matching quality and still increase performance.

[SteveMacenski]

@louietouie can you summarize your testing on various parameters, and provide the graph figure, and the output maps for each of the settings? This should be information that other people are aware of and is searchable from our email chain.

With that - what's a good setting we should change the configuration files to use by your testing?

[SteveMacenski]

@louietouie what's a good setting we should change the configuration files to use by your testing?

[louietouie]

As you increase the multiplier, the runtimes get faster (up to a certain point), but theoretically, the scan matching should get less accurate. Before this PR, the multiplier was fixed at 2. From my second image below, even changing the multiplier to 4 had a substantial performance increase.

To minimize the number of scans required, I believe the best multiplier (for correlation_search_space_coarse_resolution_multiplier) would be round(sqrt(correlation_search_space_dimension/correlation_search_space_resolution)). I think this would evenly divide the number of scans between coarse and fine.

The graph below combines scans used for both scan matching odometry (at .5 dimension and .01 resolution) and loop closure (at 4.0 dimension and .01 resolution).

Here are some tests I did at multipliers 2, 4, and 10 using the a ros2 bag file so the path the robot takes was somewhat consistent. This is partially where I struggled; the tests were not repeatable (as is seen in the differences between the two equivalent-parameter tests at each multiplier level) and therefore it will be good to get feedback from others on the effects to accuracy.

If the performance increase is substantial, but the accuracy loss is noticeable, it may be worth implementing the algorithm in D3 of this paper which shouldn't be too hard. I believe this would completely stop any accuracy losses, with close to the same performance increase this PR should provide (if any).

[SteveMacenski]

I think 4 then seems reasonable. That seems to get the bulk of the computational benefits without being such a large change that I would expect massive differences. Can you do some testing on a few maps/worlds and let me know if you see any major differences?

If the performance increase is substantial, but the accuracy loss is noticeable, it may be worth implementing the algorithm in D3 of this paper which shouldn't be too hard. I believe this would completely stop any accuracy losses, with close to the same performance increase this PR should provide (if any).

I think that's worth an experiment!

[louietouie]

Definitely I can do that. I'm crunching to learn rust and finish a robot this week, but I will try to run some tests on other Gazebo maps early next week. Is there any other metrics to measure differences that you prefer (other than timing laserCallback)?

[SteveMacenski]

I think you showed me sufficiently that this is an improvement in run-time performance. At this point I'm more interested in making sure map quality is preserved in exchange for it.

[SteveMacenski]

@louietouie Hi! I wanted to follow up, were you able to run some experiments so we can merge this in?

[louietouie]

@SteveMacenski Sorry for the big delay. It's been a busy month but I have not forgotten. I will try to work on it this and next weekend. My plan is just to make a larger Gazebo map from scratch. If you have other ideas or access to any laserscan bag files of sims or real-world examples, I'd love to test those too.

[SteveMacenski]

I really appreciate the update, thanks! I found a few datasets googling but I didn't find any in particular I can recommend

[louietouie]

Apologies for the major delay on this. Not sure if this PR has become outdated... no pressure if it has. If not, here's a new large map test.

RUN AT BASELINE OF MULTIPLIER 2

RUN AT MULTIPLIER 4

I first tried to make the runs exactly the same by recording the cmd_vel topic and replaying it, but found it led to different robot paths. So the next best option I thought of was just to time it and take the same path both times... both warehouse loops took ~2:30 - 3 minutes.

[SteveMacenski]

What about just using a rosbag dataset? That would give us the best 1:1 (just make sure to replay in real-time, don't try to speed it up). You'd need your odom, TF (with map->odom stripped out), TF static, and laserscan topics. I think that's it (?).

This isn't too old, I would still like this in the project! Please :-)

[louietouie]

Ah, good idea. I had been trying to make a rosbag dataset that would replay the robot trajectory, which would in turn result in the same sensor data/transformations. Makes much more sense to just record the sensor data directly. I will post results soon 👍

[louietouie]

The recording was made without the slam_toolbox node running...

1. ros2 bag record tf tf_static sensor/imu sensor/scan sensor/odom -o feb4_run_full1
2. ros2 bag play feb4_run_full1 --clock -r 1

Multiplier 2

Multiplier 4

[louietouie]

Let me know if there is any other tests you would like to see... i can do them pretty quickly now.

[SteveMacenski]

what's the total integrated callback time for one versus the other in running over the dataset? The 0.003 vs 0.004 is pretty coarse information. If we see that 25% increase, then absolutely this seems awesome and I see virtually no degradation in the map quality.

[louietouie]

Sure. I'm struggling to find something higher-level than laserCallback to measure. My thought to measure the laserCallback time was because, from my understanding, this is the full callback that runs every time a LaserScan message is recieved in the SlamToolbox node by the scan_filter_ subscriber.

  scan_filter_->registerCallback(
    std::bind(&SlamToolbox::laserCallback, this, std::placeholders::_1)

The way I measured this was...

void AsynchronousSlamToolbox::laserCallback(
  sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
/*****************************************************************************/
{
  const auto start = std::chrono::high_resolution_clock::now();
  // store scan header
  scan_header = scan->header;
  // no odom info
  Pose2 pose;
  if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) {
    RCLCPP_WARN(get_logger(), "Failed to compute odom pose");
    return;
  }

  // ensure the laser can be used
  LaserRangeFinder * laser = getLaser(scan);

  if (!laser) {
    RCLCPP_WARN(get_logger(), "Failed to create laser device for"
      " %s; discarding scan", scan->header.frame_id.c_str());
    return;
  }

  // if not paused, process scan
  if (shouldProcessScan(scan, pose)) {
    addScan(laser, scan, pose);
  }

  const auto stop = std::chrono::high_resolution_clock::now();
  const std::chrono::duration<double> diff = stop - start;
  std::ofstream outfile("timing2_3", std::ofstream::app);
  outfile << diff.count() << std::endl;
  outfile.close();
}

[SteveMacenski]

Sure - then just add them up for the file for the full runtime of 2 vs 4

PS we’ll also want this change in Rolling so it will be available for all future distributions

[louietouie]

Total time for Multiplier 2: 1.357905 s
Total time for Multiplier 4: 1.031652 s

[louietouie]

Makes sense. I can work on adding these same edits to the ros2 branch

[SteveMacenski]

Oh wow, nice! Do you have a larger data set to test against per-chance? That's a pretty short processing time to get some metrics from.

But that's a pretty great improvement indeed if that holds out!