Generated surface is offset by a voxel width

[ghost]

I made a test for accuracy where I am generating a small surface using cpu_tsdf and comparing it with the expected surface I have from my own software. The cpu_tsdf surface always seems to be one voxel width behind the expected surface. Increasing the resolution(decreasing the voxel size) brings it closer to the expected surface. Any clues?

The wireframe is the expected surface, whereas the solid one is from cpu_tsdf. Thanks!

[sdmiller]

Very interesting. I recall someone finding an off-by-one indexing error in older code; perhaps the "fix" for that wound up breaking the reconstructed surface. Let me take a look!

[ghost]

Indeed. I did manage to fix it recently. I do not have the diff handy, but these are the changes I had to make

In tsdf_volume_octree.cpp(Note the additional + 0.5):

pcl::PointXYZ
cpu_tsdf::TSDFVolumeOctree::getVoxelCenter (size_t x, size_t y, size_t z) const
{
  // return voxel_centers_->at (x*(yres_*zres_) + y*zres_ + z);
  float xoff = xsize_/2.0;
  float yoff = ysize_/2.0;
  float zoff = zsize_/2.0;
  return pcl::PointXYZ ((x + 0.5)*xsize_/(double)xres_ - xoff, (y + 0.5)*ysize_/(double)yres_ - yoff, (z + 0.5)*zsize_/(double)zres_ - zoff);
}

bool
cpu_tsdf::TSDFVolumeOctree::getVoxelIndex (float x, float y, float z, int &x_i, int &y_i, int &z_i) const
{
  double xoff = (double)xsize_/2.0;
  double yoff = (double)ysize_/2.0;
  double zoff = (double)zsize_/2.0;

    x_i = std::floor(((double)x + xoff)/((double)xsize_/(double)xres_));
    y_i = std::floor(((double)y + yoff)/((double)ysize_/(double)yres_));
    z_i = std::floor(((double)z + zoff)/((double)zsize_/(double)zres_));
  bool has_voxel = (x_i >= 0 && y_i >= 0 && z_i >= 0
                  && x_i < xres_ && y_i < yres_ && z_i < zres_);
  return (has_voxel);
}

In here, the pcl cloud point cloud corners have to be pushed outside by half a voxel width In marching_cubes_tsdf_octree.cpp :

  // into voxelizing it the same
  pcl::PointCloud<pcl::PointXYZ>::Ptr corner_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  for (int x_i = 0; x_i <= res_x; x_i += res_x)
  {
    for (int y_i = 0; y_i <= res_y; y_i += res_y)
    {
      for (int z_i = 0; z_i <= res_z; z_i += res_z)
      {
        pcl::PointXYZ center = tsdf_volume_->getVoxelCenter (x_i, y_i, z_i);
        // Go from center to corner
        center.x += (x_i == 0 ? -1 : 1) * 0.5 * size_x / res_x + (x_i == 0 ? 1 : -1)*(0.5*tsdf_volume_->xsize_/(double)tsdf_volume_->xres_);
        center.y += (y_i == 0 ? -1 : 1) * 0.5 * size_y / res_y + (y_i == 0 ? 1 : -1)*(0.5*tsdf_volume_->xsize_/(double)tsdf_volume_->xres_);
        center.z += (z_i == 0 ? -1 : 1) * 0.5 * size_z / res_z + (z_i == 0 ? 1 : -1)*(0.5*tsdf_volume_->xsize_/(double)tsdf_volume_->xres_);
        corner_cloud->points.push_back (center);
      }
    }
  }

Additionally, I noticed that voxels which are not leafs were making their way into the updateVoxel() function, which should also be corrected! Cheers

[sdmiller]

Ah, very interesting! Yes, I am 90% sure those "+0.5"s were removed elsewhere, due to some boundary condition. I'll hunt for that; but if you'd like to make a PR for this, I'd be much obliged :).

P.S. apologies for the slow response -- it's been a while since I looked at this codebase, and I don't always notice these github alerts.

[sdmiller]

Long time coming, but I finally brought this fix in with https://github.com/sdmiller/cpu_tsdf/pull/59