Closed KavenYau closed 2 years ago
I thought I had this conversation with someone else years ago and we fixed it but I can't seem to find any reference to it in the issue tracker... how odd.
A couple of things to characterize the problem to make sure we understand what's going on
ros2
branch do this? It might require some API messing with to get it to work, I think between Foxy and Galactic we did some minor changes My goal is to figure out 1) if this is aliasing related to local costmap. I don't think so but worth a look. 2) if the grid to costmap conversion is off or if the grid itself is off.
I thought when I had this discussion before we found that openVDB was centering the layer over cells while costmap 2D was using the corners for its coordinates (or vise versa) and we dealt with it by adding a +0.5
cell offset to handle it.
Marking into the grid: https://github.com/SteveMacenski/spatio_temporal_voxel_layer/blob/7bf1e12aea72269cda30a483ddeefbe7a333e592/src/spatio_temporal_voxel_grid.cpp#L289-L296
costmap update population from the grid: https://github.com/SteveMacenski/spatio_temporal_voxel_layer/blob/01c0a8d962e1697ff003865d5a185682fc01de0c/src/spatio_temporal_voxel_layer.cpp#L692-L694
If you do this in the global costmap, do you see the same thing?
Yes, there is an offset too.
Can you show me a figure in this setting of 1) the data 2) the costmap and 3) the voxel grid from STVL on that data? Don't show any data not being inputted into STVL, just things representing it
purple grids: LETHAL_OBSTACLE value in the costmap white points: voxel grid published from STVL color points: scan data
openVDB was centering the layer over cells while costmap 2D was using the corners for its coordinates (or vise versa) and we dealt with it by adding a +0.5 cell offset to handle it.
Could this figure represent it?
Does the current state of ros2 branch do this? It might require some API messing with to get it to work, I think between Foxy and Galactic we did some minor changes
I found a commit(https://github.com/SteveMacenski/spatio_temporal_voxel_layer/commit/7bf1e12aea72269cda30a483ddeefbe7a333e592) to fix voxel visualization but not in foxy
got it, thanks for finding that, I'd be happy to merge that into Foxy, can you submit a PR?
I found something related to this issue.
I made a simple application for testing point convertion of openvdb.
{
x = y = 0.0;
std::cout << "===========================================" << std::endl;
std::cout << "x axis:" << std::endl;
for (int i = 0; i < end; i++) {
x = start + (step * i);
openvdb::Vec3d test_mark_grid(grid->worldToIndex(openvdb::Vec3d(x, y, 0.0)));
openvdb::Vec3d pose_world =
grid->indexToWorld(openvdb::Coord(test_mark_grid[0], test_mark_grid[1], test_mark_grid[2]));
std::cout << "points: " << x << " " << y << " " << 0.0 << " pose_world: " << pose_world[0] << " " << pose_world[1]
<< " " << pose_world[2] << std::endl;
}
}
A part of the outputs:
points: -0.02 0 0 pose_world: -0.02 0 0
points: -0.019 0 0 pose_world: 0 0 0
points: -0.018 0 0 pose_world: 0 0 0
points: -0.017 0 0 pose_world: 0 0 0
points: -0.016 0 0 pose_world: 0 0 0
points: -0.015 0 0 pose_world: 0 0 0
points: -0.014 0 0 pose_world: 0 0 0
points: -0.013 0 0 pose_world: 0 0 0
points: -0.012 0 0 pose_world: 0 0 0
points: -0.011 0 0 pose_world: 0 0 0
points: -0.01 0 0 pose_world: 0 0 0
points: -0.009 0 0 pose_world: 0 0 0
points: -0.008 0 0 pose_world: 0 0 0
points: -0.007 0 0 pose_world: 0 0 0
points: -0.006 0 0 pose_world: 0 0 0
points: -0.005 0 0 pose_world: 0 0 0
points: -0.004 0 0 pose_world: 0 0 0
points: -0.003 0 0 pose_world: 0 0 0
points: -0.002 0 0 pose_world: 0 0 0
points: -0.001 0 0 pose_world: 0 0 0
points: 0 0 0 pose_world: 0 0 0
points: 0.001 0 0 pose_world: 0 0 0
points: 0.002 0 0 pose_world: 0 0 0
points: 0.003 0 0 pose_world: 0 0 0
points: 0.004 0 0 pose_world: 0 0 0
points: 0.005 0 0 pose_world: 0 0 0
points: 0.006 0 0 pose_world: 0 0 0
points: 0.007 0 0 pose_world: 0 0 0
points: 0.008 0 0 pose_world: 0 0 0
points: 0.009 0 0 pose_world: 0 0 0
points: 0.01 0 0 pose_world: 0 0 0
points: 0.011 0 0 pose_world: 0 0 0
points: 0.012 0 0 pose_world: 0 0 0
points: 0.013 0 0 pose_world: 0 0 0
points: 0.014 0 0 pose_world: 0 0 0
points: 0.015 0 0 pose_world: 0 0 0
points: 0.016 0 0 pose_world: 0 0 0
points: 0.017 0 0 pose_world: 0 0 0
points: 0.018 0 0 pose_world: 0 0 0
points: 0.019 0 0 pose_world: 0 0 0
points: 0.02 0 0 pose_world: 0.02 0 0
It seems that zero has two intervals that are (-0.02, 0.0] and (0.0, 0.02), and the voxel_size is 0.02. It means there is always an offset if the value is positive(or negative). But I only tested on my laptop(Ubuntu 20.04 LTS) and the openvdb installed by apt. Do you know something about that?
That is odd, I have no not noticed this, no. That's probably why we see the offset that we do, we assumed that it was due to definitions of coordinate systems being different (e.g. a point center is either the point where 4 box edges meet vs the center of a given box).
Luckily, everything we do is in positive coordinate space, so there shouldn't be an issue with positive and negative, its just positive.
Hi, I am trying to use STVL, but I found a issue that there is an offset between the grid of costmap and the raw data:
Did you see this before? And Have you any ideas of that?
More informations:
ROS version: Foxy built from source: 396b25b806b22d9c256aa2000691f5dd171b4571 config of local_costmap: