Function checkGeneralLine

test/geometry/test_iterator.cpp:126–197

template <typename PointT>
void checkGeneralLine (unsigned x_start, unsigned y_start, unsigned x_end, unsigned y_end, PointCloud<PointT>& cloud, bool neighorhood)
{
  PointXYZ point;
  point.x = point.y = point.z = 0.0f;
  for (unsigned yIdx = 0; yIdx < cloud.height; ++yIdx)
  {
    for (unsigned xIdx = 0; xIdx < cloud.width; ++xIdx)
    {
      PointT& point = cloud.points [yIdx * cloud.width + xIdx];
      point.x = static_cast<float>(xIdx);
      point.y = static_cast<float>(yIdx);
      point.z = 0.0f;
    }
  }

  LineIterator::Neighborhood neighbors;
  if (neighorhood)
    neighbors = LineIterator::Neighbor8;
  else
    neighbors = LineIterator::Neighbor4;
    
  LineIterator lineIt (x_start, y_start, x_end, y_end, cloud.width, neighbors);
  // use polymorphic
  OrganizedIndexIterator& iterator = lineIt;
  unsigned idx = 0;
  while (iterator.isValid ())
  {
    PointT& point = cloud [*iterator];
    EXPECT_EQ (point.x, iterator.getColumnIndex ());
    EXPECT_EQ (point.y, iterator.getRowIndex ());
    //std::cout << idx << " :: " << iterator.getPointIndex () << " :: " << iterator.getColumnIndex () << " , " << iterator.getRowIndex () << std::endl;
    point.z = 1.0f;
    ++iterator;
    ++idx;
  }
  
  int dx = x_end - x_start;
  int dy = y_end - y_start;
  unsigned dmax = std::max (std::abs(dx), std::abs(dy));

  if (neighorhood)
    EXPECT_EQ (dmax, idx);
  else
    EXPECT_EQ (std::abs(dx) + std::abs(dy), idx);
  
  float length = std::sqrt (static_cast<float>(dx * dx + dy * dy));
  float dir_x = static_cast<float>(dx) / length;
  float dir_y = static_cast<float>(dy) / length;
  
  // now all z-values should be 0 again!
  for (int yIdx = 0; yIdx < static_cast<int>(cloud.height); ++yIdx)
  {
    for (int xIdx = 0; xIdx < static_cast<int>(cloud.width); ++xIdx)
    {
      PointT& point = cloud.points [yIdx * cloud.width + xIdx];
      if (point.z != 0)
      {
        // point need to be close to line