/** \brief Recompute the plane coefficients using the given inlier set and return them to the user.
* @note: these are the coefficients of the plane model after refinement (eg. after SVD)
* \param inliers the data inliers found as supporting the model
*/
std::vector<double>
SACModelLine::refitModel (std::vector<int> inliers)
{
if (inliers.size () == 0)
return (model_coefficients_);
std::vector<double> refit_coefficients (6);
// Compute the centroid of the samples
std_msgs::Point32 centroid;
// Compute the 3x3 covariance matrix
Eigen::Matrix3d covariance_matrix;
cloud_geometry::nearest::computeCovarianceMatrix (*cloud_, inliers, covariance_matrix, centroid);
refit_coefficients[0] = centroid.x;
refit_coefficients[1] = centroid.y;
refit_coefficients[2] = centroid.z;
// Extract the eigenvalues and eigenvectors
Eigen::Vector3d eigen_values;
Eigen::Matrix3d eigen_vectors;
cloud_geometry::eigen_cov (covariance_matrix, eigen_values, eigen_vectors);
refit_coefficients[3] = eigen_vectors (0, 2) + refit_coefficients[0];
refit_coefficients[4] = eigen_vectors (1, 2) + refit_coefficients[1];
refit_coefficients[5] = eigen_vectors (2, 2) + refit_coefficients[2];
return (refit_coefficients);
}
inline void solvePlaneParametersEigen(const Eigen::Matrix3f &covariance_matrix,
float &nx, float &ny, float &nz,
float &lam0, float &lam1, float &lam2)
{
Eigen::Matrix3f eigen_vectors;
Eigen::Vector3f eigen_values;
Eigen::SelfAdjointEigenSolver
<Eigen::Matrix3f> eigensolver(covariance_matrix);
if (eigensolver.info() != Eigen::Success) {
nx = ny = nz = lam0 = lam1 = lam2 = std::numeric_limits
<float>::quiet_NaN();
return;
}
eigen_vectors = eigensolver.eigenvectors();
eigen_values = eigensolver.eigenvalues();
// first column, no?
nx = eigen_vectors(0, 0);
ny = eigen_vectors(1, 0);
nz = eigen_vectors(2, 0);
lam0 = eigen_values(0);
lam1 = eigen_values(1);
lam2 = eigen_values(2);
}
void
RockPhysicsInversion4D::SolveGEVProblem(NRLib::Matrix sigma_prior,
NRLib::Matrix sigma_posterior,
NRLib::Matrix & vOut){
//Compute transforms v1, v2, v3 and v4 ----------------------------------------
// computing filter
NRLib::SymmetricMatrix sigma_priorInv(6);
for(int i=0;i<6;i++)
for(int j=0;j<=i;j++)
sigma_priorInv(j,i)=sigma_prior(i,j);
NRLib::CholeskyInvert(sigma_priorInv);
NRLib::Matrix eye = NRLib::IdentityMatrix(6);
NRLib::Matrix filter = eye- sigma_priorInv*sigma_posterior;
// computing components
NRLib::Vector eigen_values(6);
NRLib::Matrix eigen_vectors(6,6);
NRLib::ComputeEigenVectors( filter ,eigen_values,eigen_vectors);
// extracting four best components
std::vector<int> current_index(6);
current_index[0] = 0;current_index[1] = 1;
current_index[2] = 2;current_index[3] = 3;
current_index[4] = 4;current_index[5] = 5;
std::vector<int> best_index(4);
std::vector<int> keep_index(6);
keep_index = current_index;
NRLib::Vector current_value(6);
NRLib::Vector keep_value(6);
keep_value = eigen_values;
for(int i=0;i<4;i++){
current_index=keep_index;
current_value =keep_value;
double maxVal=-999; // (the theoretical max value is less than 1 and larger than zero)
for(int j=0;j<6-i;j++){
if(current_value(j) > maxVal){
maxVal=current_value(j);
best_index[i]=current_index[j];
}
}
int counter=0;
for(int j=0;j<6-i;j++){
if(current_value(j) != maxVal){
keep_value(counter)=current_value(j);
keep_index[counter]=current_index[j];
counter++;
}
}
}
vOut.resize(6,4);
for(int i=0;i<4;i++)
for(int j=0;j<6;j++)
vOut(j,i)=eigen_vectors(j,best_index[i]);
}
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;
}
void YAPCAReduce<eltype>::reconstruct_error(const ya_type1 &input,
const ya_type2 &output,
const ya_type3 &dims,
ya_type4 &rmsd_vec) {
YA_DEBUG_ERROR(input.cols()==this->high_dim() &&
output.cols()==this->low_dim(),
"Dimensions in input matrix do not match map");
YA_DEBUG_ERROR(maximum(dims)<=this->low_dim() && minimum(dims)>0,
"A reconstruction dimensionality is greater than map");
YA_DEBUG_ERROR(eigen_vectors.rows()==this->high_dim() &&
eigen_vectors.cols()>=this->low_dim(),
"Map does not match high and low dimensions");
YA_MatT new_coords=rowrep(column_means,input.rows());
rmsd_vec.setup(dims.numel());
ya_sizet dim=0;
ya_sizet dN=dims.numel();
for (ya_sizet i=0; i<dN; i++) {
while (dim<dims(i)) {
new_coords+=output(":",dim)*transpose(eigen_vectors(":",dim));
dim++;
}
rmsd_vec(i)=rmsd(input,new_coords);
}
}
//--------------------------------------------------------------//
void
CheckPositiveDefiniteCorrMatrix(double corr01, double corr02, double corr12, std::string & errTxt)
{
NRLib::Matrix corr_matrix(3, 3, 0);
for(int i=0; i<3; i++)
corr_matrix(i,i) = 1;
corr_matrix(0,1) = corr01;
corr_matrix(1,0) = corr01;
corr_matrix(0,2) = corr02;
corr_matrix(2,0) = corr02;
corr_matrix(1,2) = corr12;
corr_matrix(2,1) = corr12;
NRLib::Vector eigen_values(3, 0);
NRLib::Matrix eigen_vectors(3, 3, 0);
NRLib::ComputeEigenVectors(corr_matrix, eigen_values, eigen_vectors);
bool pos_def = true;
for( int i=0; i<3; i++) {
if(eigen_values(i) < 0)
pos_def = false;
}
if(pos_def == false)
errTxt += "The correlations given in the tabulated rock physics model need to generate a positive definite matrix\n";
}
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);
}
void
Tabulated::CalculateSigmaSqrt(const NRLib::Grid2D<double> & Sigma)
{
// Calculate square root of positive definite correlation matrix
NRLib::Matrix corr_matrix(n_variables_,n_variables_);
for(int i=0; i<n_variables_; i++) {
for(int j=0; j<n_variables_; j++)
corr_matrix(i,j) = Sigma(i,j);
}
NRLib::Vector eigen_values(n_variables_);
NRLib::Matrix eigen_vectors(n_variables_,n_variables_);
NRLib::ComputeEigenVectors(corr_matrix, eigen_values, eigen_vectors);
NRLib::Matrix eigen_vectors_transpose(n_variables_,n_variables_);
for(int i=0; i<n_variables_; i++) {
for(int j=0; j<n_variables_; j++)
eigen_vectors_transpose(j,i) = eigen_vectors(i,j);
}
NRLib::Matrix lambda_square(n_variables_,n_variables_);
for(int i=0; i<n_variables_; i++) {
for(int j=0; j<n_variables_; j++) {
if(i == j)
lambda_square(i,j) = std::sqrt(eigen_values(i));
else
lambda_square(i,j) = 0;
}
}
NRLib::Matrix Sigma_sqrt1(n_variables_,n_variables_);
NRLib::Matrix Sigma_sqrt(n_variables_,n_variables_);
Sigma_sqrt1 = eigen_vectors * lambda_square;
Sigma_sqrt = Sigma_sqrt1 * eigen_vectors_transpose;
for(int i=0; i<n_variables_; i++) {
for(int j=0; j<n_variables_; j++)
Sigma_sqrt_(i,j) = Sigma_sqrt(i,j);
}
}
/**
* estimateNormals
*/
void ZAdaptiveNormals::estimateNormals(const v4r::DataMatrix2D<Eigen::Vector3f> &cloud, const std::vector<int> &normals_indices, std::vector<Eigen::Vector3f> &normals)
{
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
std::vector< int > indices;
#pragma omp parallel for private(eigen_vectors,indices)
for (unsigned i=0; i<normals_indices.size(); i++) {
indices.clear();
int idx = normals_indices[i];
int u = idx%width;
int v = idx/width;
const Eigen::Vector3f &pt = cloud.data[idx];
Eigen::Vector3f &n = normals[i];
if(!std::isnan(pt[0]) && !std::isnan(pt[1]) && !std::isnan(pt[2])) {
if(param.adaptive) {
int dist = (int) (pt[2]*2); // *2 => every 0.5 meter another kernel radius
getIndices(cloud, u,v, param.kernel_radius[dist], indices);
}
else
getIndices(cloud, u,v, param.kernel, indices);
}
if (indices.size()<4) {
n[0] = NaN;
continue;
}
/* curvature= */ computeNormal(cloud, indices, eigen_vectors);
n[0] = eigen_vectors (0,0);
n[1] = eigen_vectors (1,0);
n[2] = eigen_vectors (2,0);
if (n.dot(pt) > 0) {
n *= -1;
//n.getNormalVector4fMap()[3] = 0;
//n.getNormalVector4fMap()[3] = -1 * n.getNormalVector4fMap().dot(pt.getVector4fMap());
}
}
}
template <typename PointInT, typename NormalOutT> void
pcl::MovingLeastSquares<PointInT, NormalOutT>::performReconstruction (PointCloudIn &output)
{
if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
{
PCL_ERROR ("[pcl::%s::performReconstruction] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
output.width = output.height = 0;
output.points.clear ();
if (normals_)
{
normals_->width = normals_->height = 0;
normals_->points.clear ();
}
return;
}
// Compute the number of coefficients
nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
// Allocate enough space to hold the results of nearest neighbor searches
// \note resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices;
std::vector<float> nn_sqr_dists;
// Use original point positions for fitting
// \note no up/down/adapting-sampling or hole filling possible like this
output.points.resize (indices_->size ());
// Check if fake indices were used, otherwise the output loses its organized structure
if (!fake_indices_)
pcl::copyPointCloud (*input_, *indices_, output);
else
output = *input_;
// Resize the output normal dataset
if (normals_)
{
normals_->points.resize (output.points.size ());
normals_->width = output.width;
normals_->height = output.height;
normals_->is_dense = output.is_dense;
}
// For all points
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
// Get the initial estimates of point positions and their neighborhoods
///////////////////////////////////////////////////////////////////////
// Search for the nearest neighbors
if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
{
if (normals_)
normals_->points[cp].normal[0] = normals_->points[cp].normal[1] = normals_->points[cp].normal[2] = normals_->points[cp].curvature = std::numeric_limits<float>::quiet_NaN ();
continue;
}
// Check the number of nearest neighbors for normal estimation (and later
// for polynomial fit as well)
int k = nn_indices.size ();
if (k < 3)
continue;
// Get a plane approximating the local surface's tangent and project point onto it
//////////////////////////////////////////////////////////////////////////////////
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
//pcl::computePointNormal<PointInT> (*input_, nn_indices, model_coefficients, curvature);
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
pcl::compute3DCentroid (*input_, nn_indices, xyz_centroid);
// Compute the 3x3 covariance matrix
pcl::computeCovarianceMatrix (*input_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
// The normalization is not necessary, since the eigenvectors from libeigen are already normalized
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 curvature = 0;
// Compute the curvature surface change
float eig_sum = eigen_values.sum ();
if (eig_sum != 0)
curvature = fabs (eigen_values[0] / eig_sum);
// Projected point
Eigen::Vector3f point = output.points[cp].getVector3fMap ();
float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
point -= distance * model_coefficients.head<3> ();
// Perform polynomial fit to update point and normal
////////////////////////////////////////////////////
if (polynomial_fit_ && k >= nr_coeff_)
{
// For easy change between float and double
typedef Eigen::Vector3d Evector3;
typedef Eigen::VectorXd Evector;
typedef Eigen::MatrixXd Ematrix;
// Get a copy of the plane normal easy access
Evector3 plane_normal = model_coefficients.head<3> ().cast<double> ();
// Update neighborhood, since point was projected, and computing relative
// positions. Note updating only distances for the weights for speed
std::vector<Evector3> de_meaned (k);
for (int ni = 0; ni < k; ++ni)
{
de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
nn_sqr_dists[ni] = de_meaned[ni].dot (de_meaned[ni]);
}
// Allocate matrices and vectors to hold the data used for the polynomial
// fit
Evector weight_vec_ (k);
Ematrix P_ (nr_coeff_, k);
Evector f_vec_ (k);
Evector c_vec_;
Ematrix P_weight_; // size will be (nr_coeff_, k);
Ematrix P_weight_Pt_ (nr_coeff_, nr_coeff_);
// Get local coordinate system (Darboux frame)
Evector3 v = plane_normal.unitOrthogonal ();
Evector3 u = plane_normal.cross (v);
// Go through neighbors, transform them in the local coordinate system,
// save height and the evaluation of the polynome's terms
double u_coord, v_coord, u_pow, v_pow;
for (int ni = 0; ni < k; ++ni)
{
// (re-)compute weights
weight_vec_ (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);
// transforming coordinates
u_coord = de_meaned[ni].dot (u);
v_coord = de_meaned[ni].dot (v);
f_vec_(ni) = de_meaned[ni].dot (plane_normal);
// compute the polynomial's terms at the current point
int j = 0;
u_pow = 1;
for (int ui = 0; ui <= order_; ++ui)
{
v_pow = 1;
for (int vi = 0; vi <= order_ - ui; ++vi)
{
P_ (j++, ni) = u_pow * v_pow;
v_pow *= v_coord;
}
u_pow *= u_coord;
}
}
// Computing coefficients
P_weight_ = P_ * weight_vec_.asDiagonal ();
P_weight_Pt_ = P_weight_ * P_.transpose ();
c_vec_ = P_weight_ * f_vec_;
P_weight_Pt_.llt ().solveInPlace (c_vec_);
// Projection onto MLS surface along Darboux normal to the height at (0,0)
if (pcl_isfinite (c_vec_[0]))
{
point += (c_vec_[0] * plane_normal).cast<float> ();
// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec_[order_+1] and c_vec_[1]
if (normals_)
{
Evector3 n_a = u + plane_normal * c_vec_[order_ + 1];
Evector3 n_b = v + plane_normal * c_vec_[1];
model_coefficients.head<3> () = n_a.cross (n_b).cast<float> ();
model_coefficients.head<3> ().normalize ();
}
}
}
// Save results to output cloud
///////////////////////////////
output.points[cp].x = point[0];
output.points[cp].y = point[1];
output.points[cp].z = point[2];
if (normals_)
{
normals_->points[cp].normal[0] = model_coefficients[0];
normals_->points[cp].normal[1] = model_coefficients[1];
normals_->points[cp].normal[2] = model_coefficients[2];
normals_->points[cp].curvature = curvature;
}
}
}
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
{
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input) {
// std::cerr << "in cloud_cb" << std::endl;
/* 0. Importing input cloud */
std_msgs::Header header = input->header;
// std::string frame_id = input->header.frame_id;
// sensor_msgs::PointCloud2 input_cloud = *input;
pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2; // initialize object
pcl_conversions::toPCL(*input, *cloud); // from input, generate content of cloud
/* 1. Downsampling and Publishing voxel */
// LeafSize: should small enough to caputure a leaf of plants
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); // imutable pointer
pcl::PCLPointCloud2 cloud_voxel; // cloud_filtered to cloud_voxel
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr); // set input
sor.setLeafSize(0.02f, 0.02f, 0.02f); // 2cm, model equation
sor.filter(cloud_voxel); // set output
sensor_msgs::PointCloud2 output_voxel;
pcl_conversions::fromPCL(cloud_voxel, output_voxel);
pub_voxel.publish(output_voxel);
/* 2. Filtering with RANSAC */
// RANSAC initialization
pcl::SACSegmentation<pcl::PointXYZ> seg; // Create the segmentation object
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
seg.setOptimizeCoefficients(true); // Optional
// seg.setModelType(pcl::SACMODEL_PLANE);
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); // Use SACMODEL_PERPENDICULAR_PLANE instead
seg.setMethodType(pcl::SAC_RANSAC);
// minimum number of points calculated from N and distanceThres
seg.setMaxIterations (1000); // N in RANSAC
// seg.setDistanceThreshold(0.025); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
seg.setDistanceThreshold(0.050); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
// param for perpendicular
seg.setAxis(Eigen::Vector3f (0.0, 0.0, 1.0));
seg.setEpsAngle(pcl::deg2rad (10.0f)); // 5.0 -> 10.0
// convert from PointCloud2 to PointXYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(cloud_voxel, *cloud_voxel_xyz);
// RANSAC application
seg.setInputCloud(cloud_voxel_xyz);
seg.segment(*inliers, *coefficients); // values are empty at beginning
// inliers.indices have array index of the points which are included as inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane_xyz (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = inliers->indices.begin ();
pit != inliers->indices.end (); pit++) {
cloud_plane_xyz->points.push_back(cloud_voxel_xyz->points[*pit]);
}
// Organized as an image-structure
cloud_plane_xyz->width = cloud_plane_xyz->points.size ();
cloud_plane_xyz->height = 1;
/* insert code to set arbitary frame_id setting
such as frame_id ="/assemble_cloud_1"
with respect to "/odom or /base_link" */
// Conversions: PointCloud<T>, PCLPointCloud2, sensor_msgs::PointCloud2
pcl::PCLPointCloud2 cloud_plane_pcl;
pcl::toPCLPointCloud2(*cloud_plane_xyz, cloud_plane_pcl);
sensor_msgs::PointCloud2 cloud_plane_ros;
pcl_conversions::fromPCL(cloud_plane_pcl, cloud_plane_ros);
// cloud_plane_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_plane_ros.header.frame_id = header.frame_id;
cloud_plane_ros.header.stamp = header.stamp; // ros::Time::now() -> header.stamp
pub_plane.publish(cloud_plane_ros);
/* 3. PCA application to get eigen */
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(cloud_plane_xyz);
Eigen::Matrix3f eigen_vectors = pca.getEigenVectors(); // 3x3 eigen_vectors(n,m)
/* 4. PCA Visualization */
visualization_msgs::Marker points;
// points.header.frame_id = "/base_link"; // odom -> /base_link
points.header.frame_id = header.frame_id;
points.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
points.ns = "pca"; // namespace + id
points.id = 0; // pca/0
points.action = visualization_msgs::Marker::ADD;
points.type = visualization_msgs::Marker::ARROW;
points.pose.orientation.w = 1.0;
points.scale.x = 0.05;
points.scale.y = 0.05;
points.scale.z = 0.05;
points.color.g = 1.0f;
points.color.r = 0.0f;
points.color.b = 0.0f;
points.color.a = 1.0;
geometry_msgs::Point p_0, p_1;
p_0.x = 0; p_0.y = 0; p_0.z = 0; // get from tf
p_1.x = eigen_vectors(0,0);
p_1.y = eigen_vectors(0,1); // always negative
std::cerr << "y = " << eigen_vectors(0,1) << std::endl;
p_1.z = eigen_vectors(0,2);
points.points.push_back(p_0);
points.points.push_back(p_1);
pub_marker.publish(points);
/* 5. Point Cloud Rotation */
eigen_vectors(0,2) = 0; // ignore very small z-value
double norm = pow((pow(eigen_vectors(0,0), 2) + pow(eigen_vectors(0,1), 2)), 0.5);
double nx = eigen_vectors(0,0) / norm;
double ny = eigen_vectors(0,1) / norm;
Eigen::Matrix4d rot_z; // rotation inversed, convert to Matrix4f
rot_z(0,0) = nx; rot_z(0,1) = ny; rot_z(0,2) = 0; rot_z(0,3) = 0; // ny: +/-
rot_z(1,0) = -ny; rot_z(1,1) = nx; rot_z(1,2) = 0; rot_z(1,3) = 0; // ny: +/-
rot_z(2,0) = 0; rot_z(2,1) = 0; rot_z(2,2) = 1; rot_z(2,3) = 0;
rot_z(3,0) = 0; rot_z(3,1) = 0; rot_z(3,2) = 0; rot_z(3,3) = 1;
// Transformation: Rotation, Translation
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_rot (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*cloudPtr, *cloud_xyz); // from PointCloud2 to PointXYZ
pcl::transformPointCloud(*cloud_xyz, *cloud_xyz_rot, rot_z); // original, transformed, transformation
pcl::PCLPointCloud2 cloud_rot_pcl;
sensor_msgs::PointCloud2 cloud_rot_ros;
pcl::toPCLPointCloud2(*cloud_xyz_rot, cloud_rot_pcl);
pcl_conversions::fromPCL(cloud_rot_pcl, cloud_rot_ros);
pub_rot.publish(cloud_rot_ros);
/* 6. Point Cloud Reduction */
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr >
vector_cloud_separated_xyz = separate(cloud_xyz_rot, header);
pcl::PCLPointCloud2 cloud_separated_pcl;
sensor_msgs::PointCloud2 cloud_separated_ros;
pcl::toPCLPointCloud2(*vector_cloud_separated_xyz.at(1), cloud_separated_pcl);
pcl_conversions::fromPCL(cloud_separated_pcl, cloud_separated_ros);
// cloud_separated_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_separated_ros.header.frame_id = header.frame_id;
cloud_separated_ros.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
pub_red.publish(cloud_separated_ros);
// setMarker
// visualization_msgs::Marker width_min_line;
// width_min_line.header.frame_id = "/base_link";
// width_min_line.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// width_min_line.ns = "width_min";
// width_min_line.action = visualization_msgs::Marker::ADD;
// width_min_line.type = visualization_msgs::Marker::LINE_STRIP;
// width_min_line.pose.orientation.w = 1.0;
// width_min_line.id = 0;
// width_min_line.scale.x = 0.025;
// width_min_line.color.r = 0.0f;
// width_min_line.color.g = 1.0f;
// width_min_line.color.b = 0.0f;
// width_min_line.color.a = 1.0;
// width_min_line.points.push_back(point_vector.at(0));
// width_min_line.points.push_back(point_vector.at(2));
// pub_marker.publish(width_min_line);
// /* 6. Visualize center line */
// visualization_msgs::Marker line_strip;
// line_strip.header.frame_id = "/base_link"; // odom -> /base_link
// line_strip.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// line_strip.ns = "center";
// line_strip.action = visualization_msgs::Marker::ADD;
// line_strip.type = visualization_msgs::Marker::LINE_STRIP;
// line_strip.pose.orientation.w = 1.0;
// line_strip.id = 0; // set id
// line_strip.scale.x = 0.05;
// line_strip.color.r = 1.0f;
// line_strip.color.g = 0.0f;
// line_strip.color.b = 0.0f;
// line_strip.color.a = 1.0;
// // geometry_msgs::Point p_stitch, p_min;
// p_s.x = 0; p_s.y = 0; p_s.z = 0;
// p_e.x = p_m.x; p_e.y = p_m.y; p_e.z = 0;
// line_strip.points.push_back(p_s);
// line_strip.points.push_back(p_e);
// pub_marker.publish(line_strip);
/* PCA Visualization */
// geometry_msgs::Pose pose; tf::poseEigenToMsg(pca.getEigenVectors, pose);
/* to use Pose marker in rviz */
/* Automatic Measurement */
// 0-a. stitch measurement: -0.5 < z < -0.3
// 0-b. min width measurement: 0.3 < z < 5m
// 1. iterate
// 2. pick point if y < 0
// 3. compare point with all points if 0 < y
// 4. pick point-pare recording shortest distance
// 5. compare the point with previous point
// 6. update min
// 7. display value in text in between 2 points
}
