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);
}
}
}
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;
}
}
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;
}
}
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;
}
