Feature request: Combining wavemaps expressed in different reference frames

[miguelcastillon]

Hi @victorreijgwart!

Thank you for this great library, we have been testing it with the output of the lidar mounted on our quadruped robot and we are very happy with the results :)

However, there is a small functionality that we think is missing and would probably be very helpful for other projects too.

Context
Currently, our mapping pipeline has two main components:

• an odometry module that takes in lidar measurements and outputs the estimated pose for each new scan (after point cloud processing and fusion with other sensor modalities), and
• a mapping module that takes in the odometry estimation and refines it with loop closures and absolute pose measurements.

Being (very) conservative, the drift of our odometry output is negligible for travelled distances of 15-20 m. However, loop closures do improve the global map in our typical operations (with distances in the order of hundreds of meters or kilometers).

Our goal is to obtain a consistent global map that can be consumed by downstream navigation applications. We think that the way forward would be to create a set of submaps referenced to the poses in our SLAM graph and to combine them all together at the end of the mapping session using the final optimized SLAM poses.

Requested feature
In order to combine submaps referenced wrt different frames, we would need to

1. project all the submaps to world frame, and
2. combine their information.

This could be something like:

Map combineSubmaps(const MapWithPose& map1, const MapWithPose& map2)
{
  return map1.transformToGlobalFrame() + map2.transformToGlobalFrame();
}

So we would need to implement:

1. A sort of MapWithPose class that has the pose of the map in world frame and a function like transformToGlobalFrame.
2. A + operator that combines maps expressed in the same reference frame.

Please let me know what you think of this approach, especially if there are some steps you would do differently.
I'd be happy to work on it myself but I'm sure that some assistance from your side would save me a good amount of time and pain :)
So if you could provide me with some pointers to relevant classes and a brief outline of how you would do it I'd really appreciate it!

Cheers,
Miguel

[victorreijgwart]

Hi @miguelcastillon,

I'm glad to hear you're happy with the results of wavemap! The functionality you described sounds good, and I also think it would be helpful for other users. In general, the ability to transform and combine maps is a feature that would be very good to have.

To represent a submap with an associated pose, you could use wavemap's PosedObject class. It's possible to use it directly by passing the map data structure of your choice as a template parameter (e.g. PosedObject<HashedWaveletOctree>), but you could also extend it by creating a derived class. A concrete example where we do this is the PosedPointcloud class. This option would allow you to use an object-oriented syntax like the map1.transformToGlobalFrame() you suggested.

The transformToGlobalFrame functionality can probably be implemented quite concisely, but it would touch on many different aspects of the framework. It should be possible to apply the transformation very efficiently by exploiting the map's sparsity, but this is a bit trickier to implement. If it's not too urgent from your side, I could help implement it myself towards the end of this month, as this would probably save you some time. Otherwise, I could share some suggestions of how I'd approach the problem and what tools are already available.

Merging submaps by summing them, or more generally by applying any linear combination, should be very straightforward to implement. The only necessary steps would be to traverse both input data structures, apply the linear combination to their wavelet coefficients, and save the result in an output (sub)map.

Let me know if it works for you if we start working on transformToGlobalFrame toward the end of this month, or if you would already like to start earlier. Also don't hesitate if you have any questions :)

[miguelcastillon]

Thank you for the info! Yes, waiting until the end of the month is fine for me and will probably be much more efficient, so just ping me when you have more time :)

[victorreijgwart]

Great, I'll do that.

[miguelcastillon]

Hi Victor,

Just to let you know about the progress on my side:

• I have extended my version of the WavemapServer class with a service to trigger storing the current submap and creating a new one. I guess instead of creating new input_handlers_ I should find a way of changing their map pointers to the new occupancy_map_.

trigger_submap_srv_ =
    nh_private.advertiseService<std_srvs::Trigger::Request,
                                std_srvs::Trigger::Response>(
        "trigger_submap", [this](auto&, auto& response) {
          ROS_INFO("Triggering new submap");
          if (occupancy_map_) {
            occupancy_map_->threshold();
            submaps_.push_back(occupancy_map_);
            occupancy_map_ = VolumetricDataStructureFactory::create(
                data_structure_params_,
                VolumetricDataStructureType::kHashedBlocks);
            input_handlers_.clear();
            for (const auto& integrator_params : integrator_params_array_) {
              addInput(integrator_params);
            }
          } else {
            ROS_ERROR(
                "Could not trigger new submap because the current "
                "map has not yet been allocated.");
            response.success = false;
            return true;
          }
          ROS_INFO("New submap was successfully triggered.");
          response.success = true;

          return true;
        });

