Function maskForegroundPoints

tools/train_linemod_template.cpp:102–152

std::vector<bool>
maskForegroundPoints (const PointCloudXYZRGBA::ConstPtr & input,
                      float min_depth, float max_depth, float max_height)
{
  std::vector<bool> foreground_mask (input->size (), false);

  // Mask off points outside the specified near and far depth thresholds
  pcl::IndicesPtr indices (new pcl::Indices);
  for (std::size_t i = 0; i < input->size (); ++i)
  {
    const float z = (*input)[i].z;
    if (min_depth < z && z < max_depth)
    {
      foreground_mask[i] = true;
      indices->push_back (static_cast<int> (i));
    }
  }

  // Find the dominant plane between the specified near/far thresholds
  constexpr float distance_threshold = 0.02f;
  constexpr int max_iterations = 500;
  pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (distance_threshold);
  seg.setMaxIterations (max_iterations);
  seg.setInputCloud (input);
  seg.setIndices (indices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
  seg.segment (*inliers, *coefficients);

  // Mask off the plane inliers
  for (const auto &index : inliers->indices)
    foreground_mask[index] = false;

  // Mask off any foreground points that are too high above the detected plane
  const std::vector<float> & c = coefficients->values;
  for (std::size_t i = 0; i < input->size (); ++i)
  {
    if (foreground_mask[i])
    {
      const pcl::PointXYZRGBA & p = (*input)[i];
      float d = std::abs (c[0]*p.x + c[1]*p.y + c[2]*p.z + c[3]);
      foreground_mask[i] = (d < max_height);
    }
  }

  return (foreground_mask);
}

void
trainTemplate (const PointCloudXYZRGBA::ConstPtr & input, const std::vector<bool> &foreground_mask,