EdgeAwarePlaneComparator.h

bool
compare (int idx1, int idx2) const
{
  bool dist_ok;
  if(this->depth_dependent_)
  {
    Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
    float z = vec.dot (this->z_axis_);
    dist_ok = (fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < distance_threshold_ * z * z);
  }
  else
  {
    float dx = input_->points[idx1].x - input_->points[idx2].x;
    float dy = input_->points[idx1].y - input_->points[idx2].y;
    float dz = input_->points[idx1].z - input_->points[idx2].z;
    float dist = sqrtf (dx*dx + dy*dy + dz*dz);
    dist_ok = (dist < distance_threshold_);
  }
  bool normal_ok = (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ );
  bool curvature_ok = normals_->points[idx1].curvature < 0.04;
  bool plane_d_ok = fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < 0.04;

  if (distance_map_[idx1] < 5)    // 5 pixels
    curvature_ok = false;

  return (dist_ok && normal_ok && curvature_ok && plane_d_ok);
}