MLS Up Sampling only work if using polynomial fit.

[Levi-Armstrong]

Your Environment

• Operating System and version: Ubuntu 14.04
• PCL Version: master branch

Expected Behavior

When polynomial fit is not used it would use the mls plane as the surface to project onto.

Current Behavior

It currently returns the original MLS point and normal, so you get several points at the same location.

Possible Solution

These two lines of code should replace this line of code. I have tested it and everything works as expected.

Code to Reproduce

#include <pcl/point_cloud.h>
#include <pcl/surface/mls.h>
#include <pcl/io/pcd_io.h>

int main(int argc, char *argv[])
{
  pcl::PointCloud<pcl::PointXYZ> cloud;
//  std::string id = "baseline";
  std::string id = "simple";
  int gridSize = 50;

  for(unsigned int x = 0; x < gridSize; x++)
  {
    for(unsigned int y = 0; y < gridSize; y++)
    {
      double d = 0.001 * ( (double)rand() / (double)RAND_MAX );
      pcl::PointXYZ pt(x / 10.0  , y / 10.0 , 0.5 * cos(double(x)/10.0) - 0.5 * sin(double(y)/10.0) + d);
      cloud.push_back(pt);
    }
  }
  cloud.is_dense = false;

  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>(cloud));

  pcl::search::KdTree<pcl::PointXYZ>::Ptr input_cloud_tree(new pcl::search::KdTree<pcl::PointXYZ>);
  input_cloud_tree->setSortedResults(false);
  input_cloud_tree->setInputCloud(input_cloud);

  pcl::PointCloud<pcl::PointNormal>::Ptr mls_cloud(new pcl::PointCloud<pcl::PointNormal>());
  pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::Ptr mls(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(true);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_poly_no_upsampling_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  mls_cloud.reset(new pcl::PointCloud<pcl::PointNormal>());
  mls.reset(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(true);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::SAMPLE_LOCAL_PLANE);
  mls->setUpsamplingRadius(0.1);
  mls->setUpsamplingStepSize(0.05);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_poly_sample_local_plane_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  mls_cloud.reset(new pcl::PointCloud<pcl::PointNormal>());
  mls.reset(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(true);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::RANDOM_UNIFORM_DENSITY);
  mls->setPointDensity(500);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_poly_random_uniform_density_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  mls_cloud.reset(new pcl::PointCloud<pcl::PointNormal>());
  mls.reset(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(true);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::VOXEL_GRID_DILATION);
  mls->setDilationVoxelSize(0.2);
  mls->setDilationIterations(10);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_poly_voxel_grid_dilation_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  mls_cloud.reset(new pcl::PointCloud<pcl::PointNormal>());
  mls.reset(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(true);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::DISTINCT_CLOUD);
  mls->setDistinctCloud(input_cloud);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_poly_distinct_cloud_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  mls_cloud.reset(new pcl::PointCloud<pcl::PointNormal>());
  mls.reset(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(false);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_no_poly_no_upsampling_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  mls_cloud.reset(new pcl::PointCloud<pcl::PointNormal>());
  mls.reset(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(false);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::SAMPLE_LOCAL_PLANE);
  mls->setUpsamplingRadius(0.1);
  mls->setUpsamplingStepSize(0.05);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_no_poly_sample_local_plane_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  mls_cloud.reset(new pcl::PointCloud<pcl::PointNormal>());
  mls.reset(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(false);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::RANDOM_UNIFORM_DENSITY);
  mls->setPointDensity(500);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_no_poly_random_uniform_density_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  mls_cloud.reset(new pcl::PointCloud<pcl::PointNormal>());
  mls.reset(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(false);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::VOXEL_GRID_DILATION);
  mls->setDilationVoxelSize(0.2);
  mls->setDilationIterations(10);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_no_poly_voxel_grid_dilation_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  mls_cloud.reset(new pcl::PointCloud<pcl::PointNormal>());
  mls.reset(new pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>());
  mls->setComputeNormals(true);
  mls->setInputCloud(input_cloud);
  mls->setPolynomialFit(false);
  mls->setPolynomialOrder(2);
  mls->setSearchMethod(input_cloud_tree);
  mls->setSearchRadius(0.5);

  mls->setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::DISTINCT_CLOUD);
  mls->setDistinctCloud(input_cloud);

  mls->process(*mls_cloud);
  if (!mls_cloud->empty())
  {
    std::string file_path = "/tmp/mls_no_poly_distinct_cloud_" + id + ".pcd";
    pcl::io::savePCDFile(file_path, *mls_cloud);
    PCL_INFO("Wrote file: %s\n", file_path.c_str());
  }

  return 0;
}

[taketwo]

The lines in question were written by @aichim, perhaps he can comment.

[Levi-Armstrong]

@SergioRAgostinho did you get a change to take a look?

[taketwo]

We haven't heard back from the author, so I'd say you @Levi-Armstrong is now our top expert on the topic. Are you still sure about your proposed solution?

[Levi-Armstrong]

I am. I an not sure why they were commented out but I do believe that if polynomial is not used it should project to the mls plane. I will submit a PR for review.

[SergioRAgostinho]

Very late to this party. No @Levi-Armstrong I didn't manage to organize myself to have a look at this. As @taketwo mentioned, you're kind of our expert with respect to the surface module, so I'll trust your solution.