Generated surface is offset by a voxel width

[sdmiller]

Generated surface is offset by a voxel width

[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!

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.