template<typename PointT> void
pcl::approximatePolygon (const PlanarPolygon<PointT>& polygon, PlanarPolygon<PointT>& approx_polygon, float threshold, bool refine, bool closed)
{
const Eigen::Vector4f& coefficients = polygon.getCoefficients ();
const typename pcl::PointCloud<PointT>::VectorType &contour = polygon.getContour ();
Eigen::Vector3f rotation_axis (coefficients[1], -coefficients[0], 0.0f);
rotation_axis.normalize ();
float rotation_angle = acosf (coefficients [2]);
Eigen::Affine3f transformation = Eigen::Translation3f (0, 0, coefficients [3]) * Eigen::AngleAxisf (rotation_angle, rotation_axis);
typename pcl::PointCloud<PointT>::VectorType polygon2D (contour.size ());
for (unsigned pIdx = 0; pIdx < polygon2D.size (); ++pIdx)
polygon2D [pIdx].getVector3fMap () = transformation * contour [pIdx].getVector3fMap ();
typename pcl::PointCloud<PointT>::VectorType approx_polygon2D;
approximatePolygon2D<PointT> (polygon2D, approx_polygon2D, threshold, refine, closed);
typename pcl::PointCloud<PointT>::VectorType &approx_contour = approx_polygon.getContour ();
approx_contour.resize (approx_polygon2D.size ());
Eigen::Affine3f inv_transformation = transformation.inverse ();
for (unsigned pIdx = 0; pIdx < approx_polygon2D.size (); ++pIdx)
approx_contour [pIdx].getVector3fMap () = inv_transformation * approx_polygon2D [pIdx].getVector3fMap ();
}
void
MultiviewRecognizerWithChangeDetection<PointT>::reconstructionFiltering(typename pcl::PointCloud<PointT>::Ptr observation,
pcl::PointCloud<pcl::Normal>::Ptr observation_normals, const Eigen::Matrix4f &absolute_pose, size_t view_id) {
CloudPtr observation_transformed(new Cloud);
pcl::transformPointCloud(*observation, *observation_transformed, absolute_pose);
CloudPtr cloud_tmp(new Cloud);
std::vector<int> indices;
v4r::ChangeDetector<PointT>::difference(observation_transformed, removed_points_cumulated_history_[view_id],
cloud_tmp, indices, param_.tolerance_for_cloud_diff_);
/* Visualization of changes removal for reconstruction:
Cloud rec_changes;
rec_changes += *cloud_transformed;
v4r::VisualResultsStorage::copyCloudColored(*removed_points_cumulated_history_[view_id], rec_changes, 255, 0, 0);
v4r::VisualResultsStorage::copyCloudColored(*cloud_tmp, rec_changes, 200, 0, 200);
stringstream ss;
ss << view_id;
visResStore.savePcd("reconstruction_changes_" + ss.str(), rec_changes);*/
std::vector<bool> preserved_mask(observation->size(), false);
for (std::vector<int>::iterator i = indices.begin(); i < indices.end(); i++) {
preserved_mask[*i] = true;
}
for (size_t j = 0; j < preserved_mask.size(); j++) {
if (!preserved_mask[j]) {
setNan(observation->at(j));
setNan(observation_normals->at(j));
}
}
PCL_INFO("Points by change detection removed: %d\n", observation->size() - indices.size());
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
octree.setInputCloud (input_);
octree.addPointsFromInputCloud ();
typename pcl::PointCloud<PointInT>::VectorType occupied_cells;
octree.getOccupiedVoxelCenters (occupied_cells);
// Determine the voxels crosses along the line segments
// formed by every pair of occupied cells.
std::vector< std::vector<int> > line_histograms;
for (size_t i = 0; i < occupied_cells.size (); ++i)
{
Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();
for (size_t j = i+1; j < occupied_cells.size (); ++j)
{
typename pcl::PointCloud<PointInT>::VectorType intersected_cells;
Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f);
// Intersected cells are ordered from closest to furthest w.r.t. the origin.
std::vector<int> histogram;
for (size_t k = 0; k < intersected_cells.size (); ++k)
{
std::vector<int> indices;
octree.voxelSearch (intersected_cells[k], indices);
int label = emptyLabel ();
if (indices.size () != 0)
{
label = getDominantLabel (indices);
}
histogram.push_back (label);
}
line_histograms.push_back(histogram);
}
}
std::vector< std::vector<int> > transition_histograms;
computeTransitionHistograms (line_histograms, transition_histograms);
std::vector<float> distances;
computeDistancesToMean (transition_histograms, distances);
std::vector<float> gfpfh_histogram;
computeDistanceHistogram (distances, gfpfh_histogram);
output.clear ();
output.width = 1;
output.height = 1;
output.points.resize (1);
std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
}
template <typename PointT> inline float
pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, float max_dist, int nr_threads)
{
const float max_dist_sqr = max_dist * max_dist;
const std::size_t s = cloud.size ();
pcl::search::KdTree <PointT> tree;
tree.setInputCloud (cloud);
float mean_dist = 0.f;
int num = 0;
std::vector <int> ids (2);
std::vector <float> dists_sqr (2);
#ifdef _OPENMP
#pragma omp parallel for \
reduction (+:mean_dist, num) \
private (ids, dists_sqr) shared (tree, cloud) \
default (none)num_threads (nr_threads)
#endif
for (int i = 0; i < 1000; i++)
{
tree.nearestKSearch (cloud->points[rand () % s], 2, ids, dists_sqr);
if (dists_sqr[1] < max_dist_sqr)
{
mean_dist += std::sqrt (dists_sqr[1]);
num++;
}
}
return (mean_dist / num);
};
void WorldDownloadManager::cropCloudWithSphere(const Eigen::Vector3f & center,const float radius,
typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud)
{
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const PointT & pt = (*cloud)[i];
const Eigen::Vector3f ept(pt.x,pt.y,pt.z);
if ((ept - center).squaredNorm() > radius * radius)
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
uint count = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
count++;
}
out_cloud->resize(count);
}
void WorldDownloadManager::cropCloud(const Eigen::Vector3f & min,const Eigen::Vector3f & max,
typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud)
{
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const PointT & pt = (*cloud)[i];
if (pt.x > max.x() || pt.y > max.y() || pt.z > max.z() ||
pt.x < min.x() || pt.y < min.y() || pt.z < min.z())
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
uint count = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
count++;
}
out_cloud->resize(count);
}
static bool colorize(const typename pcl::PointCloud<PointTypeIn>& iCloud,
const Eigen::Affine3d& iCloudToCamera,
const bot_core::image_t& iImage,
const BotCamTrans* iCamTrans,
typename pcl::PointCloud<PointTypeOut>& oCloud) {
pcl::copyPointCloud(iCloud, oCloud);
pcl::PointCloud<PointTypeOut> tempCloud;
pcl::transformPointCloud(iCloud, tempCloud, iCloudToCamera.cast<float>());
int numPoints = iCloud.size();
for (int i = 0; i < numPoints; ++i) {
double p[3] = {tempCloud[i].x, tempCloud[i].y, tempCloud[i].z};
double pix[3];
bot_camtrans_project_point(iCamTrans, p, pix);
oCloud[i].r = oCloud[i].g = oCloud[i].b = 0;
if (pix[2] < 0) {
continue;
}
uint8_t r, g, b;
if (interpolate(pix[0], pix[1], iImage, r, g, b)) {
oCloud[i].r = r;
oCloud[i].g = g;
oCloud[i].b = b;
}
}
return true;
}
pcl::IndicesPtr radiusFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree (new KdTree(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud, indices);
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
output->resize(oi);
return output;
}
}
void projectCloudOnXYPlane(
typename pcl::PointCloud<PointT>::Ptr & cloud)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
cloud->at(i).z = 0;
}
}
VectorXb boxMask(typename pcl::PointCloud<T>::ConstPtr in, float xmin, float ymin, float zmin, float xmax, float ymax, float zmax) {
int i=0;
VectorXb out(in->size());
BOOST_FOREACH(const T& pt, in->points) {
out[i] = (pt.x >= xmin && pt.x <= xmax && pt.y >= ymin && pt.y <= ymax && pt.z >= zmin && pt.z <= zmax);
++i;
}
return out;
}
template <typename PointT> bool
PCLVisualizer::addCorrespondences (
const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
const std::vector<int> &correspondences,
const std::string &id,
int viewport)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
vtkSmartPointer<vtkAppendPolyData> polydata = vtkSmartPointer<vtkAppendPolyData>::New ();
vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
line_colors->SetNumberOfComponents (3);
line_colors->SetName ("Colors");
// Use Red by default (can be changed later)
unsigned char rgb[3];
rgb[0] = 1 * 255.0;
rgb[1] = 0 * 255.0;
rgb[2] = 0 * 255.0;
// Draw lines between the best corresponding points
for (size_t i = 0; i < source_points->size (); ++i)
{
const PointT &p_src = source_points->points[i];
const PointT &p_tgt = target_points->points[correspondences[i]];
// Add the line
vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
line->SetPoint1 (p_src.x, p_src.y, p_src.z);
line->SetPoint2 (p_tgt.x, p_tgt.y, p_tgt.z);
line->Update ();
polydata->AddInput (line->GetOutput ());
line_colors->InsertNextTupleValue (rgb);
}
polydata->Update ();
vtkSmartPointer<vtkPolyData> line_data = polydata->GetOutput ();
line_data->GetCellData ()->SetScalars (line_colors);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (line_data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetOpacity (0.5);
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
//style_->setCloudActorMap (cloud_actor_map_);
return (true);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr toXYZ(typename pcl::PointCloud<T>::ConstPtr in) {
pcl::PointCloud<pcl::PointXYZ>::Ptr out(new pcl::PointCloud<PointXYZ>());
out->reserve(in->size());
out->width = in->width;
out->height = in->height;
BOOST_FOREACH(const T& pt, in->points) {
out->points.push_back(PointXYZ(pt.x, pt.y, pt.z));
}
return out;
}
void ICCVTutorial<FeatureType>::findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const
{
cout << "correspondence assignment..." << std::flush;
correspondences.resize (source->size());
// Use a KdTree to search for the nearest matches in feature space
pcl::KdTreeFLANN<FeatureType> descriptor_kdtree;
descriptor_kdtree.setInputCloud (target);
// Find the index of the best match for each keypoint, and store it in "correspondences_out"
const int k = 1;
std::vector<int> k_indices (k);
std::vector<float> k_squared_distances (k);
for (size_t i = 0; i < source->size (); ++i)
{
descriptor_kdtree.nearestKSearch (*source, i, k, k_indices, k_squared_distances);
correspondences[i] = k_indices[0];
}
cout << "OK" << endl;
}
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud)
{
if ( cloud->size () == 0 )
{
PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
return;
}
input_ = cloud;
adjacency_octree_->setInputCloud (cloud);
}
void SurfelMapPublisher::publishPointCloud(const boost::shared_ptr<MapType>& map)
{
if (m_pointCloudPublisher.getNumSubscribers() == 0)
return;
typename pcl::PointCloud<PointType>::Ptr cellPointsCloud(new pcl::PointCloud<PointType>());
map->lock();
map->getCellPoints(cellPointsCloud);
map->unlock();
cellPointsCloud->header.frame_id = map->getFrameId();
cellPointsCloud->header.stamp = pcl_conversions::toPCL(map->getLastUpdateTimestamp());
m_pointCloudPublisher.publish(cellPointsCloud);
ROS_DEBUG_STREAM("publishing cell points: " << cellPointsCloud->size());
}
size_t bbox_filter(typename pcl::PointCloud<P>::Ptr in,
typename pcl::PointCloud<P>::Ptr inliers, // inside interval
typename pcl::PointCloud<P>::Ptr outliers, // outside interval
std::string axis, double min, double max)
{
typename pcl::PassThrough<P> pass;
cout << "filtering " << axis << " " << min << " - " << max << endl;
pass.setInputCloud (in);
pass.setFilterFieldName (axis);
pass.setFilterLimits (min, max);
pass.setFilterLimitsNegative(false);
pass.filter (*inliers);
pass.setFilterLimitsNegative(true);
pass.filter (*outliers);
return inliers->size();
}
int loadPointCloud(typename pcl::PointCloud<PointT>::Ptr &pPointCloud,
const std::string &strFilePath,
const bool fAccumulation = false)
{
// point cloud
typename pcl::PointCloud<PointT>::Ptr pTempCloud(new pcl::PointCloud<PointT>());
// load the file based on the file extension
int result = -2;
const std::string strFileExtension(extractFileExtension(strFilePath));
if(strFileExtension.compare(".pcd") == 0) result = pcl::io::loadPCDFile<PointT>(strFilePath.c_str(), *pTempCloud);
if(strFileExtension.compare(".ply") == 0) result = pcl::io::loadPLYFile<PointT>(strFilePath.c_str(), *pTempCloud);
// error
switch(result)
{
case -2:
{
PCL_ERROR("Unknown file extension!");
system("pause");
break;
}
case -1:
{
PCL_ERROR("Couldn't read the file!");
system("pause");
break;
}
}
//remove NaN Points
pcl::removeNaNFromPointCloud(*pTempCloud, *pTempCloud, std::vector<int>());
// accumulate
if(fAccumulation) *pPointCloud += *pTempCloud;
else pPointCloud = pTempCloud;
return pPointCloud->size();
}
void
gestureCallback (const openni_tracker_with_pointing::PointingGestureJoints& msg)
{
ROS_INFO("gesture rcvd!");
publishMarkerArrayForGesture(msg);
Eigen::Vector3f hand_pt (msg.hand.x, msg.hand.y, msg.hand.z);
Eigen::Vector3f elbow_pt (msg.elbow.x, msg.elbow.y, msg.elbow.z);
Eigen::Vector3f head_pt (msg.head.x, msg.head.y, msg.head.z);
//double hor,ver;
//getAngleErrors(hand_pt,elbow_pt,hand_pt,hor,ver);
for (size_t i = 0; i < clusters_.size (); i++)
{
}
}
int computeNormalsAndEigenvalues(const typename pcl::PointCloud<PointInT>::Ptr
&in_cloud,
const float &radius, const int &n_threads,
pcl::PointCloud<PointOutT> &out_cloud)
{
size_t n_points = in_cloud->size();
// resize the out cloud
out_cloud.resize(n_points);
// build the kdtree
pcl::KdTreeFLANN<PointInT> flann;
flann.setInputCloud(in_cloud);
// place for neighs ids and distances
std::vector<int> indices;
std::vector<float> distances;
#ifdef USE_OPENMP
#pragma omp parallel for shared(out_cloud) private(indices, distances) \
num_threads(n_threads)
#endif
for (int i = 0; i < n_points; ++i) {
// get neighbors for this point
flann.radiusSearch((*in_cloud)[i], radius, indices, distances);
// estimate normal and eigenvalues
computePointNormal(*in_cloud, indices, out_cloud[i].normal_x,
out_cloud[i].normal_y, out_cloud[i].normal_z,
out_cloud[i].lam0, out_cloud[i].lam1,
out_cloud[i].lam2);
flipNormal
<PointInT>((*(in_cloud))[i], 0.0, 0.0, 0.0, out_cloud[i].normal_x,
out_cloud[i].normal_y, out_cloud[i].normal_z);
}
return 1;
}
inline std::vector<int> all_indices(const typename pcl::PointCloud<T> &cloud) {
std::vector<int> indices;
indices.resize(cloud.size());
std::iota(indices.begin(), indices.end(), 0);
return indices;
}
template<typename PointT> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
const typename pcl::search::KdTree<PointT>::Ptr kdtree,
std::vector<Eigen::Matrix3d>& cloud_covariances)
{
if (k_correspondences_ > int (cloud->size ()))
{
PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
return;
}
Eigen::Vector3d mean;
std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
// We should never get there but who knows
if(cloud_covariances.size () < cloud->size ())
cloud_covariances.resize (cloud->size ());
typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
for(;
points_iterator != cloud->end ();
++points_iterator, ++matrices_iterator)
{
const PointT &query_point = *points_iterator;
Eigen::Matrix3d &cov = *matrices_iterator;
// Zero out the cov and mean
cov.setZero ();
mean.setZero ();
// Search for the K nearest neighbours
kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
// Find the covariance matrix
for(int j = 0; j < k_correspondences_; j++) {
const PointT &pt = (*cloud)[nn_indecies[j]];
mean[0] += pt.x;
mean[1] += pt.y;
mean[2] += pt.z;
cov(0,0) += pt.x*pt.x;
cov(1,0) += pt.y*pt.x;
cov(1,1) += pt.y*pt.y;
cov(2,0) += pt.z*pt.x;
cov(2,1) += pt.z*pt.y;
cov(2,2) += pt.z*pt.z;
}
mean /= static_cast<double> (k_correspondences_);
// Get the actual covariance
for (int k = 0; k < 3; k++)
for (int l = 0; l <= k; l++)
{
cov(k,l) /= static_cast<double> (k_correspondences_);
cov(k,l) -= mean[k]*mean[l];
cov(l,k) = cov(k,l);
}
// Compute the SVD (covariance matrix is symmetric so U = V')
Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
cov.setZero ();
Eigen::Matrix3d U = svd.matrixU ();
// Reconstitute the covariance matrix with modified singular values using the column // vectors in V.
for(int k = 0; k < 3; k++) {
Eigen::Vector3d col = U.col(k);
double v = 1.; // biggest 2 singular values replaced by 1
if(k == 2) // smallest singular value replaced by gicp_epsilon
v = gicp_epsilon_;
cov+= v * col * col.transpose();
}
}
}
template <typename PointT> void
pcl::approximatePolygon2D (const typename pcl::PointCloud<PointT>::VectorType &polygon,
typename pcl::PointCloud<PointT>::VectorType &approx_polygon,
float threshold, bool refine, bool closed)
{
approx_polygon.clear ();
if (polygon.size () < 3)
return;
std::vector<std::pair<unsigned, unsigned> > intervals;
std::pair<unsigned,unsigned> interval (0, 0);
if (closed)
{
float max_distance = .0f;
for (unsigned idx = 1; idx < polygon.size (); ++idx)
{
float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) +
(polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);
if (distance > max_distance)
{
max_distance = distance;
interval.second = idx;
}
}
for (unsigned idx = 1; idx < polygon.size (); ++idx)
{
float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) +
(polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);
if (distance > max_distance)
{
max_distance = distance;
interval.first = idx;
}
}
if (max_distance < threshold * threshold)
return;
intervals.push_back (interval);
std::swap (interval.first, interval.second);
intervals.push_back (interval);
}
else
{
interval.first = 0;
interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
intervals.push_back (interval);
}
std::vector<unsigned> result;
// recursively refine
while (!intervals.empty ())
{
std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);
line_x *= linelen;
line_y *= linelen;
line_d *= linelen;
float max_distance = 0.0;
unsigned first_index = currentInterval.first + 1;
unsigned max_index = 0;
// => 0-crossing
if (currentInterval.first > currentInterval.second)
{
for (unsigned idx = first_index; idx < polygon.size(); idx++)
{
float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
if (distance > max_distance)
{
max_distance = distance;
max_index = idx;
}
}
first_index = 0;
}
for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
{
float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
if (distance > max_distance)
{
max_distance = distance;
max_index = idx;
}
}
if (max_distance > threshold)
{
std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
currentInterval.second = max_index;
intervals.push_back (interval);
}
else
{
result.push_back (currentInterval.second);
intervals.pop_back ();
}
}
approx_polygon.reserve (result.size ());
if (refine)
{
std::vector<Eigen::Vector3f> lines (result.size ());
std::reverse (result.begin (), result.end ());
for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
{
unsigned nIdx = rIdx + 1;
if (nIdx == result.size ())
nIdx = 0;
Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
unsigned pIdx = result[rIdx];
unsigned num_points = 0;
if (pIdx > result[nIdx])
{
num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
for (; pIdx < polygon.size (); ++pIdx)
{
covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
centroid [0] += polygon [pIdx].x;
centroid [1] += polygon [pIdx].y;
}
pIdx = 0;
}
num_points += result[nIdx] - pIdx;
for (; pIdx < result[nIdx]; ++pIdx)
{
covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
centroid [0] += polygon [pIdx].x;
centroid [1] += polygon [pIdx].y;
}
covariance.coeffRef (2) = covariance.coeff (1);
float norm = 1.0f / float (num_points);
centroid *= norm;
covariance *= norm;
covariance.coeffRef (0) -= centroid [0] * centroid [0];
covariance.coeffRef (1) -= centroid [0] * centroid [1];
covariance.coeffRef (3) -= centroid [1] * centroid [1];
float eval;
Eigen::Vector2f normal;
eigen22 (covariance, eval, normal);
// select the one which is more "parallel" to the original line
Eigen::Vector2f direction;
direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
direction.normalize ();
if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
{
std::swap (normal [0], normal [1]);
normal [0] = -normal [0];
}
// needs to be on the left side of the edge
if (direction [0] * normal [1] < direction [1] * normal [0])
normal *= -1.0;
lines [rIdx].head<2> () = normal;
lines [rIdx] [2] = -normal.dot (centroid);
}
float threshold2 = threshold * threshold;
for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
{
unsigned nIdx = rIdx + 1;
if (nIdx == result.size ())
nIdx = 0;
Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
vertex /= vertex [2];
vertex [2] = 0.0;
PointT point;
// test whether we need another edge since the intersection point is too far away from the original vertex
Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
pq [2] = 0.0;
float distance = pq.squaredNorm ();
if (distance > threshold2)
{
// test whether the old point is inside the new polygon or outside
if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
(pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
{
float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];
point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];
approx_polygon.push_back (point);
vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
}
}
point.getVector3fMap () = vertex;
approx_polygon.push_back (point);
}
}
else
{
// we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)
for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
approx_polygon.push_back (polygon [*it]);
}
}
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize,
float groundNormalAngle,
int minClusterSize)
{
if(cloud->size() == 0)
{
return;
}
pcl::IndicesPtr groundIndices, obstaclesIndices;
segmentObstaclesFromGround<PointT>(
cloud,
indices,
groundIndices,
obstaclesIndices,
cellSize,
groundNormalAngle,
minClusterSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
//project on XY plane
util3d::projectCloudOnXYPlane(groundCloud);
//voxelize to grid cell size
groundCloud = util3d::voxelize(groundCloud, cellSize);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
//project on XY plane
util3d::projectCloudOnXYPlane(obstaclesCloud);
//voxelize to grid cell size
obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
}
ground = cv::Mat();
if(groundCloud->size())
{
ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
for(unsigned int i=0;i<groundCloud->size(); ++i)
{
ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
}
}
obstacles = cv::Mat();
if(obstaclesCloud->size())
{
obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
{
obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
}
}
}
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
const kinfu_msgs::KinfuCloudPoint & max,typename pcl::PointCloud<PointT>::ConstPtr cloud,
TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud, TrianglesPtr out_triangles)
{
const uint triangles_size = triangles->size();
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
std::vector<uint> valid_points_remap(cloud_size,0);
uint offset;
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const PointT & pt = (*cloud)[i];
if (pt.x > max.x || pt.y > max.y || pt.z > max.z ||
pt.x < min.x || pt.y < min.y || pt.z < min.z)
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
offset = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
// save new position for triangles remap
valid_points_remap[i] = offset;
offset++;
}
out_cloud->resize(offset);
// discard invalid triangles
out_triangles->clear();
out_triangles->reserve(triangles_size);
offset = 0;
for (uint i = 0; i < triangles_size; i++)
{
const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i];
bool is_valid = true;
// validate all the vertices
for (uint h = 0; h < 3; h++)
if (!valid_points[tri.vertex_id[h]])
{
is_valid = false;
break;
}
if (is_valid)
{
kinfu_msgs::KinfuMeshTriangle out_tri;
// remap the triangle
for (uint h = 0; h < 3; h++)
out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]];
out_triangles->push_back(out_tri);
offset++;
}
}
out_triangles->resize(offset);
}
void WorldDownloadManager::removeDuplicatePoints(typename pcl::PointCloud<PointT>::ConstPtr cloud,
TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud,TrianglesPtr out_triangles)
{
const uint64 input_size = cloud->size();
std::vector<uint64> ordered(input_size);
for (uint64 i = 0; i < input_size; i++)
ordered[i] = i;
std::sort(ordered.begin(),ordered.end(),WorldDownloadManager_mergeMeshTrianglesComparator<PointT>(*cloud));
typedef WorldDownloadManager_mergeMeshTrianglesAverageClass<PointT> AverageClass;
std::vector<uint64> re_association_vector(input_size);
std::vector<AverageClass, Eigen::aligned_allocator<AverageClass> > average_vector;
uint64 output_i = 0;
re_association_vector[ordered[0]] = 0;
for (uint64 src_i = 1; src_i < input_size; src_i++)
{
const PointT & prev = (*cloud)[ordered[src_i - 1]];
const PointT & current = (*cloud)[ordered[src_i]];
const int not_equal = std::memcmp(prev.data,current.data,sizeof(float) * 3);
if (not_equal)
output_i++;
re_association_vector[ordered[src_i]] = output_i;
}
const uint64 output_size = output_i + 1;
out_cloud->resize(output_size);
average_vector.resize(output_size);
for (uint64 i = 0; i < input_size; i++)
average_vector[re_association_vector[i]].insert((*cloud)[i]);
for (uint64 i = 0; i < output_size; i++)
(*out_cloud)[i] = average_vector[i].get();
if (out_triangles && triangles)
{
const uint64 triangles_size = triangles->size();
uint64 out_triangles_size = 0;
out_triangles->resize(triangles_size);
for (uint64 i = 0; i < triangles_size; i++)
{
kinfu_msgs::KinfuMeshTriangle new_tri;
for (uint h = 0; h < 3; h++)
new_tri.vertex_id[h] = re_association_vector[(*triangles)[i].vertex_id[h]];
bool degenerate = false;
for (uint h = 0; h < 3; h++)
if (new_tri.vertex_id[h] == new_tri.vertex_id[(h + 1) % 3])
degenerate = true;
if (!degenerate)
{
(*out_triangles)[out_triangles_size] = new_tri;
out_triangles_size++;
}
}
out_triangles->resize(out_triangles_size);
}
}
void MapPublisher::publishMap(const boost::shared_ptr<MapType>& map)
{
if (pub_map_msg_.getNumSubscribers() == 0)
return;
pcl::StopWatch timer;
timer.reset();
mrs_laser_mapping::MultiResolutionMapPtr map_msg(new mrs_laser_mapping::MultiResolutionMap);
map_msg->header.stamp = map->getLastUpdateTimestamp();
map_msg->header.frame_id = map->getFrameId();
map_msg->levels = map->getLevels();
map_msg->size_in_meters = map->getSizeInMeters();
map_msg->resolution = map->getResolution();
map_msg->cell_capacity = map->getCellCapacity();
unsigned int nr_points = 0;
for (unsigned int i = 0; i < map->getLevels(); i++)
{
typename pcl::PointCloud<PointType>::Ptr points(new pcl::PointCloud<PointType>());
map->getCellPoints(points, i);
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(*points, ros_cloud);
map_msg->cloud.push_back(ros_cloud);
nr_points += points->size();
}
/*
* get all free cells
*/
std::vector<float> cell_sizes;
std::vector<std::vector<Eigen::Vector3f>> occupied_cell_offsets;
typename MapType::CellPointerVectorVector occupied_grid_cells;
int levels = map->getLevels();
for (int l = 0; l < levels; l++)
{
cell_sizes.push_back(map->getCellSize(l));
std::vector<Eigen::Vector3f> occupied_cells_level;
typename MapType::CellPointerVector occupied_grid_cells_level;
map->getCellsWithOffset(occupied_grid_cells_level, occupied_cells_level, l, -99999.f); // TODO: this sucks...
occupied_cell_offsets.push_back(occupied_cells_level);
occupied_grid_cells.push_back(occupied_grid_cells_level);
}
for (int l = cell_sizes.size() - 1; l >= 0; l--)
{
for (size_t i = 0; i < occupied_cell_offsets[l].size(); i++)
{
if ((*occupied_grid_cells[l][i]).getOccupancy() <= 0.f)
{ // thos
// if ( occupiedGridCells[l][i]->m_debugState == mrs_laser_maps::GridCellWithStatistics::FREE ) {
geometry_msgs::Point p;
p.x = occupied_cell_offsets[l][i](0);
p.y = occupied_cell_offsets[l][i](1);
p.z = occupied_cell_offsets[l][i](2);
map_msg->free_cell_centers.push_back(p);
}
}
}
pub_map_msg_.publish(map_msg);
ROS_DEBUG_STREAM_NAMED("map_publisher","published map message took: " << timer.getTime() << " ms with " << nr_points
<< " points. from timestamp " << map_msg->header.stamp << " at time "
<< ros::Time::now());
}
void
process ()
{
std::cout << "threshold: " << threshold_ << std::endl;
std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n");
unsigned char red [6] = {255, 0, 0, 255, 255, 0};
unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);
typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
refinement_compare->setDistanceThreshold (threshold_, depth_dependent_);
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (5000);
mps.setAngularThreshold (0.017453 * 3.0); //3 degrees
mps.setDistanceThreshold (0.03); //2cm
mps.setRefinementComparator (refinement_compare);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
typename pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
typename pcl::PointCloud<PointT>::Ptr approx_contour (new pcl::PointCloud<PointT>);
char name[1024];
typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
double normal_start = pcl::getTime ();
ne.setInputCloud (cloud);
ne.compute (*normal_cloud);
double normal_end = pcl::getTime ();
std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;
double plane_extract_start = pcl::getTime ();
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud);
if (refine_)
mps.segmentAndRefine (regions);
else
mps.segment (regions);
double plane_extract_end = pcl::getTime ();
std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << " with planar regions found: " << regions.size () << std::endl;
std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
viewer.removeAllPointClouds (0);
viewer.removeAllShapes (0);
pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0);
viewer.addPointCloud<PointT> (cloud, single_color, "cloud");
pcl::PlanarPolygon<PointT> approx_polygon;
//Draw Visualization
for (size_t i = 0; i < regions.size (); i++)
{
Eigen::Vector3f centroid = regions[i].getCentroid ();
Eigen::Vector4f model = regions[i].getCoefficients ();
pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
centroid[1] + (0.5f * model[1]),
centroid[2] + (0.5f * model[2]));
sprintf (name, "normal_%d", unsigned (i));
viewer.addArrow (pt2, pt1, 1.0, 0, 0, std::string (name));
contour->points = regions[i].getContour ();
sprintf (name, "plane_%02d", int (i));
pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
viewer.addPointCloud (contour, color, name);
pcl::approximatePolygon (regions[i], approx_polygon, threshold_, polygon_refinement_);
approx_contour->points = approx_polygon.getContour ();
std::cout << "polygon: " << contour->size () << " -> " << approx_contour->size () << std::endl;
typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour;
// sprintf (name, "approx_plane_%02d", int (i));
// viewer.addPolygon<PointT> (approx_contour_const, 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
for (unsigned idx = 0; idx < approx_contour->points.size (); ++idx)
{
sprintf (name, "approx_plane_%02d_%03d", int (i), int(idx));
viewer.addLine (approx_contour->points [idx], approx_contour->points [(idx+1)%approx_contour->points.size ()], 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
}
}
}
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
// Find the ground
pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
cloud,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalRadiusSearch*2.0f,
Eigen::Vector4f(0,0,100,0));
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
cloud,
flatSurfaces,
normalRadiusSearch*2.0f,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
if((int)i!=biggestFlatSurfaceIndex)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
}
}
if(ground->size() != cloud->size())
{
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);
//Cluster remaining stuff (obstacles)
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
cloud,
otherStuffIndices,
normalRadiusSearch*2.0f,
minClusterSize);
// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
}
}
bool
segmentCloud (pointing_object_evaluation::ObjectSegmentationRequest::Request &req, pointing_object_evaluation::ObjectSegmentationRequest::Response &res)
{
CloudPtr cloud (new Cloud ());
{
//boost::lock_guard<boost::mutex> lock (cloud_mutex_);
*cloud = *cloud_;
}
unsigned char red [6] = {255, 0, 0, 255, 255, 0};
unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
pcl::PointCloud<pcl::PointXYZRGB> aggregate_cloud;
// Estimate Normals
double ne_start = pcl::getTime ();
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne_.setInputCloud (cloud);
ne_.compute (*normal_cloud);
double ne_end = pcl::getTime ();
//std::cout << "NE took: " << double(ne_end - ne_start) << std::endl;
// Segment Planes
double mps_start = pcl::getTime ();
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
mps_.setInputNormals (normal_cloud);
mps_.setInputCloud (cloud);
//mps.segment (regions);
mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
//Segment Objects
typename pcl::PointCloud<PointT>::CloudVectorType clusters;
if (regions.size () > 0)
{
//printf ("got some planes!\n");
std::vector<bool> plane_labels;
plane_labels.resize (label_indices.size (), false);
for (size_t i = 0; i < label_indices.size (); i++)
{
if (label_indices[i].indices.size () > 10000)
{
plane_labels[i] = true;
}
}
//printf ("made label vec\n");
typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>());
euclidean_cluster_comparator_->setInputCloud (cloud);
euclidean_cluster_comparator_->setLabels (labels);
euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
euclidean_cluster_comparator_->setDistanceThreshold (0.01f, false);
pcl::PointCloud<pcl::Label> euclidean_labels;
std::vector<pcl::PointIndices> euclidean_label_indices;
pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
euclidean_segmentation.setInputCloud (cloud);
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
//printf ("done segmenting the clusters\n");
std::vector<Eigen::Vector4f> centroids;
for (size_t i = 0; i < euclidean_label_indices.size (); i++)
{
if (euclidean_label_indices[i].indices.size () > 1000)
{
pcl::PointCloud<PointT> cluster;
pcl::copyPointCloud (*cloud,euclidean_label_indices[i].indices,cluster);
clusters.push_back (cluster);
/*Eigen::Vector4f centroid;
pcl::compute3DCentroid (cluster, centroid);
centroids.push_back (centroid);*/
//pcl::PointXYZ centroid_pt (centroid[0], centroid[1], centroid[2]);
//double dist = sqrt (centroid[0]*centroid[0] + centroid[1]*centroid[1] + centroid[2]*centroid[2]);
//object_points_.push_back (centroid_pt);
// Send to RViz
pcl::PointCloud<pcl::PointXYZRGB> color_cluster;
pcl::copyPointCloud (cluster, color_cluster);
for (size_t j = 0; j < color_cluster.size (); j++)
{
color_cluster.points[j].r = (color_cluster.points[j].r + red[i%6]) / 2;
color_cluster.points[j].g = (color_cluster.points[j].g + grn[i%6]) / 2;
color_cluster.points[j].b = (color_cluster.points[j].b + blu[i%6]) / 2;
}
aggregate_cloud += color_cluster;
}
}
PCL_INFO ("Got %d euclidean clusters!\n", clusters.size ());
//PCL_INFO ("Got %d centroids!\n", centroids.size ());
aggregate_cloud.header = cloud->header;
sensor_msgs::PointCloud2 cloud_msg;
pcl::toROSMsg (aggregate_cloud, cloud_msg);
object_cloud_pub_.publish (cloud_msg);
// TODO: mutex
/*clusters_ = clusters;
centroids_= centroids;*/
}
}
void OctreeVoxelGrid::generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr& input_msg)
{
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
typename pcl::PointCloud<PointT>::Ptr cloud_voxeled (new pcl::PointCloud<PointT> ());
pcl::fromROSMsg(*input_msg, *cloud);
// generate octree
typename pcl::octree::OctreePointCloud<PointT> octree(resolution_);
// add point cloud to octree
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
// get points where grid is occupied
typename pcl::octree::OctreePointCloud<PointT>::AlignedPointTVector point_vec;
octree.getOccupiedVoxelCenters(point_vec);
// put points into point cloud
cloud_voxeled->width = point_vec.size();
cloud_voxeled->height = 1;
for (int i = 0; i < point_vec.size(); i++) {
cloud_voxeled->push_back(point_vec[i]);
}
// publish point cloud
sensor_msgs::PointCloud2 output_msg;
toROSMsg(*cloud_voxeled, output_msg);
output_msg.header = input_msg->header;
pub_cloud_.publish(output_msg);
// publish marker
if (publish_marker_flag_) {
visualization_msgs::Marker marker_msg;
marker_msg.type = visualization_msgs::Marker::CUBE_LIST;
marker_msg.scale.x = resolution_;
marker_msg.scale.y = resolution_;
marker_msg.scale.z = resolution_;
marker_msg.header = input_msg->header;
marker_msg.pose.orientation.w = 1.0;
if (marker_color_ == "flat") {
marker_msg.color = jsk_topic_tools::colorCategory20(0);
marker_msg.color.a = marker_color_alpha_;
}
// compute min and max
Eigen::Vector4f minpt, maxpt;
pcl::getMinMax3D<PointT>(*cloud_voxeled, minpt, maxpt);
PointT p;
for (size_t i = 0; i < cloud_voxeled->size(); i++) {
p = cloud_voxeled->at(i);
geometry_msgs::Point point_ros;
point_ros.x = p.x;
point_ros.y = p.y;
point_ros.z = p.z;
marker_msg.points.push_back(point_ros);
if (marker_color_ == "flat") {
marker_msg.colors.push_back(jsk_topic_tools::colorCategory20(0));
}
else if (marker_color_ == "z") {
marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.z - minpt[2]) / (maxpt[2] - minpt[2])));
}
else if (marker_color_ == "x") {
marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.x - minpt[0]) / (maxpt[0] - minpt[0])));
}
else if (marker_color_ == "y") {
marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.y - minpt[1]) / (maxpt[1] - minpt[1])));
}
marker_msg.colors[marker_msg.colors.size() - 1].a = marker_color_alpha_;
}
pub_marker_.publish(marker_msg);
}
}
