TSDF on unorganised smoothed clouds

[Jason5207]

Hi there,

Firstly great work with the TSDF code! One thing I wondered is whether there is a way to get this to work with unorganised clouds? My input data from my sensor is very noisy so I tend to range filter, downsample and apply smoothing like stastical outlier removal or MLS. The issue is then I cannot give this cloud to the TSDF. My planar surfaces become very bumpy as a result.

Any advice?

[sdmiller]

Hi @Jason5207 ,

So sorry for the delay, I somehow didn't notice this alert! If you know the intrinsics of your sensor (which you'll need for TSDFs to work anyway) you can always reproject your unorganized points back into pixel space. In my own example program, integrate.cpp, I do this by default -- see the "--organized" flag:

    // Make organized
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_organized (new pcl::PointCloud<pcl::PointXYZRGBA> (width_, height_));
    if (organized)
    {
      if (cloud->height != height_ || cloud->width != width_)
      {
        PCL_ERROR ("Error: cloud %d has size %d x %d, but TSDF is initialized for %d x %d pointclouds\n", i+1, cloud->width, cloud->height, width_, height_);
        return (1);
      }
      pcl::copyPointCloud (*cloud, *cloud_organized);
    }
    else
    {
      size_t nonnan_original = 0;
      size_t nonnan_new = 0;
      float min_x = std::numeric_limits<float>::infinity ();
      float min_y = std::numeric_limits<float>::infinity ();
      float min_z = std::numeric_limits<float>::infinity ();
      float max_x = -std::numeric_limits<float>::infinity ();
      float max_y = -std::numeric_limits<float>::infinity ();
      float max_z = -std::numeric_limits<float>::infinity ();
      for (size_t j = 0; j < cloud_organized->size (); j++)
        cloud_organized->at (j).z = std::numeric_limits<float>::quiet_NaN ();
      for (size_t j = 0; j < cloud->size (); j++)
      {
        const pcl::PointXYZRGBA &pt = cloud->at (j);
        if (verbose && !pcl_isnan (pt.z))
          nonnan_original++;
        int u, v;
        if (reprojectPoint (pt, u, v))
        {
          pcl::PointXYZRGBA &pt_old = (*cloud_organized) (u, v);
          if (pcl_isnan (pt_old.z) || (pt_old.z > pt.z))
          {
            if (verbose)
            {
              if (pcl_isnan (pt_old.z))
                nonnan_new++;
              if (pt.x < min_x) min_x = pt.x;
              if (pt.y < min_y) min_y = pt.y;
              if (pt.z < min_z) min_z = pt.z;
              if (pt.x > max_x) max_x = pt.x;
              if (pt.y > max_y) max_y = pt.y;
              if (pt.z > max_z) max_z = pt.z;
            }
            pt_old = pt;
          }
        }
      }
      if (verbose)
      {
        PCL_INFO ("Reprojection yielded %d valid points, of initial %d\n", nonnan_new, nonnan_original);
        PCL_INFO ("Cloud bounds: [%f, %f], [%f, %f], [%f, %f]\n", min_x, max_x, min_y, max_y, min_z, max_x);
      }
    }

[sdmiller]

Note that most of that code was only for verbose logs. Here's the stripped down version:

// Assume "cloud" is an unorganized point cloud. The output, cloud_organized, is its organized equivalent
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_organized (new pcl::PointCloud<pcl::PointXYZRGBA> (width_, height_));
for (size_t j = 0; j < cloud_organized->size (); j++)
   cloud_organized->at (j).z = std::numeric_limits<float>::quiet_NaN ();
for (size_t j = 0; j < cloud->size (); j++)
{
  const pcl::PointXYZRGBA &pt = cloud->at (j);
  int u, v;
  if (reprojectPoint (pt, u, v))
  {
     pcl::PointXYZRGBA &pt_old = (*cloud_organized) (u, v);
     if (pcl_isnan (pt_old.z) || (pt_old.z > pt.z))
       pt_old = pt;
   }
}

Which uses the following function (sub in your own focal lengths and principal points):

inline bool
reprojectPoint (const pcl::PointXYZRGBA &pt, int &u, int &v)
{
  u = (pt.x * focal_length_x_ / pt.z) + principal_point_x_;
  v = (pt.y * focal_length_y_ / pt.z) + principal_point_y_;
  return (!pcl_isnan (pt.z) && pt.z > 0 && u >= 0 && u < width_ && v >= 0 && v < height_);
}

[sdmiller]

(It just occurred to me that you might not be asking about how to input an unorganized cloud, but how to work with pre-smoothed data. In general, filtering/smoothing techniques tend to violate the raytracing principles that the TSDF is built on, and have the strange effect or sort of double-smoothing. In my own work I've typically limited preprocessing to discarding distant points and some small outlier removal -- I would not suggest anything that modifies the actual XYZ coordinates of your points.)

[JiayongLF]

@sdmiller hi there , it's really a nice work. But I am wandering it we don't pre-process the input point cloud can we guarantee the final mesh is good? Assume that I have a noisy point cloud, it's a unorganized one and I do some pre-process on it such as filter and smooth, so how could I turn it to an organized one? As you said I should reproject the processed unorganized point cloud to the organized one using the parameter such as focal length and principle point?

[sdmiller]

Hi @JiayongLF ,

Sorry again for the delay! I need to start turning e-mail alerts back on.

The good news is, most filter methods in PCL support organized clouds. The convention is that, rather than removing these points, you simply set them to pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); That way they keep their "place" in the pixel array, but still get removed.

[JiayongLF]

Hi @sdmiller Thank you for your reply. It's true that the filter algorithms support the organized clouds. But recently I am facing the problem in turning it to the windows. I want to turn the .h and .cpp file into lib so that I could use it. It seems that the file only contains the Cmakefile that use in linux. So could you provide the Cmakefile on windows or some ideas on this issue? Thank you very much!

[sdmiller]

As discussed in e-mail, I'm afraid I haven't really had much experience in windows. But if you do find changes to the CMakeList.txt which help compile in a Windows environment, it would be great if you submitted them as a pull request so others could benefit :)