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);
}
}
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
coeff[3] = 0;
distances.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
// Calculate the angular distance between the point normal and the plane normal
double d_normal = fabs (getAngle3D (n, coeff));
d_normal = (std::min) (d_normal, M_PI - d_normal);
distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
}
}
template <typename PointT> void
pcl::MaximumLikelihoodSampleConsensus<PointT>::getMinMax (
const PointCloudConstPtr &cloud,
const boost::shared_ptr <std::vector<int> > &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)
{
solvePlaneParameters (covariance_matrix,
(float &)plane_parameters [0], (float &)plane_parameters [1], (float &)plane_parameters [2],
curvature);
plane_parameters[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
plane_parameters[3] = -1 * plane_parameters.dot (point);
}
template <typename PointT, typename PointNT> int
pcl::SampleConsensusModelCylinder<PointT, PointNT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return (0);
int nr_p = 0;
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float ptdotdir = line_pt.dot (line_dir);
float dirdotdir = 1.0f / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
{
// Aproximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = line_pt + k * line_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
nr_p++;
}
return (nr_p);
}
//TODO: move to semantics
void
PlaneExtraction::findClosestTable(std::vector<pcl::PointCloud<Point>, Eigen::aligned_allocator<pcl::PointCloud<Point> > >& v_cloud_hull,
std::vector<pcl::ModelCoefficients>& v_coefficients_plane,
Eigen::Vector3f& robot_pose,
unsigned int& idx)
{
std::vector<unsigned int> table_candidates;
for(unsigned int i=0; i<v_cloud_hull.size(); i++)
{
//if(fabs(v_coefficients_plane[i].values[3])>0.5 && fabs(v_coefficients_plane[i].values[3])<1.2)
table_candidates.push_back(i);
}
if(table_candidates.size()>0)
{
for(unsigned int i=0; i<table_candidates.size(); i++)
{
double d_min = 1000;
double d = d_min;
Eigen::Vector4f centroid;
pcl::compute3DCentroid(v_cloud_hull[i], centroid);
//for(unsigned int j=0; j<v_cloud_hull[i].size(); j++)
//{
// Eigen::Vector3f p = v_cloud_hull[i].points[j].getVector3fMap();
// d += fabs((p-robot_pose).norm());
//}
//d /= v_cloud_hull[i].size();
Eigen::Vector3f centroid3 = centroid.head(3);
d = fabs((centroid3-robot_pose).norm());
ROS_INFO("d: %f", d);
if(d<d_min)
{
d_min = d;
idx = table_candidates[i];
}
}
}
}
geometry_msgs::Quaternion calc_quaternion_average( std::vector<geometry_msgs::Pose> pose_vector ){
Eigen::Matrix4f averaging_matrix;
Eigen::Vector4f quaternion;
averaging_matrix.setZero();
for( unsigned int sample_ii=0; sample_ii<pose_vector.size(); sample_ii++){
quaternion(0) = pose_vector[sample_ii].orientation.x;
quaternion(1) = pose_vector[sample_ii].orientation.y;
quaternion(2) = pose_vector[sample_ii].orientation.z;
quaternion(3) = pose_vector[sample_ii].orientation.w;
averaging_matrix = averaging_matrix + quaternion*quaternion.transpose();
}// for all samples
Eigen::EigenSolver<Eigen::Matrix4f> ev_solver;
ev_solver.compute( averaging_matrix);
Eigen::Vector4cf eigen_values = ev_solver.eigenvalues();
float max_ev=eigen_values(0).real();
unsigned int max_ii = 0;
for( unsigned int ev_ii=1; ev_ii<4; ev_ii++){
if( eigen_values(ev_ii).real()>max_ev ){
max_ev = eigen_values(ev_ii).real();
max_ii=ev_ii;
}
}
Eigen::Vector4f eigen_vector = ev_solver.eigenvectors().col(max_ii).real();
eigen_vector.normalize();
geometry_msgs::Quaternion quaternion_msg;
quaternion_msg.x = eigen_vector(0);
quaternion_msg.y = eigen_vector(1);
quaternion_msg.z = eigen_vector(2);
quaternion_msg.w = eigen_vector(3);
return quaternion_msg;
}
template <typename PointT, typename PointNT> int
pcl::SampleConsensusModelCone<PointT, PointNT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return (0);
int nr_p = 0;
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = apex + k * axis_dir;
// Calculate the direction of the point from center
Eigen::Vector4f pp_pt_dir = pt - pt_proj;
pp_pt_dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pt_proj;
double actual_cone_radius = tan(opening_angle) * height.norm ();
height.normalize ();
// Calculate the cones perfect normals
Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * pp_pt_dir;
// Aproximate the distance from the point to the cone as the difference between
// dist(point,cone_axis) and actual cone radius
double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, cone_normal));
d_normal = (std::min) (d_normal, M_PI - d_normal);
if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
nr_p++;
}
return (nr_p);
}
template <typename PointInT> double
pcl::tracking::NormalCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
{
Eigen::Vector4f n = source.getNormalVector4fMap ();
Eigen::Vector4f n_dash = target.getNormalVector4fMap ();
if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 )
{
PCL_ERROR("norm might be ZERO!\n");
std::cout << "source: " << source << std::endl;
std::cout << "target: " << target << std::endl;
exit (1);
return 0.0;
}
else
{
n.normalize ();
n_dash.normalize ();
double theta = pcl::getAngle3D (n, n_dash);
if (!pcl_isnan (theta))
return 1.0 / (1.0 + weight_ * theta * theta);
else
return 0.0;
}
}
vtkSmartPointer<vtkDataSet>
pcl::visualization::createLine (const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
{
vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
line->SetPoint1 (pt1.x (), pt1.y (), pt1.z ());
line->SetPoint2 (pt2.x (), pt2.y (), pt2.z ());
line->Update ();
return (line->GetOutput ());
}
bool
pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a,
const Eigen::VectorXf &line_b,
Eigen::Vector4f &point, double sqr_eps)
{
Eigen::Vector4f p1, p2;
lineToLineSegment (line_a, line_b, p1, p2);
// If the segment size is smaller than a pre-given epsilon...
double sqr_dist = (p1 - p2).squaredNorm ();
if (sqr_dist < sqr_eps)
{
point = p1;
return (true);
}
point.setZero ();
return (false);
}
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Needs a valid model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return (false);
}
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float openning_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
{
Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pt_proj;
double actual_cone_radius = tan (openning_angle) * height.norm ();
// Aproximate the distance from the point to the cone as the difference between
// dist(point,cone_axis) and actual cone radius
if (fabs (static_cast<double>(pointToAxisDistance (pt, model_coefficients) - actual_cone_radius)) > threshold)
return (false);
}
return (true);
}
template <typename PointT> inline unsigned int
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f ¢roid)
{
if (cloud.points.empty ())
return (0);
// Initialize to 0
centroid.setZero ();
// For each point in the cloud
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
for (size_t i = 0; i < cloud.points.size (); ++i)
centroid += cloud.points[i].getVector4fMap ();
centroid[3] = 0;
centroid /= static_cast<float> (cloud.points.size ());
return (static_cast<unsigned int> (cloud.points.size ()));
}
// NaN or Inf values could exist => check for them
else
{
unsigned cp = 0;
for (size_t i = 0; i < cloud.points.size (); ++i)
{
// Check if the point is invalid
if (!isFinite (cloud [i]))
continue;
centroid += cloud[i].getVector4fMap ();
++cp;
}
centroid[3] = 0;
centroid /= static_cast<float> (cp);
return (cp);
}
}
template <typename PointT> inline void
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
Eigen::Vector4f ¢roid)
{
// Initialize to 0
centroid.setZero ();
if (indices.empty ())
return;
// For each point in the cloud
int cp = 0;
// 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)
centroid += cloud.points[indices[i]].getVector4fMap ();
centroid[3] = 0;
centroid /= indices.size ();
}
// 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;
centroid += cloud.points[indices[i]].getVector4fMap ();
cp++;
}
centroid[3] = 0;
centroid /= cp;
}
}
template<typename PointType> void
pcl::apps::DominantPlaneSegmentation<PointType>::compute_table_plane ()
{
// Has the input dataset been set already?
if (!input_)
{
PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
CloudConstPtr cloud_;
CloudPtr cloud_filtered_ (new Cloud ());
CloudPtr cloud_downsampled_ (new Cloud ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
CloudPtr table_projected_ (new Cloud ());
CloudPtr table_hull_ (new Cloud ());
typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
// Normal estimation parameters
n3d_.setKSearch (k_);
n3d_.setSearchMethod (normals_tree_);
// Table model fitting parameters
seg_.setDistanceThreshold (sac_distance_threshold_);
seg_.setMaxIterations (2000);
seg_.setNormalDistanceWeight (0.1);
seg_.setOptimizeCoefficients (true);
seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg_.setMethodType (pcl::SAC_RANSAC);
seg_.setProbability (0.99);
proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
// ---[ PassThroughFilter
pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
pass_.setFilterFieldName ("z");
pass_.setInputCloud (input_);
pass_.filter (*cloud_filtered_);
if (int (cloud_filtered_->points.size ()) < k_)
{
PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.",
cloud_filtered_->points.size ());
return;
}
// Downsample the point cloud
grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
grid_.setDownsampleAllData (false);
grid_.setInputCloud (cloud_filtered_);
grid_.filter (*cloud_downsampled_);
// ---[ Estimate the point normals
n3d_.setInputCloud (cloud_downsampled_);
n3d_.compute (*cloud_normals_);
// ---[ Perform segmentation
seg_.setInputCloud (cloud_downsampled_);
seg_.setInputNormals (cloud_normals_);
seg_.segment (*table_inliers_, *table_coefficients_);
if (table_inliers_->indices.size () == 0)
{
PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
proj_.setInputCloud (cloud_downsampled_);
proj_.setIndices (table_inliers_);
proj_.setModelCoefficients (table_coefficients_);
proj_.filter (*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
CloudPtr table_hull (new Cloud ());
hull_.setInputCloud (table_projected_);
hull_.reconstruct (*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
model_coefficients[0] = table_coefficients_->values[0];
model_coefficients[1] = table_coefficients_->values[1];
model_coefficients[2] = table_coefficients_->values[2];
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
Eigen::Vector4f vp (0, 0, 0, 0);
// See if we need to flip any plane normals
vp -= table_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 (table_hull->points[0].getVector4fMap ()));
}
//Set table_coeffs
table_coeffs_ = model_coefficients;
}
template<typename PointType> void
pcl::apps::DominantPlaneSegmentation<PointType>::compute_fast (std::vector<CloudPtr> & clusters)
{
// Has the input dataset been set already?
if (!input_)
{
PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
// Is the input dataset organized?
if (input_->is_dense)
{
PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] compute_fast can only be used with organized point clouds!\n");
return;
}
CloudConstPtr cloud_;
CloudPtr cloud_filtered_ (new Cloud ());
CloudPtr cloud_downsampled_ (new Cloud ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
CloudPtr table_projected_ (new Cloud ());
CloudPtr table_hull_ (new Cloud ());
CloudPtr cloud_objects_ (new Cloud ());
CloudPtr cluster_object_ (new Cloud ());
typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
clusters_tree_->setEpsilon (1);
// Normal estimation parameters
n3d_.setKSearch (k_);
n3d_.setSearchMethod (normals_tree_);
// Table model fitting parameters
seg_.setDistanceThreshold (sac_distance_threshold_);
seg_.setMaxIterations (2000);
seg_.setNormalDistanceWeight (0.1);
seg_.setOptimizeCoefficients (true);
seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg_.setMethodType (pcl::SAC_RANSAC);
seg_.setProbability (0.99);
proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
prism_.setHeightLimits (object_min_height_, object_max_height_);
// Clustering parameters
cluster_.setClusterTolerance (object_cluster_tolerance_);
cluster_.setMinClusterSize (object_cluster_min_size_);
cluster_.setSearchMethod (clusters_tree_);
// ---[ PassThroughFilter
pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
pass_.setFilterFieldName ("z");
pass_.setInputCloud (input_);
pass_.filter (*cloud_filtered_);
if (int (cloud_filtered_->points.size ()) < k_)
{
PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.",
cloud_filtered_->points.size ());
return;
}
// Downsample the point cloud
grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
grid_.setDownsampleAllData (false);
grid_.setInputCloud (cloud_filtered_);
grid_.filter (*cloud_downsampled_);
// ---[ Estimate the point normals
n3d_.setInputCloud (cloud_downsampled_);
n3d_.compute (*cloud_normals_);
// ---[ Perform segmentation
seg_.setInputCloud (cloud_downsampled_);
seg_.setInputNormals (cloud_normals_);
seg_.segment (*table_inliers_, *table_coefficients_);
if (table_inliers_->indices.size () == 0)
{
PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
proj_.setInputCloud (cloud_downsampled_);
proj_.setIndices (table_inliers_);
proj_.setModelCoefficients (table_coefficients_);
proj_.filter (*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
CloudPtr table_hull (new Cloud ());
hull_.setInputCloud (table_projected_);
hull_.reconstruct (*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
model_coefficients[0] = table_coefficients_->values[0];
model_coefficients[1] = table_coefficients_->values[1];
model_coefficients[2] = table_coefficients_->values[2];
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
Eigen::Vector4f vp (0, 0, 0, 0);
// See if we need to flip any plane normals
vp -= table_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 (table_hull->points[0].getVector4fMap ()));
}
//Set table_coeffs
table_coeffs_ = model_coefficients;
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices;
prism_.setInputCloud (input_);
prism_.setInputPlanarHull (table_hull);
prism_.segment (cloud_object_indices);
pcl::ExtractIndices<PointType> extract_object_indices;
extract_object_indices.setInputCloud (input_);
extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
extract_object_indices.filter (*cloud_objects_);
//create new binary pointcloud with intensity values (0 and 1), 0 for non-object pixels and 1 otherwise
pcl::PointCloud<pcl::PointXYZI>::Ptr binary_cloud (new pcl::PointCloud<pcl::PointXYZI>);
{
binary_cloud->width = input_->width;
binary_cloud->height = input_->height;
binary_cloud->points.resize (input_->points.size ());
binary_cloud->is_dense = input_->is_dense;
size_t idx;
for (size_t i = 0; i < cloud_object_indices.indices.size (); ++i)
{
idx = cloud_object_indices.indices[i];
binary_cloud->points[idx].getVector4fMap () = input_->points[idx].getVector4fMap ();
binary_cloud->points[idx].intensity = 1.0;
}
}
//connected components on the binary image
std::map<float, float> connected_labels;
float c_intensity = 0.1f;
float intensity_incr = 0.1f;
{
int wsize = wsize_;
for (int i = 0; i < int (binary_cloud->width); i++)
{
for (int j = 0; j < int (binary_cloud->height); j++)
{
if (binary_cloud->at (i, j).intensity != 0)
{
//check neighboring pixels, first left and then top
//be aware of margins
if ((i - 1) < 0 && (j - 1) < 0)
{
//top-left pixel
(*binary_cloud) (i, j).intensity = c_intensity;
}
else
{
if ((j - 1) < 0)
{
//top-row, check on the left of pixel to assign a new label or not
int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (left)
{
//Nothing found on the left, check bigger window
bool found = false;
for (int kk = 2; kk < wsize && !found; kk++)
{
if ((i - kk) < 0)
continue;
int left = check ((*binary_cloud) (i - kk, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (left == 0)
{
found = true;
}
}
if (!found)
{
c_intensity += intensity_incr;
(*binary_cloud) (i, j).intensity = c_intensity;
}
}
}
else
{
if ((i - 1) == 0)
{
//check only top
int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (top)
{
bool found = false;
for (int kk = 2; kk < wsize && !found; kk++)
{
if ((j - kk) < 0)
continue;
int top = check ((*binary_cloud) (i, j - kk), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (top == 0)
{
found = true;
}
}
if (!found)
{
c_intensity += intensity_incr;
(*binary_cloud) (i, j).intensity = c_intensity;
}
}
}
else
{
//check left and top
int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (left == 0 && top == 0)
{
//both top and left had labels, check if they are different
//if they are, take the smallest one and mark labels to be connected..
if ((*binary_cloud) (i - 1, j).intensity != (*binary_cloud) (i, j - 1).intensity)
{
float smaller_intensity = (*binary_cloud) (i - 1, j).intensity;
float bigger_intensity = (*binary_cloud) (i, j - 1).intensity;
if ((*binary_cloud) (i - 1, j).intensity > (*binary_cloud) (i, j - 1).intensity)
{
smaller_intensity = (*binary_cloud) (i, j - 1).intensity;
bigger_intensity = (*binary_cloud) (i - 1, j).intensity;
}
connected_labels[bigger_intensity] = smaller_intensity;
(*binary_cloud) (i, j).intensity = smaller_intensity;
}
}
if (left == 1 && top == 1)
{
//if none had labels, increment c_intensity
//search first on bigger window
bool found = false;
for (int dist = 2; dist < wsize && !found; dist++)
{
if (((i - dist) < 0) || ((j - dist) < 0))
continue;
int left = check ((*binary_cloud) (i - dist, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
int top = check ((*binary_cloud) (i, j - dist), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
if (left == 0 && top == 0)
{
if ((*binary_cloud) (i - dist, j).intensity != (*binary_cloud) (i, j - dist).intensity)
{
float smaller_intensity = (*binary_cloud) (i - dist, j).intensity;
float bigger_intensity = (*binary_cloud) (i, j - dist).intensity;
if ((*binary_cloud) (i - dist, j).intensity > (*binary_cloud) (i, j - dist).intensity)
{
smaller_intensity = (*binary_cloud) (i, j - dist).intensity;
bigger_intensity = (*binary_cloud) (i - dist, j).intensity;
}
connected_labels[bigger_intensity] = smaller_intensity;
(*binary_cloud) (i, j).intensity = smaller_intensity;
found = true;
}
}
else if (left == 0 || top == 0)
{
//one had label
found = true;
}
}
if (!found)
{
//none had label in the bigger window
c_intensity += intensity_incr;
(*binary_cloud) (i, j).intensity = c_intensity;
}
}
}
}
}
}
}
}
}
std::map<float, pcl::PointIndices> clusters_map;
{
std::map<float, float>::iterator it;
for (int i = 0; i < int (binary_cloud->width); i++)
{
for (int j = 0; j < int (binary_cloud->height); j++)
{
if (binary_cloud->at (i, j).intensity != 0)
{
//check if this is a root label...
it = connected_labels.find (binary_cloud->at (i, j).intensity);
while (it != connected_labels.end ())
{
//the label is on the list, change pixel intensity until it has a root label
(*binary_cloud) (i, j).intensity = (*it).second;
it = connected_labels.find (binary_cloud->at (i, j).intensity);
}
std::map<float, pcl::PointIndices>::iterator it_indices;
it_indices = clusters_map.find (binary_cloud->at (i, j).intensity);
if (it_indices == clusters_map.end ())
{
pcl::PointIndices indices;
clusters_map[binary_cloud->at (i, j).intensity] = indices;
}
clusters_map[binary_cloud->at (i, j).intensity].indices.push_back (static_cast<int> (j * binary_cloud->width + i));
}
}
}
}
clusters.resize (clusters_map.size ());
std::map<float, pcl::PointIndices>::iterator it_indices;
int k = 0;
for (it_indices = clusters_map.begin (); it_indices != clusters_map.end (); it_indices++)
{
if (int ((*it_indices).second.indices.size ()) >= object_cluster_min_size_)
{
clusters[k] = CloudPtr (new Cloud ());
pcl::copyPointCloud (*input_, (*it_indices).second.indices, *clusters[k]);
k++;
indices_clusters_.push_back((*it_indices).second);
}
}
clusters.resize (k);
}
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// ... do data processing
sensor_msgs::PointCloud2 output;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*input, *cloud);
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.10); // 2cm
ec.setMinClusterSize (20);
ec.setMaxClusterSize (2500);
ec.setSearchMethod (tree);
ec.setInputCloud(cloud);
ec.extract (cluster_indices);
ROS_INFO("cluster_indices has %d data points.", (int) cluster_indices.size());
ROS_INFO("cloud has %d data points.", (int) cloud->points.size());
visualization_msgs::Marker marker;
marker.header = cloud->header;
marker.id = 1;
marker.type = visualization_msgs::Marker::CUBE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.color.r = 1;
marker.color.g = 0;
marker.color.b = 0;
marker.color.a = 0.7;
marker.scale.x = 0.2;
marker.scale.y = 0.2;
marker.scale.z = 0.2;
marker.lifetime = ros::Duration(60.0);
Eigen::Vector4f minPoint;
Eigen::Vector4f maxPoint;
// pcl::getMinMax3D(*cloud, minPoint, maxPoint);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud->points[*pit]);
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
// Merge current clusters to whole point cloud
*clustered_cloud += *cloud_cluster;
// for(size_t j = 0; j < cloud_cluster->points.size() - 1; j++)
// {
/*
geometry_msgs::Point pt1;
pt1.x = cloud_cluster->points[j].x;
pt1.y = cloud_cluster->points[j].y;
pt1.z = cloud_cluster->points[j].z;
geometry_msgs::Point pt2;
pt2.x = cloud_cluster->points[j+1].x;
pt2.y = cloud_cluster->points[j+1].y;
pt2.z = cloud_cluster->points[j+1].z;
marker.points.push_back(pt1);
marker.points.push_back(pt2);
*/
//Seg for top of prism
geometry_msgs::Point pt3;
pt3.x = 0.0;
pt3.y = 0.0;
pt3.z = 0.0;
std_msgs::ColorRGBA colors;
colors.r = 0.0;
colors.g = 0.0;
colors.b = 0.0;
for(size_t i=0; i<cloud_cluster->points.size(); i++)
{
pt3.x += cloud_cluster->points[i].x;
pt3.y += cloud_cluster->points[i].y;
pt3.z += cloud_cluster->points[i].z;
}
pt3.x /= cloud_cluster->points.size();
pt3.y /= cloud_cluster->points.size();
pt3.z /= cloud_cluster->points.size();
pcl::getMinMax3D(*cloud_cluster, minPoint, maxPoint);
marker.scale.y= maxPoint.y();
//marker.scale.x= maxPoint.x();
//marker.scale.z= maxPoint.z();
marker.scale.x= maxPoint.x()-minPoint.x();
colors.r = marker.scale.x;
// colors.g = marker.scale.y;
//marker.scale.z= maxPoint.z()-minPoint.z();
//pt3.z = maxPoint.z();
//geometry_msgs::Point pt4;
//pt4.x = cloud_cluster->points[j+1].x;
//pt4.y = cloud_cluster->points[j+1].y;
//pt4.z = cloud_cluster->points[j+1].z;
//pt4.z = maxPoint.z();
//marker.pose.position.x = pt3.x;
//marker.pose.position.y = pt3.y;
//marker.pose.position.z = pt3.z;
//marker.colors.push_back(colors);
marker.points.push_back(pt3);
//marker.points.push_back(pt4);
//Seg for bottom vertices to top vertices
// marker.points.push_back(pt1);
//marker.points.push_back(pt3);
// }
object_pub.publish(marker);
}
// Publish the data
pcl::toROSMsg(*clustered_cloud, output);
output.header = cloud->header;
// output.header.frame_id="/camera_link";
// output.header.stamp = ros::Time::now();
pub.publish (output);
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// ---[ Step 1a : compute the centroid in XYZ space
Eigen::Vector4f xyz_centroid;
if (use_given_centroid_)
xyz_centroid = centroid_to_use_;
else
compute3DCentroid (*surface_, *indices_, xyz_centroid); // Estimate the XYZ centroid
// ---[ Step 1b : compute the centroid in normal space
Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
int cp = 0;
// If the data is dense, we don't need to check for NaN
if (use_given_normal_)
normal_centroid = normal_to_use_;
else
{
if (normals_->is_dense)
{
for (size_t i = 0; i < indices_->size (); ++i)
{
normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
cp++;
}
}
// NaN or Inf values could exist => check for them
else
{
for (size_t i = 0; i < indices_->size (); ++i)
{
if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
||
!pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
||
!pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
continue;
normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
cp++;
}
}
normal_centroid /= cp;
}
// Compute the direction of view from the viewpoint to the centroid
Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
d_vp_p.normalize ();
// Estimate the SPFH at nn_indices[0] using the entire cloud
computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
// We only output _1_ signature
output.points.resize (1);
output.width = 1;
output.height = 1;
// Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature
for (int d = 0; d < hist_f1_.size (); ++d)
output.points[0].histogram[d + 0] = hist_f1_[d];
size_t data_size = hist_f1_.size ();
for (int d = 0; d < hist_f2_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f2_[d];
data_size += hist_f2_.size ();
for (int d = 0; d < hist_f3_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f3_[d];
data_size += hist_f3_.size ();
for (int d = 0; d < hist_f4_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f4_[d];
// ---[ Step 2 : obtain the viewpoint component
hist_vp_.setZero (nr_bins_vp_);
double hist_incr;
if (normalize_bins_)
hist_incr = 100.0 / (double)(indices_->size ());
else
hist_incr = 1.0;
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
normals_->points[(*indices_)[i]].normal[1],
normals_->points[(*indices_)[i]].normal[2], 0);
// Normalize
double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
int fi = floor (alpha * hist_vp_.size ());
if (fi < 0)
fi = 0;
if (fi > ((int)hist_vp_.size () - 1))
fi = hist_vp_.size () - 1;
// Bin into the histogram
hist_vp_ [fi] += hist_incr;
}
data_size += hist_f4_.size ();
// Copy the resultant signature
for (int d = 0; d < hist_vp_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_vp_[d];
}
int main(int argc, char * argv[])
{
using namespace Eigen;
using namespace igl;
using namespace std;
// init mesh
string filename = "../shared/beast.obj";
if(argc < 2)
{
cerr<<"Usage:"<<endl<<" ./example input.obj"<<endl;
cout<<endl<<"Opening default mesh..."<<endl;
}else
{
// Read and prepare mesh
filename = argv[1];
}
// dirname, basename, extension and filename
string d,b,ext,f;
pathinfo(filename,d,b,ext,f);
// Convert extension to lower case
transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
vector<vector<double > > vV,vN,vTC;
vector<vector<int > > vF,vFTC,vFN;
if(ext == "obj")
{
// Convert extension to lower case
if(!igl::readOBJ(filename,vV,vTC,vN,vF,vFTC,vFN))
{
return 1;
}
}else if(ext == "off")
{
// Convert extension to lower case
if(!igl::readOFF(filename,vV,vF,vN))
{
return 1;
}
}else if(ext == "wrl")
{
// Convert extension to lower case
if(!igl::readWRL(filename,vV,vF))
{
return 1;
}
//}else
//{
// // Convert extension to lower case
// MatrixXi T;
// if(!igl::readMESH(filename,V,T,F))
// {
// return 1;
// }
// //if(F.size() > T.size() || F.size() == 0)
// {
// boundary_faces(T,F);
// }
}
if(vV.size() > 0)
{
if(!list_to_matrix(vV,V))
{
return 1;
}
triangulate(vF,F);
}
// Compute normals, centroid, colors, bounding box diagonal
per_vertex_normals(V,F,N);
mid = 0.5*(V.colwise().maxCoeff() + V.colwise().minCoeff());
bbd = (V.colwise().maxCoeff() - V.colwise().minCoeff()).maxCoeff();
// Init embree
ei.init(V.cast<float>(),F.cast<int>());
// Init glut
glutInit(&argc,argv);
if( !TwInit(TW_OPENGL, NULL) )
{
// A fatal error occured
fprintf(stderr, "AntTweakBar initialization failed: %s\n", TwGetLastError());
return 1;
}
// Create a tweak bar
rebar.TwNewBar("TweakBar");
rebar.TwAddVarRW("scene_rot", TW_TYPE_QUAT4F, &scene_rot, "");
rebar.TwAddVarRW("lights_on", TW_TYPE_BOOLCPP, &lights_on, "key=l");
rebar.TwAddVarRW("color", TW_TYPE_COLOR4F, color.data(), "colormode=hls");
rebar.TwAddVarRW("ao_factor", TW_TYPE_DOUBLE, &ao_factor, "min=0 max=1 step=0.2 keyIncr=] keyDecr=[ ");
rebar.TwAddVarRW("ao_normalize", TW_TYPE_BOOLCPP, &ao_normalize, "key=n");
rebar.TwAddVarRW("ao_on", TW_TYPE_BOOLCPP, &ao_on, "key=a");
rebar.TwAddVarRW("light_intensity", TW_TYPE_DOUBLE, &light_intensity, "min=0 max=0.4 step=0.1 keyIncr=} keyDecr={ ");
rebar.load(REBAR_NAME);
glutInitDisplayString( "rgba depth double samples>=8 ");
glutInitWindowSize(glutGet(GLUT_SCREEN_WIDTH)/2.0,glutGet(GLUT_SCREEN_HEIGHT));
glutCreateWindow("ambient-occlusion");
glutDisplayFunc(display);
glutReshapeFunc(reshape);
glutKeyboardFunc(key);
glutMouseFunc(mouse);
glutMotionFunc(mouse_drag);
glutPassiveMotionFunc((GLUTmousemotionfun)TwEventMouseMotionGLUT);
glutMainLoop();
return 0;
}
template<typename PointT, typename PointNT, typename PointLT> void
pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients,
std::vector<PointIndices>& inlier_indices,
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
pcl::PointCloud<PointLT>& labels,
std::vector<pcl::PointIndices>& label_indices)
{
if (!initCompute ())
return;
// Check that we got the same number of points and normals
if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ()))
{
PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%zu) and normal cloud (%zu) do not match!\n",
getClassName ().c_str (), input_->points.size (),
normals_->points.size ());
return;
}
// Check that the cloud is organized
if (!input_->isOrganized ())
{
PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
getClassName ().c_str ());
return;
}
// Calculate range part of planes' hessian normal form
std::vector<float> plane_d (input_->points.size ());
for (unsigned int i = 0; i < input_->size (); ++i)
plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ());
// Make a comparator
//PlaneCoefficientComparator<PointT,PointNT> plane_comparator (plane_d);
compare_->setPlaneCoeffD (plane_d);
compare_->setInputCloud (input_);
compare_->setInputNormals (normals_);
compare_->setAngularThreshold (static_cast<float> (angular_threshold_));
compare_->setDistanceThreshold (static_cast<float> (distance_threshold_), true);
// Set up the output
OrganizedConnectedComponentSegmentation<PointT,pcl::Label> connected_component (compare_);
connected_component.setInputCloud (input_);
connected_component.segment (labels, label_indices);
Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
Eigen::Matrix3f clust_cov;
pcl::ModelCoefficients model;
model.values.resize (4);
// Fit Planes to each cluster
for (size_t i = 0; i < label_indices.size (); i++)
{
if (static_cast<unsigned> (label_indices[i].indices.size ()) > min_inliers_)
{
pcl::computeMeanAndCovarianceMatrix (*input_, label_indices[i].indices, clust_cov, clust_centroid);
Eigen::Vector4f plane_params;
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
pcl::eigen33 (clust_cov, eigen_value, eigen_vector);
plane_params[0] = eigen_vector[0];
plane_params[1] = eigen_vector[1];
plane_params[2] = eigen_vector[2];
plane_params[3] = 0;
plane_params[3] = -1 * plane_params.dot (clust_centroid);
vp -= clust_centroid;
float cos_theta = vp.dot (plane_params);
if (cos_theta < 0)
{
plane_params *= -1;
plane_params[3] = 0;
plane_params[3] = -1 * plane_params.dot (clust_centroid);
}
// Compute the curvature surface change
float curvature;
float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
if (eig_sum != 0)
curvature = fabsf (eigen_value / eig_sum);
else
curvature = 0;
if (curvature < maximum_curvature_)
{
model.values[0] = plane_params[0];
model.values[1] = plane_params[1];
model.values[2] = plane_params[2];
model.values[3] = plane_params[3];
model_coefficients.push_back (model);
inlier_indices.push_back (label_indices[i]);
centroids.push_back (clust_centroid);
covariances.push_back (clust_cov);
}
}
}
deinitCompute ();
}
bool checkCloud(const sensor_msgs::PointCloud2& cloud_msg,
typename pcl::PointCloud<T>::Ptr hand_cloud,
//typename pcl::PointCloud<T>::Ptr finger_cloud,
const std::string& frame_id,
tf::Vector3& hand_position,
tf::Vector3& arm_position,
tf::Vector3& arm_direction)
{
typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>);
pcl::fromROSMsg(cloud_msg, *cloud);
if((cloud->points.size() < g_config.min_cluster_size) ||
(cloud->points.size() > g_config.max_cluster_size))
return false;
pcl::PCA<T> pca;
pca.setInputCloud(cloud);
Eigen::Vector4f mean = pca.getMean();
if((mean.coeff(0) < g_config.min_x) || (mean.coeff(0) > g_config.max_x)) return false;
if((mean.coeff(1) < g_config.min_y) || (mean.coeff(1) > g_config.max_y)) return false;
if((mean.coeff(2) < g_config.min_z) || (mean.coeff(2) > g_config.max_z)) return false;
Eigen::Vector3f eigen_value = pca.getEigenValues();
double ratio = eigen_value.coeff(0) / eigen_value.coeff(1);
if((ratio < g_config.min_eigen_value_ratio) || (ratio > g_config.max_eigen_value_ratio)) return false;
T search_point;
Eigen::Matrix3f ev = pca.getEigenVectors();
Eigen::Vector3f main_axis(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0));
main_axis.normalize();
arm_direction.setX(main_axis.coeff(0));
arm_direction.setY(main_axis.coeff(1));
arm_direction.setZ(main_axis.coeff(2));
arm_position.setX(mean.coeff(0));
arm_position.setY(mean.coeff(1));
arm_position.setZ(mean.coeff(2));
main_axis = (-main_axis * 0.3) + Eigen::Vector3f(mean.coeff(0), mean.coeff(1), mean.coeff(2));
search_point.x = main_axis.coeff(0);
search_point.y = main_axis.coeff(1);
search_point.z = main_axis.coeff(2);
//find hand
pcl::KdTreeFLANN<T> kdtree;
kdtree.setInputCloud(cloud);
//find the closet point from the serach_point
std::vector<int> point_indeices(1);
std::vector<float> point_distances(1);
if ( kdtree.nearestKSearch (search_point, 1, point_indeices, point_distances) > 0 )
{
//update search point
search_point = cloud->points[point_indeices[0]];
//show seach point
if(g_marker_array_pub.getNumSubscribers() != 0)
{
pushSimpleMarker(search_point.x, search_point.y, search_point.z,
1.0, 0, 0,
0.02,
g_marker_id, g_marker_array, frame_id);
}
//hand
point_indeices.clear();
point_distances.clear();
kdtree.radiusSearch(search_point, g_config.hand_lenght, point_indeices, point_distances);
for (size_t i = 0; i < point_indeices.size (); ++i)
{
hand_cloud->points.push_back(cloud->points[point_indeices[i]]);
hand_cloud->points.back().r = 255;
hand_cloud->points.back().g = 0;
hand_cloud->points.back().b = 0;
}
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*hand_cloud, centroid);
if(g_marker_array_pub.getNumSubscribers() != 0)
{
pushSimpleMarker(centroid.coeff(0), centroid.coeff(1), centroid.coeff(2),
0.0, 1.0, 0,
0.02,
g_marker_id, g_marker_array, frame_id);
}
hand_position.setX(centroid.coeff(0));
hand_position.setY(centroid.coeff(1));
hand_position.setZ(centroid.coeff(2));
#if 0
//fingers
search_point.x = centroid.coeff(0);
search_point.y = centroid.coeff(1);
search_point.z = centroid.coeff(2);
std::vector<int> point_indeices_inner;
std::vector<float> point_distances_inner;
kdtree.radiusSearch(search_point, 0.07, point_indeices_inner, point_distances_inner);
std::vector<int> point_indeices_outter;
std::vector<float> point_distances_outter;
kdtree.radiusSearch(search_point, 0.1, point_indeices_outter, point_distances_outter);
//ROS_INFO("before %d %d", point_indeices_inner.size(), point_indeices_outter.size());
std::vector<int>::iterator it;
for(size_t i = 0; i < point_indeices_inner.size(); i++)
{
it = std::find(point_indeices_outter.begin(), point_indeices_outter.end(), point_indeices_inner[i]);
if(it != point_indeices_outter.end())
{
point_indeices_outter.erase(it);
}
}
//ROS_INFO("after %d %d", point_indeices_inner.size(), point_indeices_outter.size());
//ROS_DEBUG_THROTTLE(1.0, "found %lu", point_indeices.size ());
for (size_t i = 0; i < point_indeices_outter.size (); ++i)
{
finger_cloud->points.push_back(cloud->points[point_indeices_outter[i]]);
finger_cloud->points.back().r = 255;
finger_cloud->points.back().g = 0;
finger_cloud->points.back().b = 0;
}
#endif
}
else
{
return false;
}
if(g_marker_array_pub.getNumSubscribers() != 0)
{
pushEigenMarker<T>(pca, g_marker_id, g_marker_array, 0.1, frame_id);
}
return true;
}
int LoadPCD::compute()
{
//for each selected filename
for (int k = 0; k < m_filenames.size(); ++k)
{
Eigen::Vector4f origin;
Eigen::Quaternionf orientation;
QString filename = m_filenames[k];
boost::shared_ptr<PCLCloud> cloud_ptr_in = loadSensorMessage(filename, origin, orientation);
if (!cloud_ptr_in) //loading failed?
return 0;
PCLCloud::Ptr cloud_ptr;
if (!cloud_ptr_in->is_dense) //data may contain nans. Remove them
{
//now we need to remove nans
pcl::PassThrough<PCLCloud> passFilter;
passFilter.setInputCloud(cloud_ptr_in);
cloud_ptr = PCLCloud::Ptr(new PCLCloud);
passFilter.filter(*cloud_ptr);
}
else
{
cloud_ptr = cloud_ptr_in;
}
//now we construct a ccGBLSensor with these characteristics
ccGBLSensor * sensor = new ccGBLSensor;
// get orientation as rot matrix
Eigen::Matrix3f eigrot = orientation.toRotationMatrix();
// and copy it into a ccGLMatrix
ccGLMatrix ccRot;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
ccRot.getColumn(j)[i] = eigrot(i,j);
}
}
ccRot.getColumn(3)[3] = 1.0;
// now in a format good for CloudComapre
//ccGLMatrix ccRot = ccGLMatrix::FromQuaternion(orientation.coeffs().data());
//ccRot = ccRot.transposed();
ccRot.setTranslation(origin.data());
sensor->setRigidTransformation(ccRot);
sensor->setDeltaPhi(static_cast<PointCoordinateType>(0.05));
sensor->setDeltaTheta(static_cast<PointCoordinateType>(0.05));
sensor->setVisible(true);
//uncertainty to some default
sensor->setUncertainty(static_cast<PointCoordinateType>(0.01));
ccPointCloud* out_cloud = sm2ccConverter(cloud_ptr).getCloud();
if (!out_cloud)
return -31;
sensor->setGraphicScale(out_cloud->getBB().getDiagNorm() / 10);
//do the projection on sensor
ccGenericPointCloud* cloud = ccHObjectCaster::ToGenericPointCloud(out_cloud);
int errorCode;
CCLib::SimpleCloud* projectedCloud = sensor->project(cloud,errorCode,true);
if (projectedCloud)
{
//DGM: we don't use it but we still have to delete it!
delete projectedCloud;
projectedCloud = 0;
}
QString cloud_name = QFileInfo(filename).baseName();
out_cloud->setName(cloud_name);
QFileInfo fi(filename);
QString containerName = QString("%1 (%2)").arg(fi.fileName()).arg(fi.absolutePath());
ccHObject* cloudContainer = new ccHObject(containerName);
out_cloud->addChild(sensor);
cloudContainer->addChild(out_cloud);
emit newEntity(cloudContainer);
}
return 1;
}
void VertexProcessor::parameter(vp::Parameter param, const Eigen::Vector4f& v)
{
parameter(param, v.x(), v.y(), v.z(), v.w());
}
void
pcl::apps::DominantPlaneSegmentation<PointType>::compute_full (std::vector<CloudPtr> & clusters)
{
// Has the input dataset been set already?
if (!input_)
{
PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
CloudConstPtr cloud_;
CloudPtr cloud_filtered_ (new Cloud ());
CloudPtr cloud_downsampled_ (new Cloud ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
CloudPtr table_projected_ (new Cloud ());
CloudPtr table_hull_ (new Cloud ());
CloudPtr cloud_objects_ (new Cloud ());
CloudPtr cluster_object_ (new Cloud ());
typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
clusters_tree_->setEpsilon (1);
// Normal estimation parameters
n3d_.setKSearch (10);
n3d_.setSearchMethod (normals_tree_);
// Table model fitting parameters
seg_.setDistanceThreshold (sac_distance_threshold_);
seg_.setMaxIterations (2000);
seg_.setNormalDistanceWeight (0.1);
seg_.setOptimizeCoefficients (true);
seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg_.setMethodType (pcl::SAC_MSAC);
seg_.setProbability (0.98);
proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
prism_.setHeightLimits (object_min_height_, object_max_height_);
// Clustering parameters
cluster_.setClusterTolerance (object_cluster_tolerance_);
cluster_.setMinClusterSize (object_cluster_min_size_);
cluster_.setSearchMethod (clusters_tree_);
// ---[ PassThroughFilter
pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
pass_.setFilterFieldName ("z");
pass_.setInputCloud (input_);
pass_.filter (*cloud_filtered_);
if (int (cloud_filtered_->points.size ()) < k_)
{
PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.",
cloud_filtered_->points.size ());
return;
}
// Downsample the point cloud
grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
grid_.setDownsampleAllData (false);
grid_.setInputCloud (cloud_filtered_);
grid_.filter (*cloud_downsampled_);
PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering&downsampling (%f -> %f): %zu out of %zu\n",
min_z_bounds_, max_z_bounds_, cloud_downsampled_->points.size (), input_->points.size ());
// ---[ Estimate the point normals
n3d_.setInputCloud (cloud_downsampled_);
n3d_.compute (*cloud_normals_);
PCL_INFO ("[DominantPlaneSegmentation] %zu normals estimated. \n", cloud_normals_->points.size ());
// ---[ Perform segmentation
seg_.setInputCloud (cloud_downsampled_);
seg_.setInputNormals (cloud_normals_);
seg_.segment (*table_inliers_, *table_coefficients_);
if (table_inliers_->indices.size () == 0)
{
PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
proj_.setInputCloud (cloud_downsampled_);
proj_.setIndices (table_inliers_);
proj_.setModelCoefficients (table_coefficients_);
proj_.filter (*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
CloudPtr table_hull (new Cloud ());
hull_.setInputCloud (table_projected_);
hull_.reconstruct (*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
model_coefficients[0] = table_coefficients_->values[0];
model_coefficients[1] = table_coefficients_->values[1];
model_coefficients[2] = table_coefficients_->values[2];
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
Eigen::Vector4f vp (0, 0, 0, 0);
// See if we need to flip any plane normals
vp -= table_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 (table_hull->points[0].getVector4fMap ()));
}
//Set table_coeffs
table_coeffs_ = model_coefficients;
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices;
prism_.setInputCloud (cloud_filtered_);
prism_.setInputPlanarHull (table_hull);
prism_.segment (cloud_object_indices);
pcl::ExtractIndices<PointType> extract_object_indices;
extract_object_indices.setInputCloud (cloud_downsampled_);
extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
extract_object_indices.filter (*cloud_objects_);
if (cloud_objects_->points.size () == 0)
return;
// ---[ Split the objects into Euclidean clusters
std::vector<pcl::PointIndices> clusters2;
cluster_.setInputCloud (cloud_filtered_);
cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
cluster_.extract (clusters2);
PCL_INFO ("[DominantPlaneSegmentation::compute_full()] Number of clusters found matching the given constraints: %zu.\n",
clusters2.size ());
clusters.resize (clusters2.size ());
for (size_t i = 0; i < clusters2.size (); ++i)
{
clusters[i] = CloudPtr (new Cloud ());
pcl::copyPointCloud (*cloud_filtered_, clusters2[i].indices, *clusters[i]);
}
}
template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid model coefficients
if (!isModelValid (model_coefficients))
return;
// Obtain the line point and direction
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
Eigen::Vector4f pp = line_pt + k * line_dir;
// Calculate the projection of the point on the line (pointProj = A + k * B)
projected_points.points[inliers[i]].x = pp[0];
projected_points.points[inliers[i]].y = pp[1];
projected_points.points[inliers[i]].z = pp[2];
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
Eigen::Vector4f pp = line_pt + k * line_dir;
// Calculate the projection of the point on the line (pointProj = A + k * B)
projected_points.points[i].x = pp[0];
projected_points.points[i].y = pp[1];
projected_points.points[i].z = pp[2];
}
}
}
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
// normalize the vector perpendicular to the plane...
mc.normalize ();
// ... and store the resulting normal as a local copy of the model coefficients
Eigen::Vector4f tmp_mc = model_coefficients;
tmp_mc[0] = mc[0];
tmp_mc[1] = mc[1];
tmp_mc[2] = mc[2];
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < input_->points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// Calculate the distance from the point to the plane
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
// use normalized coefficients to calculate the scalar projection
float distance_to_plane = tmp_mc.dot (p);
pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// Calculate the distance from the point to the plane
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
// use normalized coefficients to calculate the scalar projection
float distance_to_plane = tmp_mc.dot (p);
pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::interpolateSingleChannel (
const std::vector<int> &indices,
const std::vector<float> &sqr_dists,
const int index,
std::vector<double> &binDistance,
const int nr_bins,
Eigen::VectorXf &shot)
{
const Eigen::Vector4f& central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
const PointRFT& current_frame = (*frames_)[index];
Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
if (!pcl_isfinite(binDistance[i_idx]))
continue;
Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
delta[3] = 0;
// Compute the Euclidean norm
double distance = sqrt (sqr_dists[i_idx]);
if (areEquals (distance, 0.0))
continue;
double xInFeatRef = delta.dot (current_frame_x);
double yInFeatRef = delta.dot (current_frame_y);
double zInFeatRef = delta.dot (current_frame_z);
// To avoid numerical problems afterwards
if (fabs (yInFeatRef) < 1E-30)
yInFeatRef = 0;
if (fabs (xInFeatRef) < 1E-30)
xInFeatRef = 0;
if (fabs (zInFeatRef) < 1E-30)
zInFeatRef = 0;
unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
unsigned char bit3 = static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
assert (bit3 == 0 || bit3 == 1);
int desc_index = (bit4<<3) + (bit3<<2);
desc_index = desc_index << 1;
if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
else
desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0;
desc_index += zInFeatRef > 0 ? 1 : 0;
// 2 RADII
desc_index += (distance > radius1_2_) ? 2 : 0;
int step_index = static_cast<int>(floor (binDistance[i_idx] +0.5));
int volume_index = desc_index * (nr_bins+1);
//Interpolation on the cosine (adjacent bins in the histogram)
binDistance[i_idx] -= step_index;
double intWeight = (1- fabs (binDistance[i_idx]));
if (binDistance[i_idx] > 0)
shot[volume_index + ((step_index+1) % nr_bins)] += static_cast<float> (binDistance[i_idx]);
else
shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += - static_cast<float> (binDistance[i_idx]);
//Interpolation on the distance (adjacent husks)
if (distance > radius1_2_) //external sphere
{
double radiusDistance = (distance - radius3_4_) / radius1_2_;
if (distance > radius3_4_) //most external sector, votes only for itself
intWeight += 1 - radiusDistance; //peso=1-d
else //3/4 of radius, votes also for the internal sphere
{
intWeight += 1 + radiusDistance;
shot[(desc_index - 2) * (nr_bins+1) + step_index] -= static_cast<float> (radiusDistance);
}
}
else //internal sphere
{
double radiusDistance = (distance - radius1_4_) / radius1_2_;
if (distance < radius1_4_) //most internal sector, votes only for itself
intWeight += 1 + radiusDistance; //weight=1-d
else //3/4 of radius, votes also for the external sphere
{
intWeight += 1 - radiusDistance;
shot[(desc_index + 2) * (nr_bins+1) + step_index] += static_cast<float> (radiusDistance);
}
}
//Interpolation on the inclination (adjacent vertical volumes)
double inclinationCos = zInFeatRef / distance;
if (inclinationCos < - 1.0)
inclinationCos = - 1.0;
if (inclinationCos > 1.0)
inclinationCos = 1.0;
double inclination = acos (inclinationCos);
assert (inclination >= 0.0 && inclination <= PST_RAD_180);
if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
{
double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
if (inclination > PST_RAD_135)
intWeight += 1 - inclinationDistance;
else
{
intWeight += 1 + inclinationDistance;
assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_);
shot[(desc_index + 1) * (nr_bins+1) + step_index] -= static_cast<float> (inclinationDistance);
}
}
else
{
double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
if (inclination < PST_RAD_45)
intWeight += 1 + inclinationDistance;
else
{
intWeight += 1 - inclinationDistance;
assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_);
shot[(desc_index - 1) * (nr_bins+1) + step_index] += static_cast<float> (inclinationDistance);
}
}
if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
{
//Interpolation on the azimuth (adjacent horizontal volumes)
double azimuth = atan2 (yInFeatRef, xInFeatRef);
int sel = desc_index >> 2;
double angularSectorSpan = PST_RAD_45;
double angularSectorStart = - PST_RAD_PI_7_8;
double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
if (azimuthDistance > 0)
{
intWeight += 1 - azimuthDistance;
int interp_index = (desc_index + 4) % maxAngularSectors_;
assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
shot[interp_index * (nr_bins+1) + step_index] += static_cast<float> (azimuthDistance);
}
else
{
int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
intWeight += 1 + azimuthDistance;
shot[interp_index * (nr_bins+1) + step_index] -= static_cast<float> (azimuthDistance);
}
}
assert (volume_index + step_index >= 0 && volume_index + step_index < descLength_);
shot[volume_index + step_index] += static_cast<float> (intWeight);
}
int
main (int argc, char **argv)
{
double dist = 2.5;
pcl::console::parse_argument (argc, argv, "-d", dist);
int iter = 10;
pcl::console::parse_argument (argc, argv, "-i", iter);
int lumIter = 1;
pcl::console::parse_argument (argc, argv, "-l", lumIter);
double loopDist = 5.0;
pcl::console::parse_argument (argc, argv, "-D", loopDist);
int loopCount = 20;
pcl::console::parse_argument (argc, argv, "-c", loopCount);
pcl::registration::LUM<PointType> lum;
lum.setMaxIterations (lumIter);
lum.setConvergenceThreshold (0.001f);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
lum.addPointCloud (clouds[i].second);
}
for (int i = 0; i < iter; i++)
{
for (size_t i = 1; i < clouds.size (); i++)
for (size_t j = 0; j < i; j++)
{
Eigen::Vector4f ci, cj;
pcl::compute3DCentroid (*(clouds[i].second), ci);
pcl::compute3DCentroid (*(clouds[j].second), cj);
Eigen::Vector4f diff = ci - cj;
//std::cout << i << " " << j << " " << diff.norm () << std::endl;
if(diff.norm () < loopDist && (i - j == 1 || i - j > loopCount))
{
if(i - j > loopCount)
std::cout << "add connection between " << i << " (" << clouds[i].first << ") and " << j << " (" << clouds[j].first << ")" << std::endl;
pcl::registration::CorrespondenceEstimation<PointType, PointType> ce;
ce.setInputTarget (clouds[i].second);
ce.setInputSource (clouds[j].second);
pcl::CorrespondencesPtr corr (new pcl::Correspondences);
ce.determineCorrespondences (*corr, dist);
if (corr->size () > 2)
lum.setCorrespondences (j, i, corr);
}
}
lum.compute ();
for(size_t i = 0; i < lum.getNumVertices (); i++)
{
//std::cout << i << ": " << lum.getTransformation (i) (0, 3) << " " << lum.getTransformation (i) (1, 3) << " " << lum.getTransformation (i) (2, 3) << std::endl;
clouds[i].second = lum.getTransformedCloud (i);
}
}
for(size_t i = 0; i < lum.getNumVertices (); i++)
{
std::string result_filename (clouds[i].first);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
//std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
template <typename PointT, typename PointNT, typename PointFeature> void
pcl::NormalBasedSignatureEstimation<PointT, PointNT, PointFeature>::computeFeature (FeatureCloud &output)
{
// do a few checks before starting the computations
PointFeature test_feature;
(void)test_feature;
if (N_prime_ * M_prime_ != sizeof (test_feature.values) / sizeof (float))
{
PCL_ERROR ("NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_, sizeof (test_feature.values) / sizeof (float));
return;
}
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
tree_->setInputCloud (input_);
output.points.resize (indices_->size ());
for (size_t index_i = 0; index_i < indices_->size (); ++index_i)
{
size_t point_i = (*indices_)[index_i];
Eigen::MatrixXf s_matrix (N_, M_);
Eigen::Vector4f center_point = input_->points[point_i].getVector4fMap ();
for (size_t k = 0; k < N_; ++k)
{
Eigen::VectorXf s_row (M_);
for (size_t l = 0; l < M_; ++l)
{
Eigen::Vector4f normal = normals_->points[point_i].getNormalVector4fMap ();
Eigen::Vector4f normal_u = Eigen::Vector4f::Zero ();
Eigen::Vector4f normal_v = Eigen::Vector4f::Zero ();
if (fabs (normal.x ()) > 0.0001f)
{
normal_u.x () = - normal.y () / normal.x ();
normal_u.y () = 1.0f;
normal_u.z () = 0.0f;
normal_u.normalize ();
}
else if (fabs (normal.y ()) > 0.0001f)
{
normal_u.x () = 1.0f;
normal_u.y () = - normal.x () / normal.y ();
normal_u.z () = 0.0f;
normal_u.normalize ();
}
else
{
normal_u.x () = 0.0f;
normal_u.y () = 1.0f;
normal_u.z () = - normal.y () / normal.z ();
}
normal_v = normal.cross3 (normal_u);
Eigen::Vector4f zeta_point = 2.0f * static_cast<float> (l + 1) * scale_h_ / static_cast<float> (M_) *
(cosf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_u +
sinf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_v);
// Compute normal by using the neighbors
Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point;
PointT zeta_point_pcl;
zeta_point_pcl.x = zeta_point_plus_center.x (); zeta_point_pcl.y = zeta_point_plus_center.y (); zeta_point_pcl.z = zeta_point_plus_center.z ();
tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances);
// Do k nearest search if there are no neighbors nearby
if (k_indices.size () == 0)
{
k_indices.resize (5);
k_sqr_distances.resize (5);
tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances);
}
Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
float average_normalization_factor = 0.0f;
// Normals weighted by 1/squared_distances
for (size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i)
{
if (k_sqr_distances[nn_i] < 1e-7f)
{
average_normal = normals_->points[k_indices[nn_i]].getNormalVector4fMap ();
average_normalization_factor = 1.0f;
break;
}
average_normal += normals_->points[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i];
average_normalization_factor += 1.0f / k_sqr_distances[nn_i];
}
average_normal /= average_normalization_factor;
float s = zeta_point.dot (average_normal) / zeta_point.norm ();
s_row[l] = s;
}
// do DCT on the s_matrix row-wise
Eigen::VectorXf dct_row (M_);
for (int m = 0; m < s_row.size (); ++m)
{
float Xk = 0.0f;
for (int n = 0; n < s_row.size (); ++n)
Xk += static_cast<float> (s_row[n] * cos (M_PI / (static_cast<double> (M_ * n) + 0.5) * static_cast<double> (k)));
dct_row[m] = Xk;
}
s_row = dct_row;
s_matrix.row (k).matrix () = dct_row;
}
// do DFT on the s_matrix column-wise
Eigen::MatrixXf dft_matrix (N_, M_);
for (size_t column_i = 0; column_i < M_; ++column_i)
{
Eigen::VectorXf dft_col (N_);
for (size_t k = 0; k < N_; ++k)
{
float Xk_real = 0.0f, Xk_imag = 0.0f;
for (size_t n = 0; n < N_; ++n)
{
Xk_real += static_cast<float> (s_matrix (n, column_i) * cos (2.0f * M_PI / static_cast<double> (N_ * k * n)));
Xk_imag += static_cast<float> (s_matrix (n, column_i) * sin (2.0f * M_PI / static_cast<double> (N_ * k * n)));
}
dft_col[k] = sqrtf (Xk_real*Xk_real + Xk_imag*Xk_imag);
}
dft_matrix.col (column_i).matrix () = dft_col;
}
Eigen::MatrixXf final_matrix = dft_matrix.block (0, 0, N_prime_, M_prime_);
PointFeature feature_point;
for (size_t i = 0; i < N_prime_; ++i)
for (size_t j = 0; j < M_prime_; ++j)
feature_point.values[i*M_prime_ + j] = final_matrix (i, j);
output.points[index_i] = feature_point;
}
}
template<template<class > class Distance, typename PointT, typename FeatureT> bool
GlobalNNPipelineROS<Distance,PointT,FeatureT>::classifyROS (classifier_srv_definitions::classify::Request & req, classifier_srv_definitions::classify::Response & response)
{
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>());
pcl::fromROSMsg(req.cloud, *cloud);
this->input_ = cloud;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pClusteredPCl (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::copyPointCloud(*cloud, *pClusteredPCl);
//clear all data from previous visualization steps and publish empty markers/point cloud
for (size_t i=0; i < markerArray_.markers.size(); i++)
{
markerArray_.markers[i].action = visualization_msgs::Marker::DELETE;
}
vis_pub_.publish( markerArray_ );
sensor_msgs::PointCloud2 scenePc2;
vis_pc_pub_.publish(scenePc2);
markerArray_.markers.clear();
for(size_t cluster_id=0; cluster_id < req.clusters_indices.size(); cluster_id++)
{
std::vector<int> cluster_indices_int;
Eigen::Vector4f centroid;
Eigen::Matrix3f covariance_matrix;
const float r = std::rand() % 255;
const float g = std::rand() % 255;
const float b = std::rand() % 255;
this->indices_.resize(req.clusters_indices[cluster_id].data.size());
for(size_t kk=0; kk < req.clusters_indices[cluster_id].data.size(); kk++)
{
this->indices_[kk] = static_cast<int>(req.clusters_indices[cluster_id].data[kk]);
pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).r = 0.8*r;
pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).g = 0.8*g;
pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).b = 0.8*b;
}
this->classify ();
std::cout << "for cluster " << cluster_id << " with size " << cluster_indices_int.size() << ", I have following hypotheses: " << std::endl;
object_perception_msgs::classification class_tmp;
for(size_t kk=0; kk < this->categories_.size(); kk++)
{
std::cout << this->categories_[kk] << " with confidence " << this->confidences_[kk] << std::endl;
std_msgs::String str_tmp;
str_tmp.data = this->categories_[kk];
class_tmp.class_type.push_back(str_tmp);
class_tmp.confidence.push_back( this->confidences_[kk] );
}
response.class_results.push_back(class_tmp);
typename pcl::PointCloud<PointT>::Ptr pClusterPCl_transformed (new pcl::PointCloud<PointT>());
pcl::computeMeanAndCovarianceMatrix(*this->input_, cluster_indices_int, covariance_matrix, centroid);
Eigen::Matrix3f eigvects;
Eigen::Vector3f eigvals;
pcl::eigen33(covariance_matrix, eigvects, eigvals);
Eigen::Vector3f centroid_transformed = eigvects.transpose() * centroid.topRows(3);
Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Zero(4,4);
transformation_matrix.block<3,3>(0,0) = eigvects.transpose();
transformation_matrix.block<3,1>(0,3) = -centroid_transformed;
transformation_matrix(3,3) = 1;
pcl::transformPointCloud(*this->input_, cluster_indices_int, *pClusterPCl_transformed, transformation_matrix);
//pcl::transformPointCloud(*frame_, cluster_indices_int, *frame_eigencoordinates_, eigvects);
PointT min_pt, max_pt;
pcl::getMinMax3D(*pClusterPCl_transformed, min_pt, max_pt);
std::cout << "Elongations along eigenvectors: " << max_pt.x - min_pt.x << ", " << max_pt.y - min_pt.y
<< ", " << max_pt.z - min_pt.z << std::endl;
geometry_msgs::Point32 centroid_ros;
centroid_ros.x = centroid[0];
centroid_ros.y = centroid[1];
centroid_ros.z = centroid[2];
response.centroid.push_back(centroid_ros);
// calculating the bounding box of the cluster
Eigen::Vector4f min;
Eigen::Vector4f max;
pcl::getMinMax3D (*this->input_, cluster_indices_int, min, max);
object_perception_msgs::BBox bbox;
geometry_msgs::Point32 pt;
pt.x = min[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt);
pt.x = min[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt);
pt.x = min[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt);
pt.x = min[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt);
pt.x = max[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt);
pt.x = max[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt);
pt.x = max[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt);
pt.x = max[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt);
response.bbox.push_back(bbox);
// getting the point cloud of the cluster
typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
pcl::copyPointCloud(*this->input_, cluster_indices_int, *cluster);
sensor_msgs::PointCloud2 pc2;
pcl::toROSMsg (*cluster, pc2);
response.cloud.push_back(pc2);
// visualize the result as ROS topic
visualization_msgs::Marker marker;
marker.header.frame_id = camera_frame_;
marker.header.stamp = ros::Time::now();
//marker.header.seq = ++marker_seq_;
marker.ns = "object_classification";
marker.id = cluster_id;
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = centroid_ros.x;
marker.pose.position.y = centroid_ros.y - 0.1;
marker.pose.position.z = centroid_ros.z - 0.1;
marker.pose.orientation.x = 0;
marker.pose.orientation.y = 0;
marker.pose.orientation.z = 0;
marker.pose.orientation.w = 1.0;
marker.scale.z = 0.1;
marker.color.a = 1.0;
marker.color.r = r/255.f;
marker.color.g = g/255.f;
marker.color.b = b/255.f;
std::stringstream marker_text;
marker_text.precision(2);
marker_text << this->categories_[0] << this->confidences_[0];
marker.text = marker_text.str();
markerArray_.markers.push_back(marker);
}
pcl::toROSMsg (*pClusteredPCl, scenePc2);
vis_pc_pub_.publish(scenePc2);
vis_pub_.publish( markerArray_ );
response.clusters_indices = req.clusters_indices;
return true;
}