template <typename PointT, typename Scalar> inline unsigned int
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const pcl::PointIndices &indices,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix));
}
template<typename PointT, typename PointNT> double
pcl::CovarianceSampling<PointT, PointNT>::computeConditionNumber ()
{
Eigen::Matrix<double, 6, 6> covariance_matrix;
if (!computeCovarianceMatrix (covariance_matrix))
return (-1.);
Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
eigen_solver.compute (covariance_matrix, true);
Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues ();
double max_ev = -std::numeric_limits<double>::max (),
min_ev = std::numeric_limits<double>::max ();
for (size_t i = 0; i < 6; ++i)
{
if (real (complex_eigenvalues (i, 0)) > max_ev)
max_ev = real (complex_eigenvalues (i, 0));
if (real (complex_eigenvalues (i, 0)) < min_ev)
min_ev = real (complex_eigenvalues (i, 0));
}
return (max_ev / min_ev);
}
/*!
Constructs a robust multiresolution estimator by implementing
the iterative reweighted least squares (IRLS) technique.
*/
CMotion2DEstimator::CMotion2DEstimator()
{
if (DEBUG_LEVEL1)
cout << "Debut CMotion2DEstimator::CMotion2DEstimator()" << endl;
rapport_poids = 0.0;
sigma2res = 0.0;
verbose = false;
setRobustEstimator(true);
setNIterationMaxLevel(7);
setLastEstimationLevel(0);
setFirstEstimationLevel(3);
setLevelParameterIntroduction(-1, -1, -1);
setRobustFunction(Tukey);
setCConstant(CConst_Fixed, 8.0);
setNIterationMaxIRLS(8);
setReliableSupportRate(0.4f);
computeSupportSize(true, 0.2f);
computeResidualVariance(false);
computeCovarianceMatrix(false);
nrows = 0;
ncols = 0;
pyr_level_max = 0;
init_mem_weightsIsDone = false;
init_mem_estIsDone = false;
if (DEBUG_LEVEL1)
cout << "Fin CMotion2DEstimator::CMotion2DEstimator()" << endl;
}
template <typename PointInT, typename PointOutT> void
pcl::TBB_NormalEstimationTBB<PointInT, PointOutT>::operator () (const tbb::blocked_range <size_t> &r) const
{
float vpx, vpy, vpz;
feature_->getViewPoint (vpx, vpy, vpz);
// Iterating over the entire index vector
for (size_t idx = r.begin (); idx != r.end (); ++idx)
{
std::vector<int> nn_indices (feature_->getKSearch ());
std::vector<float> nn_dists (feature_->getKSearch ());
feature_->searchForNeighbors ((*feature_->getIndices ())[idx], feature_->getSearchParameter (), nn_indices, nn_dists);
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*feature_->getSearchSurface (), nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*feature_->getSearchSurface (), nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output_.points[idx].normal[0], output_.points[idx].normal[1], output_.points[idx].normal[2], output_.points[idx].curvature);
flipNormalTowardsViewpoint<PointInT> (feature_->getSearchSurface ()->points[idx], vpx, vpy, vpz,
output_.points[idx].normal[0], output_.points[idx].normal[1], output_.points[idx].normal[2]);
}
}
template<typename PointT, typename PointNT> double
pcl::CovarianceSampling<PointT, PointNT>::computeConditionNumber ()
{
Eigen::Matrix<double, 6, 6> covariance_matrix;
if (!computeCovarianceMatrix (covariance_matrix))
return (-1.);
return computeConditionNumber (covariance_matrix);
}
template <typename PointInT> void
pcl16::NormalEstimationOMP<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl16::PointCloud<Eigen::MatrixXf> &output)
{
float vpx, vpy, vpz;
getViewPoint (vpx, vpy, vpz);
output.is_dense = true;
// Resize the output dataset
output.points.resize (indices_->size (), 4);
// GCC 4.2.x seems to segfault with "internal compiler error" on MacOS X here
#if defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2))
#pragma omp parallel for schedule (dynamic, threads_)
#endif
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*surface_, nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3));
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
}
}
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
optimized_coefficients = model_coefficients;
return;
}
// Need at least 3 points to estimate a plane
if (inliers.size () < 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
Eigen::Vector4f plane_parameters;
// Use Least-Squares to fit the plane through all the given sample points and find out its coefficients
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*input_, inliers, xyz_centroid);
xyz_centroid[3] = 0;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*input_, inliers, xyz_centroid, covariance_matrix);
// Compute the model coefficients
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
// Hessian form (D = nc . p_plane (centroid here) + p)
optimized_coefficients.resize (4);
optimized_coefficients[0] = eigen_vectors (0, 0);
optimized_coefficients[1] = eigen_vectors (1, 0);
optimized_coefficients[2] = eigen_vectors (2, 0);
optimized_coefficients[3] = 0;
optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid);
}
template <typename PointInT, typename PointOutT> void
pcl16::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
float vpx, vpy, vpz;
getViewPoint (vpx, vpy, vpz);
output.is_dense = true;
// Iterating over the entire index vector
#pragma omp parallel for schedule (dynamic, threads_)
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*surface_, nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
}
}
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
{
optimized_coefficients = model_coefficients;
return;
}
// Need at least 2 points to estimate a line
if (inliers.size () <= 2)
{
PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
optimized_coefficients.resize (6);
// Compute the 3x3 covariance matrix
Eigen::Vector4f centroid;
compute3DCentroid (*input_, inliers, centroid);
Eigen::Matrix3f covariance_matrix;
computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix);
optimized_coefficients[0] = centroid[0];
optimized_coefficients[1] = centroid[1];
optimized_coefficients[2] = centroid[2];
// Extract the eigenvalues and eigenvectors
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
pcl::eigen33 (covariance_matrix, eigen_values);
pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector);
//pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
//optimized_coefficients.template tail<3> () = eigen_vector;
optimized_coefficients[0] = eigen_vector[0];
optimized_coefficients[1] = eigen_vector[1];
optimized_coefficients[2] = eigen_vector[2];
}
/**
* ComputeNormal
*/
float ZAdaptiveNormals::computeNormal(const v4r::DataMatrix2D<Eigen::Vector3f> &cloud, std::vector<int> &indices, Eigen::Matrix3f &eigen_vectors)
{
if (indices.size()<4)
return NaN;
Eigen::Vector3f mean;
mean.setZero();
for (unsigned j=0; j<indices.size(); j++)
mean += cloud.data[indices[j]];
mean /= (float)indices.size();
Eigen::Matrix3f cov;
computeCovarianceMatrix (cloud, indices, mean, cov);
Eigen::Vector3f eigen_values;
v4r::eigen33 (cov, eigen_vectors, eigen_values);
float eigsum = eigen_values.sum();
if (eigsum != 0)
return fabs (eigen_values[0] / eigsum );
return NaN;
}
template<typename PointT, typename PointNT> void
pcl::CovarianceSampling<PointT, PointNT>::applyFilter (std::vector<int> &sampled_indices)
{
Eigen::Matrix<double, 6, 6> c_mat;
// Invokes initCompute()
if (!computeCovarianceMatrix (c_mat))
return;
const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6> > solver (c_mat);
const Eigen::Matrix<double, 6, 6> x = solver.eigenvectors ();
//--- Part B from the paper
/// TODO figure out how to fill the candidate_indices - see subsequent paper paragraphs
std::vector<size_t> candidate_indices;
candidate_indices.resize (indices_->size ());
for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
candidate_indices[p_i] = p_i;
// Compute the v 6-vectors
typedef Eigen::Matrix<double, 6, 1> Vector6d;
std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
v.resize (candidate_indices.size ());
for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
{
v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross (
(*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).template cast<double> ();
v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast<double> ();
}
// Set up the lists to be sorted
std::vector<std::list<std::pair<int, double> > > L;
L.resize (6);
for (size_t i = 0; i < 6; ++i)
{
for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
L[i].push_back (std::make_pair (p_i, fabs (v[p_i].dot (x.block<6, 1> (0, i)))));
// Sort in decreasing order
L[i].sort (sort_dot_list_function);
}
// Initialize the 6 t's
std::vector<double> t (6, 0.0);
sampled_indices.resize (num_samples_);
std::vector<bool> point_sampled (candidate_indices.size (), false);
// Now select the actual points
for (size_t sample_i = 0; sample_i < num_samples_; ++sample_i)
{
// Find the most unconstrained dimension, i.e., the minimum t
size_t min_t_i = 0;
for (size_t i = 0; i < 6; ++i)
{
if (t[min_t_i] > t[i])
min_t_i = i;
}
// Add the point from the top of the list corresponding to the dimension to the set of samples
while (point_sampled [L[min_t_i].front ().first])
L[min_t_i].pop_front ();
sampled_indices[sample_i] = L[min_t_i].front ().first;
point_sampled[L[min_t_i].front ().first] = true;
L[min_t_i].pop_front ();
// Update the running totals
for (size_t i = 0; i < 6; ++i)
{
double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i));
t[i] += val * val;
}
}
// Remap the sampled_indices to the input_ cloud
for (int &sampled_index : sampled_indices)
sampled_index = (*indices_)[candidate_indices[sampled_index]];
}
template <typename PointT> bool
pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
{
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (polygon, xyz_centroid);
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (polygon, xyz_centroid, covariance_matrix);
// Compute the model coefficients
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
eigen33 (covariance_matrix, eigen_vectors, eigen_values);
model_coefficients[0] = eigen_vectors (0, 0);
model_coefficients[1] = eigen_vectors (1, 0);
model_coefficients[2] = eigen_vectors (2, 0);
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
float distance_to_plane = model_coefficients[0] * point.x +
model_coefficients[1] * point.y +
model_coefficients[2] * point.z +
model_coefficients[3];
PointT ppoint;
// Calculate the projection of the point on the plane
ppoint.x = point.x - distance_to_plane * model_coefficients[0];
ppoint.y = point.y - distance_to_plane * model_coefficients[1];
ppoint.z = point.z - distance_to_plane * model_coefficients[2];
// Create a X-Y projected representation for within bounds polygonal checking
int k0, k1, k2;
// Determine the best plane to project points onto
k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
k1 = (k0 + 1) % 3;
k2 = (k0 + 2) % 3;
// Project the convex hull
pcl::PointCloud<PointT> xy_polygon;
xy_polygon.points.resize (polygon.points.size ());
for (size_t i = 0; i < polygon.points.size (); ++i)
{
Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0);
xy_polygon.points[i].x = pt[k1];
xy_polygon.points[i].y = pt[k2];
xy_polygon.points[i].z = 0;
}
PointT xy_point;
xy_point.z = 0;
Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
xy_point.x = pt[k1];
xy_point.y = pt[k2];
return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon));
}
template <typename PointT> void
pcl::ExtractPolygonalPrismDataDebug<PointT>::segment (pcl::PointIndices &output)
{
output.header = input_->header;
if (!initCompute ())
{
output.indices.clear ();
return;
}
if ((int)planar_hull_->points.size () < min_pts_hull_)
{
PCL_ERROR ("[pcl::%s::segment] Not enough points (%lu) in the hull!\n", getClassName ().c_str (), (unsigned long)planar_hull_->points.size ());
output.indices.clear ();
return;
}
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*planar_hull_, xyz_centroid);
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*planar_hull_, xyz_centroid, covariance_matrix);
// Compute the model coefficients
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
eigen33 (covariance_matrix, eigen_vectors, eigen_values);
model_coefficients[0] = eigen_vectors (0, 0);
model_coefficients[1] = eigen_vectors (1, 0);
model_coefficients[2] = eigen_vectors (2, 0);
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
// Need to flip the plane normal towards the viewpoint
Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
// See if we need to flip any plane normals
vp -= planar_hull_->points[0].getVector4fMap ();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
float cos_theta = vp.dot (model_coefficients);
// Flip the plane normal
if (cos_theta < 0)
{
model_coefficients *= -1;
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ()));
}
// Project all points
PointCloud projected_points;
SampleConsensusModelPlane<PointT> sacmodel (input_);
sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false);
// Create a X-Y projected representation for within bounds polygonal checking
int k0, k1, k2;
// Determine the best plane to project points onto
k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
k1 = (k0 + 1) % 3;
k2 = (k0 + 2) % 3;
// Project the convex hull
pcl::PointCloud<PointT> polygon;
polygon.points.resize (planar_hull_->points.size ());
for (size_t i = 0; i < planar_hull_->points.size (); ++i)
{
Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0);
polygon.points[i].x = pt[k1];
polygon.points[i].y = pt[k2];
polygon.points[i].z = 0;
}
PointT pt_xy;
pt_xy.z = 0;
output.indices.resize (indices_->size ());
int l = 0;
int count_inside = 0;
int count_dist = 0;
for (size_t i = 0; i < projected_points.points.size (); ++i)
{
// Check the distance to the user imposed limits from the table planar model
double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients);
if (distance < height_limit_min_ || distance > height_limit_max_){
count_dist++;
continue;
}
// Check what points are inside the hull
Eigen::Vector4f pt (projected_points.points[i].x,
projected_points.points[i].y,
projected_points.points[i].z, 0);
pt_xy.x = pt[k1];
pt_xy.y = pt[k2];
if (!pcl::isXYPointIn2DXYPolygon(pt_xy, polygon, vertices_)){
count_inside++;
continue;
}
output.indices[l++] = (*indices_)[i];
}
output.indices.resize (l);
std::cout << count_dist << " Points not complying with height limits " << count_inside << "Points not inside 2D polygon" << std::endl;
deinitCompute ();
}
template <typename PointInT> void
pcl::ConvexHull<PointInT>::performReconstruction (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
bool fill_polygon_data)
{
// FInd the principal directions
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
compute3DCentroid (*input_, *indices_, xyz_centroid);
computeCovarianceMatrix (*input_, *indices_, xyz_centroid, covariance_matrix);
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
Eigen::Affine3f transform1;
transform1.setIdentity ();
int dim = 3;
if (eigen_values[0] / eigen_values[2] < 1.0e-5)
{
// We have points laying on a plane, using 2d convex hull
// Compute transformation bring eigen_vectors.col(i) to z-axis
eigen_vectors.col (2) = eigen_vectors.col (0).cross (eigen_vectors.col (1));
eigen_vectors.col (1) = eigen_vectors.col (2).cross (eigen_vectors.col (0));
transform1 (0, 2) = eigen_vectors (0, 0);
transform1 (1, 2) = eigen_vectors (1, 0);
transform1 (2, 2) = eigen_vectors (2, 0);
transform1 (0, 1) = eigen_vectors (0, 1);
transform1 (1, 1) = eigen_vectors (1, 1);
transform1 (2, 1) = eigen_vectors (2, 1);
transform1 (0, 0) = eigen_vectors (0, 2);
transform1 (1, 0) = eigen_vectors (1, 2);
transform1 (2, 0) = eigen_vectors (2, 2);
transform1 = transform1.inverse ();
dim = 2;
}
else
transform1.setIdentity ();
PointCloud cloud_transformed;
pcl::demeanPointCloud (*input_, *indices_, xyz_centroid, cloud_transformed);
pcl::transformPointCloud (cloud_transformed, cloud_transformed, transform1);
// True if qhull should free points in qh_freeqhull() or reallocation
boolT ismalloc = True;
// option flags for qhull, see qh_opt.htm
char flags[] = "qhull Tc";
// output from qh_produce_output(), use NULL to skip qh_produce_output()
FILE *outfile = NULL;
// error messages from qhull code
FILE *errfile = stderr;
// 0 if no error from qhull
int exitcode;
// Array of coordinates for each point
coordT *points = (coordT *)calloc (cloud_transformed.points.size () * dim, sizeof(coordT));
for (size_t i = 0; i < cloud_transformed.points.size (); ++i)
{
points[i * dim + 0] = (coordT)cloud_transformed.points[i].x;
points[i * dim + 1] = (coordT)cloud_transformed.points[i].y;
if (dim > 2)
points[i * dim + 2] = (coordT)cloud_transformed.points[i].z;
}
// Compute convex hull
exitcode = qh_new_qhull (dim, cloud_transformed.points.size (), points, ismalloc, flags, outfile, errfile);
if (exitcode != 0)
{
PCL_ERROR ("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n",
getClassName ().c_str (), (unsigned long) input_->points.size ());
//check if it fails because of NaN values...
if (!cloud_transformed.is_dense)
{
bool NaNvalues = false;
for (size_t i = 0; i < cloud_transformed.size (); ++i)
{
if (!pcl_isfinite (cloud_transformed.points[i].x) ||
!pcl_isfinite (cloud_transformed.points[i].y) ||
!pcl_isfinite (cloud_transformed.points[i].z))
{
NaNvalues = true;
break;
}
}
if (NaNvalues)
PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
}
hull.points.resize (0);
hull.width = hull.height = 0;
polygons.resize (0);
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
return;
}
qh_triangulate ();
int num_facets = qh num_facets;
int num_vertices = qh num_vertices;
hull.points.resize (num_vertices);
vertexT * vertex;
int i = 0;
// Max vertex id
int max_vertex_id = -1;
FORALLvertices
{
if ((int)vertex->id > max_vertex_id)
max_vertex_id = vertex->id;
}
++max_vertex_id;
std::vector<int> qhid_to_pcidx (max_vertex_id);
FORALLvertices
{
// Add vertices to hull point_cloud
hull.points[i].x = vertex->point[0];
hull.points[i].y = vertex->point[1];
if (dim>2)
hull.points[i].z = vertex->point[2];
else
hull.points[i].z = 0;
qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
++i;
}
if (fill_polygon_data)
{
if (dim == 3)
{
polygons.resize (num_facets);
int dd = 0;
facetT * facet;
FORALLfacets
{
polygons[dd].vertices.resize (3);
// Needed by FOREACHvertex_i_
int vertex_n, vertex_i;
FOREACHvertex_i_((*facet).vertices)
//facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
++dd;
}
}
else
{