personalrobotics / OpenChisel

An open-source version of the Chisel chunked TSDF library.
463 stars 128 forks source link

Wrong results when computing Chunk- and Voxel-IDs #8

Closed schnaubelt closed 8 years ago

schnaubelt commented 8 years ago

Hi, the usage of meter floating points for distances and resolutions cause numerical issues. For example, we have a 1D map with a voxel resolution of 0.03m and a chunk size of 16. The position -0.48m would be assigned to ChunkID -1. But with the issues, the chunkmanager gives me chunk ID -2. This is because the computed float Chunk ID is slightly smaller then -1.0 and thus std::floor computes -2.

When you compute now the corresponding voxel coordinates with the wrong ChunkID, these are wrong as well.

To fix this, one could this imperformant "hack":

        inline ChunkID GetIDAt(const Vec3& pos) const
        {
            float voxelResolution = std::round(this->voxelResolutionMeters*1000);

            float x = std::round(pos(0)*1000) / (voxelResolution * chunkSize(0));
            float y = std::round(pos(1)*1000) / (voxelResolution * chunkSize(1));
            float z = std::round(pos(2)*1000) / (voxelResolution * chunkSize(2));

            return ChunkID(static_cast<int>(std::floor(x)),
                           static_cast<int>(std::floor(y)),
                           static_cast<int>(std::floor(z)));
        }

or use internally millimieters to improve the numerical stability.

These pictures (integrating chunks into a chunk map) visualize the problem, that the voxels nearby chunk borders doesn't get visited.

mesh_without_bugfix

Mesh without bugfix.

mesh_with_bugfix

After fixing the computation of the IDs, the mesh doesn't have the holes anymore.

mklingen commented 8 years ago

These pictures (integrating chunks into a chunk map) visualize the problem, that the voxels nearby chunk borders doesn't get visited.

What? I don't understand how this mesh was created. Were you integrating a depth image? Was the depth camera moving?

schnaubelt commented 8 years ago

I basically get all the chunks (e.g. resolution 0.03m, chunk size 16^3) from a running chisel node. In this case it was a depth image from a fixed RBG-D camera.

This is the recorded scene:

base_scene

Then I insert all chunks into another chisel map (same resolution, different chunk size). In order to do this, I compute for every voxel the new chunk- and voxel-id and update the corresponding voxel in the target chisel map.

Because every voxel still has the same position (resolution is unchanged), the resulting image should be complete again.

But be cause of the problem described in the first post, some IDs are invalid and therefore don't get overwritten.

mklingen commented 8 years ago

Then I insert all chunks into another chisel map (same resolution, different chunk size). In order to do this, I compute for every voxel the new chunk- and voxel-id and update the corresponding voxel in the target chisel map.

When doing this kind of thing, its necessary to offset everything by half a voxel length. Otherwise, you get the rounding issues. A voxel begins at its minimum point and takes up all of the space to its maximum point. When you get the "coordinate" of a voxel, you are getting its minimum point, which is on the boundary of the voxel. By offsetting your queries by half a voxel length, you get the center of the voxel, and your rounding issues will go away.

Normally, when depth images/point clouds get projected into the voxel grid, the likelihood that they end up exactly on a boundary will be very low. But in your case, you are scanning through a voxel grid and re-inserting the values into a new voxel grid. When this happens, you're bound to get aliasing. Depending on the relative sizes of the chunks you're re-inserting into, you may need to do something like supersampling or interpolation. This is actually identical to the problems you might have resizing 2D images.

For this reason I don't recommend changing the units to millimeters or anything, because in the vast majority of use cases it isn't necessary.

schnaubelt commented 8 years ago

Thank you very much for your explanation! I tested your suggestion and it works well.

Yes, I can already interpolate Chunks to handle such cases.