Collision checking with wavemap

[sverrevr]

Even though all the information is available here and here, I still used some time to figure out exactly how to do collision checking with wavemap in ros. So to help future users, here is my implementation. Its probably not an optimal implementation, but maybe it can be a starting point. If you have some improvements please post them below, or modify the post itself, would be nice to work out a good minimal example people can use.

#pragma once
#include <ros/ros.h>
#include <wavemap/core/map/hashed_wavelet_octree.h>
#include <wavemap/core/utils/iterate/grid_iterator.h>
#include <wavemap/core/utils/query/map_interpolator.h>
#include <wavemap/core/utils/query/probability_conversions.h>
#include <wavemap/core/utils/query/query_accelerator.h>
#include <wavemap_ros_conversions/map_msg_conversions.h>

class WaveMapWrapper
{
    ros::NodeHandle nh_;
    wavemap::HashedWaveletOctree::Ptr wavemap_;

    ros::Subscriber wavemap_sub_ = nh_.subscribe<wavemap_msgs::Map>(
        "/wavemap/map", 1, [&](const auto& msg) {
            auto pure_map_pointer =
                std::dynamic_pointer_cast<wavemap::MapBase>(wavemap_);
            wavemap::convert::rosMsgToMap(*msg, pure_map_pointer);
            wavemap_ = std::dynamic_pointer_cast<wavemap::HashedWaveletOctree>(
                pure_map_pointer);
        });
  public:
    auto get_all_occupied_cells(const Eigen::Vector3d& reference_position,
                                double radius) const
    {
        const Eigen::Vector3d radius_vec{radius, radius, radius};
        const Eigen::Vector3d lower_bound{reference_position - radius_vec};
        const Eigen::Vector3d upper_bound{reference_position + radius_vec};

        const wavemap::Point3D lower_bound_wavemap = lower_bound.cast<float>();
        const wavemap::Point3D upper_bound_wavemap = upper_bound.cast<float>();

        const wavemap::FloatingPoint min_cell_width =
            wavemap_->getMinCellWidth();
        const wavemap::Index3D min_corner_index =
            wavemap::convert::pointToFloorIndex(lower_bound_wavemap,
                                                1.f / min_cell_width);
        const wavemap::Index3D max_corner_index =
            wavemap::convert::pointToCeilIndex(upper_bound_wavemap,
                                               1.f / min_cell_width);

        wavemap::QueryAccelerator query_accelerator(*wavemap_);

        std::vector<Eigen::Vector3d> occupied_cells;
        for (const auto& query_index :
             wavemap::Grid<3>(min_corner_index, max_corner_index))
        {
            const wavemap::FloatingPoint occupancy_log_odds =
                query_accelerator.getCellValue(query_index);
            const wavemap::FloatingPoint occupancy_probability =
                wavemap::convert::logOddsToProbability(occupancy_log_odds);
            if (occupancy_probability > 0.7)
            {
                auto cell_location = wavemap::convert::indexToCenterPoint(
                    query_index, min_cell_width);

                Eigen::Vector3d cell_location_eigen =
                    cell_location.cast<double>();
                if ((cell_location_eigen - reference_position).norm() < radius)
                {
                    occupied_cells.push_back(cell_location_eigen);
                }
            }
        }
        return occupied_cells;
    }
};

[victorreijgwart]

Thanks for sharing @sverrevr! This is a great starting point for a generic function that gets all occupied cells around a point. Adding more examples on how to use wavemap for path planning is on our roadmap, but together with quite a few other features so it'll probably take some time until we get to it. Would you be interested in collaborating on this? :)

[victorreijgwart]

Sounds good. What sort of planner are you using? Is it search-based, sampling-based, or something else (e.g. some reactive policy)? For the example code you shared, do you use it to collision-check individual poses or path segments? We had a few internal projects on path planning with wavemap - I might be able to share some code/ideas with you if they're relevant to your use case.

[sverrevr]

As a first step we are working on sampling based collision avoidance, and then we plan to expand to path planning (online re-planning).
We are collision checking individual poses, but I guess we could probably save some computations by considering path segments instead.

Would be interesting to hear your ideas :)

I just want to warn you that we are not fully committed to wavemap yet. Our CPU is already heavily loaded, so we might have to switch over to nvblox to offload the work over to the GPU, if we can get nvblox to run on our hardware. So far I find the interface of wavemap much more pleasant than nvblox and voxblox, so thanks for creating a really nice interface :)

[victorreijgwart]

Yes there are a lot of optimizations you can apply when checking path segments directly, instead of breaking them into individual poses.
Which hardware are you using? Glad to hear you like wavemap's interface :)

[victorreijgwart]

In case you're still comparing frameworks, I think some meaningful advantages of wavemap for your setup are that it achieves higher recall on thin objects than TSDF-based methods, making it safer for obstacle avoidance.
The map's hierarchical structure can also be used to massively speed up downstream tasks (e.g. collision checking, ray tracing, search-based planning, frontier extraction for exploration, and change detection). Compared to GPU acceleration, which at best yields a speedup linear in the number of threads, using hierarchical algorithms reduces many problems from O(N) to O(logN).
Of course, GPU acceleration is still really nice to have, so we're actively working on adding this to wavemap, and we expect it to be available toward the end of this year :)

[sverrevr]

Thanks for all the answers victor, we had to leave on some field work right before you answered so we had to lock in the settings we then had. Now that we are back I can start to test out the things you proposed :)
We did manage to find some combination of settings that kinda worked right before leaving, but the CPU load is still a bit too high.
Another advantage of the GPU is that its currently just sitting there doing nothing, so even with no speed up it would help us to just move load over to it from the CPU which is already heavily loaded.