• When the function to store the map is called, I first combine the submaps into a global map.

HashedWaveletOctree::Ptr combineSubmaps(
    const std::vector<VolumetricDataStructureBase::Ptr>& submaps) {
  HashedWaveletOctreeConfig config;  // fill with desired params
  HashedWaveletOctree::Ptr map(new HashedWaveletOctree(config));
  for (const auto& submap : submaps) {
    submap->forEachLeaf(
        [&map](const OctreeIndex& node_index, FloatingPoint value) {
          const auto index = convert::nodeIndexToMinCornerIndex(node_index);  // is this the correct function?
          map->addToCellValue(index, value);
        });
  }
  map->threshold();  // needed?
  return map;
}

Both functions are just missing the projection of the submaps to the global reference frame, but my SLAM is ready to pass an initial guess of the submap pose when it's created and update it when the graph is optimized. I have also created a class class PosedHashedWaveletOctree : public PosedObject<HashedWaveletOctree> that could implement functions such as e.g. getSubmapLocal() and getSubmapGlobal().

How does this all look to you so far?

Also: I've been looking into expanding the map data structure to include traversability information. I'll open a new feature request now to have each discussion in a different thread.

Cheers,
Miguel

[victorreijgwart]

Hi Miguel,

This looks great!

For the input handlers, I think your current solution is actually the safest and cleanest option. I'd expect the input handlers to be relatively cheap to rebuild (we can double-check this with Tracy profiler), and doing it this way eliminates potential bookkeeping errors. For example, the HashedWaveletIntegrator stores a HashedWaveletOctree::Ptr while its base class already stores a MapBase::Ptr -- updating one while forgetting the other would cause some hard-to-find bugs.

For the frame conversions, the code you suggested above looks like a great starting point and might already be good enough to get a full prototype system to run :) You could perform the frame transformation using something like this:

  HashedWaveletOctree::Ptr combineSubmaps(
      const std::vector<VolumetricDataStructureBase::Ptr>& submaps) {
    HashedWaveletOctreeConfig config;  // fill with desired params
    HashedWaveletOctree::Ptr map(new HashedWaveletOctree(config));
    for (const auto& submap : submaps) {
      const Transformation3D T_W_S = submap.getPose();
      const FloatingPoint cell_width = submap.getMinCellWidth();
      const FloatingPoint cell_width_inv = 1.f / cell_width;
      submap->forEachLeaf(
          [&map, &T_W_S, cell_width, cell_width_inv](
              const OctreeIndex& node_index, FloatingPoint value) {
            const auto S_min_corner =
                convert::nodeIndexToMinCornerIndex(node_index);
            const auto S_max_corner =
                convert::nodeIndexToMaxCornerIndex(node_index);
            for (const auto& S_index : Grid<3>(S_min_corner, S_max_corner)) {
              const Point3D S_cell =
                  convert::indexToCenterPoint(S_index, cell_width);
              const Point3D W_cell = T_W_S * S_cell;
              const Index3D W_index =
                  convert::pointToNearestIndex(W_cell, cell_width_inv);
              map->addToCellValue(index, value);
            }
          });
    }
    map->prune();
    return map;
  }

It'd be possible to get it to run faster by exploiting the sparsity of the free space, and aliasing artifacts could be reduced by linearly interpolating into the new grid (instead of using a single nearest neighbor as above). It's usually easiest to implement this interpolation by iterating over a grid in the destination map frame instead of in the source frame. You could then wrap the interpolate::trilinear helper around the QueryAccelerator to efficiently perform the interpolation.

Best wishes,
Victor

[miguelcastillon]

Hi Victor,

For the input handlers, I think your current solution is actually the safest and cleanest option.

Great!

You could perform the frame transformation using something like this:

Perfect, this is indeed already enough to have a first prototype running. The results are good but I do have observed some inconsistencies in the combined global map, which probably come from my side. I'll debug it further.

It'd be possible to get it to run faster by exploiting the sparsity of the free space, and aliasing artifacts could be reduced by linearly interpolating into the new grid

Great, I'll look into that.

Thank you,
Miguel