Пример #1
0
template <typename PointT> inline void
pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
                  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
{
  min_pt.setConstant (FLT_MAX);
  max_pt.setConstant (-FLT_MAX);

  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < indices.size (); ++i)
    {
      pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap ();
      min_pt = min_pt.array ().min (pt);
      max_pt = max_pt.array ().max (pt);
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    for (size_t i = 0; i < indices.size (); ++i)
    {
      // Check if the point is invalid
      if (!pcl_isfinite (cloud.points[indices[i]].x) || 
          !pcl_isfinite (cloud.points[indices[i]].y) || 
          !pcl_isfinite (cloud.points[indices[i]].z))
        continue;
      pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap ();
      min_pt = min_pt.array ().min (pt);
      max_pt = max_pt.array ().max (pt);
    }
  }
}
Пример #2
0
void getMinMax(const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_p, Eigen::Vector4f &max_p) {
    min_p.setConstant(FLT_MAX);
    max_p.setConstant(-FLT_MAX);
    min_p[3] = max_p[3] = 0;

    for (size_t i = 0; i < cloud.points.size(); ++i) {
        if (cloud.points[i].x < min_p[0]) min_p[0] = cloud.points[i].x;
        if (cloud.points[i].y < min_p[1]) min_p[1] = cloud.points[i].y;
        if (cloud.points[i].z < min_p[2]) min_p[2] = cloud.points[i].z;

        if (cloud.points[i].x > max_p[0]) max_p[0] = cloud.points[i].x;
        if (cloud.points[i].y > max_p[1]) max_p[1] = cloud.points[i].y;
        if (cloud.points[i].z > max_p[2]) max_p[2] = cloud.points[i].z;
    }
}
Пример #3
0
template <typename PointT> void
pcl::MaximumLikelihoodSampleConsensus<PointT>::getMinMax (
    const PointCloudConstPtr &cloud, const IndicesPtr &indices, Eigen::Vector4f &min_p, Eigen::Vector4f &max_p)
{
  min_p.setConstant (FLT_MAX);
  max_p.setConstant (-FLT_MAX);
  min_p[3] = max_p[3] = 0;

  for (size_t i = 0; i < indices->size (); ++i)
  {
    if (cloud->points[(*indices)[i]].x < min_p[0]) min_p[0] = cloud->points[(*indices)[i]].x;
    if (cloud->points[(*indices)[i]].y < min_p[1]) min_p[1] = cloud->points[(*indices)[i]].y;
    if (cloud->points[(*indices)[i]].z < min_p[2]) min_p[2] = cloud->points[(*indices)[i]].z;

    if (cloud->points[(*indices)[i]].x > max_p[0]) max_p[0] = cloud->points[(*indices)[i]].x;
    if (cloud->points[(*indices)[i]].y > max_p[1]) max_p[1] = cloud->points[(*indices)[i]].y;
    if (cloud->points[(*indices)[i]].z > max_p[2]) max_p[2] = cloud->points[(*indices)[i]].z;
  }
}
Пример #4
0
inline void
pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 
                           const Eigen::Vector4f &point,
                           Eigen::Vector4f &plane_parameters, float &curvature)
{
  // Avoid getting hung on Eigen's optimizers
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
      if (!pcl_isfinite (covariance_matrix (i, j)))
      {
        //ROS_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!");
        plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
        curvature = std::numeric_limits<float>::quiet_NaN ();
        return;
      }

  // Extract the eigenvalues and eigenvectors
  //Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> ei_symm (covariance_matrix);
  //EIGEN_ALIGN16 Eigen::Vector3f eigen_values  = ei_symm.eigenvalues ();
  //EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors = ei_symm.eigenvectors ();
  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
  EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
  pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);

  // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
  // Note: Remember to take care of the eigen_vectors ordering
  //float norm = 1.0 / eigen_vectors.col (0).norm ();

  //plane_parameters[0] = eigen_vectors (0, 0) * norm;
  //plane_parameters[1] = eigen_vectors (1, 0) * norm;
  //plane_parameters[2] = eigen_vectors (2, 0) * norm;

  // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
  plane_parameters[0] = eigen_vectors (0, 0);
  plane_parameters[1] = eigen_vectors (1, 0);
  plane_parameters[2] = eigen_vectors (2, 0);
  plane_parameters[3] = 0;

  // Hessian form (D = nc . p_plane (centroid here) + p)
  plane_parameters[3] = -1 * plane_parameters.dot (point);

  // Compute the curvature surface change
  float eig_sum = eigen_values.sum ();
  if (eig_sum != 0)
    curvature = fabs ( eigen_values[0] / eig_sum );
  else
    curvature = 0;
}