Redesign Environmental Representation #1278
Comments
|
Costmap representations also don't have to be same in both local and global. The local can be a gradient map but the global can be a much more coarse traversibiltiy estimate map refs |
|
Snippet of my thoughts on this from another conversation: So because sensors are noisy, and especially sensors like the realsense cameras (ugh) we have to play some games. There's some usual filtering pipeline to throw out exaggerated outliers, but its hard to robustly and efficiently throw out outliers without running a probabilistic filter (like PCL's statistical outlier filter) which is really expensive and honestly just not practical for real-time applications on mobile robot computers. So we have to be using some data points in our environmental representation that are going to probably be trash. Raycasting is one way to get those out, if you have 1 point very close, the density of the output will let multiple pixels of the depth image cast through the cell and maybe clear it so the user never notices, if nothing else with speckling noise, the next update cycle will clear it.Anyhow, because of noise, I've found that "a" way to deal with it (note: not saying its the best way, really open for suggestions or other ideas) is to raise the minimum Z value of measurements we take in. We see alot of ground, and if ground is noisy, then we can't say that we take measurements exactly at Z=0 meters because then something that's 0.005 m from the ground will appear as an obstacle right in front of our robot. So we raise that up to like 10cm so that we don't deal with ground noise, but then you lose the ability to specifically identify smaller objects. Another way is to just detect the ground plane with ransac and then remove it, which is more or less what the thing below is doing, but only for the single camera feed that is used for ground-detections rather than running over all of them. A third way I've thought about is the mark threshold parameter, but I think that column-based "noise" removal is really not a good replacement for filtering and it is pretty hacky. Functionally there are lots of things that are flat and thin that then this would not mark in the costmap (tables, shelves, things you only see from the top, etc) I've dealt with that then by implementing a single, probabilistic-derived filter to take in a single depth stream and compute normals to find smaller objects, as I've had usually 1 camera pointed down-ish and the others pointed up-ish. Running a filter like that on 1 stream at a low rate is practical for a mobile robotics application. That however, lacks generality and while it worked, it certainly wasn't perfect. so taking in gradient info rather than voxels, my hope is with a bunch of measurements, even noisey, when we're estimating the gradient given some set of measurements over a set of poses, that we'll have a much closer look at what it is in reality and be able to detect more fine changes in gradient to find smaller obstacles. For 2D robots, we just set a very fine threshold for navigability on the gradient of the ground, and for outdoor robots, we set a sharper one. I think this would also not involve raycasting (maybe) to clear or maintain space, so just like my STVL work, we then have a bunch of CPU we can use to play games with statistics on the measurements we have for obstacles / gradients. |
|
For redesign - we need to make an interface for the costmap_2d and our new thing to use so the models can be swapped out. This should include:
|
|
Re-evalutate openvdb given it has many similar elements as grid_Maps with iterators but also GPU optimization via the nvidia optimized version. At least in considering a raytracing 3D costmap method but even then an elevation mapping representation, though won't make use of the sparsity well. Other random notes: I think a 3D cost grid is an intuitive way to go from a 2D cost grid thing like costmap_2d, but elevation representations instead serve better in my opinion. You can still use raycasting if you want, or you can use more lightweight probablistic methods. Plus it scales N2 instead of N3. Plus you dont have to "guess" what the normals are from a voxel grid, you have them pretty much directly with a light convolution I want the new ROS environment to work with 2D/3D cost grids as well as elevation mapping (pick you own representation or mix and match based on sensor modalities). and potentially have MoveIt and Autoware use it too, the one environmental model to rule them all, but would have to add some self-filters and such |
|
http://ais.informatik.uni-freiburg.de/teaching/ss13/robotics/slides/17-3dmapping.pdf https://www.digitalfish.com/portfolio-item/nasa-terrain-mapping/ https://nicolovaligi.com/occupancy-mapping-benchmarks.html --> more proof STVL's introduction of openVDB is good https://developer.nvidia.com/gvdb-samples --> GPU version of openVDB https://www.digitalfish.com/portfolio-item/nasa-terrain-mapping/ Really time to start designing a solution now that grid_maps is almost done. Main topics:
|
|
Interesting thought for integrating both height maps and 3D costmaps; Find if pts in a column have 2 modalities. If so lower is ground upper is voxel grid. The minimum difference in modality a parameter depending on environment type (flat ground can be lower, non-flat higher). That would differentiate:
Then voxel grid is collision checked against robot model & height map is against gradients able to traverse (traversibility estimation) Best of both worlds! No 2d costmap info but 2d info can be represented by 3D voxel grid (openvdb/gvoxels) |
SteveMacenski
changed the title
|
Some more notes on this topic: integrated costmap (now called "risk map") and height map. The separation of ideas is the height map processes raw data (3D lidar, 2D lidar, depth from RGBD) for straight representation of the environment. Then the risk map is all of the association costs like inflation, detections that are treated uniquely with rules, etc. The risk map is then a "overlay" on the height map which restricts the height map's viable search locations based on an overlay of "rules" or annotations. This can include things like:
So no longer think of them as "cost" maps, but now as "risk" maps and the costs for actual measurements from the environment are in the height model. This helps integrate both 2D annotations / data with the height map in a coherent way. I want to create something somewhat novel similar to height maps, but more focused on the gradients between the heights than the heights themselves. the gradient is the pertinent information for whether it's passable or not Raw notes: Using semantic segmentation and detection to identify objects that could be risky and add sp. rules to them. Not just obstacles, but what kind of obstacles & treating that vision more seriously & use it. Examples:
I suppose this could be tied into or thought of as motion or "use" models You're always going to buffer in depth data in case there are missed detections / segmentations / class identification -- height map. But, we could largely reduce it to just edge cases and most normal cases are handled based on detections / segmentation / dynamic tracking. Do we need to correlate the height map obstacles and the risk map detections in the representation to prevent error-based aliasing? The “costmap” really turns into a "risk map" of detections / segmentations overlayed on a height map to better represent smaller things and traversibility in more types of spaces. Risk based on this info is an important perception overlay to the general “is this even possible” traversibility height map. It provides the "secret sauce" for where to go when, while the height map just tells us where is even at all possible. Think of a risk map based on models, detections, tracks, and semantic information about those things. That info is overlayed and used as a cost function on the traversibility map underlay, which is purely about what’s possible with no other knowledge about these rules (based on depth and probabilistic models). Risk overlay is all derived quantities like:
And naturally fits in the idea of having a merged height & cost maps. Why height map?
Why risk map?
http://ais.informatik.uni-freiburg.de/teaching/ss19/robotics/slides/17-3dmapping.pdf |
|
See #2229 for some discussion on this topic further. I believe height maps are the way to go with a layer built around 2D still in order to represent semantic information like keepouts. For the total outdoor environment case, this would only be used for the local trajectory planning, and for the planning we use a route planner that doesn't require an environmental model. |
|
Notes from another conversation My general plan was this:
In terms of tech, I was thinking about using the grid_maps package from ETH-Zurich, but they haven't really been maintaining it for ROS2. I've taken the liberty of getting it ported over to ROS2 and doing a 'nights and weekends' maintenance of it to keep it up to date roughly speaking so its ready if/when Nav2 wants to use it. I'm obviously familiar with the codebase, but know there's some legwork to figure out all of what it provides and what we need to put on top of it for a navigation application. They also have elevation_mapping and some other work based on that, I was going to take a look at those but not use them, but take some lessons learned from them in creating a new one It is probably wise at this point to also consider GPU / hardware accelerated options for those that have them available, since this kind of work is leaning in that direction |
|
Pulling #1517 into here: pluginize environmental models so we can use this new model as an option to a costmap representation |
|
@SteveMacenski I am just curious, in your view, what's the latest on this topic? Have your above thoughts and notes changed in anyway since last year? |
|
Not much has changed here. It was on my plans for next year but you know how that looks at this point... |
|
+1 to add height map & 3D map version of planner/control servers to support the multiple environmental types of requests |
|
Hi. I'm very interested in this topic especially since I'm trying to localize my robot in both outdoor and indoor environment. What is your view on using my own 3d localization method using a 3d lidar pointcloud (such as scan matching) to replace with the current AMCL? Is it possible to achieve that? |
|
Consider: have getCost APIs be rather than point-costs be cost-to-go. these could be still point-costs, but could have different costs based on the transition. So have last pose and proposed pose as the API fields. That would help handle situations like traveling out of keepouts (once in them) differently or other more advanced traversal functions based on gradient directions in height maps potentially Also worth potentially having an (1) environmental grid model, (2) semantics model including things like what we have as costmap filters now, and (3) vectorized AI models that can be treated with that information separately that are combined on value-request rather than squashing each iteration. That way, grid/height can say X, AI detection model can say Y, and semantics can override for behaviors in context. Also, not quanitizing Ai information into a grid and can take into account transition functions with timestamps potentially for dynamic models |
|
Some really high level jotted thoughts for @DLu https://docs.google.com/document/d/1obaQQ_lNVa0vRhjJK1yKaVOIT8VdjAGlR5-61_wsAOI/edit?usp=sharing. I can't seem to find my design documents from 2021 when I was thinking about this more seriously unfortunately :/ I had some reasonably thought out solutions to a few of the problems we'll have to redo |
|
Following the discussion at #4186 I will begin extending Costmap2D to Costmap3D as a starting point this week. Costmap2D offers comprehensive APIs and layering concepts, ensuring compatibility with the rest of Navigation2. Indeed, Costmap2D is a voxel map. Voxels should continue to play an important role in the future due to their intuitiveness. Voxel maps can be converted into more advanced formats, such as OpenVDB, deep learning models, or 3D Gaussian, to enhance efficiency. However, the APIs and layering concepts should remain consistent. |
Today we use a 2D costmap of independent cells. While this is a good and cheap way of saving some relevant information, it will bound us for the foreseeable future to 2.5D land.
If we want to support navigating on irregular surfaces (e.g. outdoor unstructured / natural), we need to make some steps towards that. A gradient map can still provide obstacle information similar but less regularly than a direct costmap. And to be fair, the costmaps are binned measurements so we're already a bit in that direction.
I propose a 2D gradient map being the default environmental model type. This can still be used in the same way as today's costmap to check for collisions based on gradients / absolute height in 2.5D space, but will also allow modelling for people to navigate permissibly high spaces in outdoor environments.
Gradient over 3D to allow for ramps or hills, representing downwards stairs or drops. I'm not certain for most cases what 3D collision checking really buys us if we’re only monitoring from ground to robot height. Gradient doesn't help if docking with objects under or alongside, but nor does 2d.
remove thinking if free or marked, and think now: can I traverse this
for 2D: invert that a bit, find if flat surface and then un-mark but then mark edges to not go off
The text was updated successfully, but these errors were encountered: