void TerrainRenderingView::VisitHeightMapNode(HeightMapNode* node) {
bool bufferSupport = arg->renderer.BufferSupport();
GeometrySetPtr geom = node->GetGeometrySet();
this->ApplyGeometrySet(geom);
IShaderResourcePtr shader = node->GetLandscapeShader();
if (this->renderShader && shader){
// Setup uniforms
shader->SetUniform("lightDir", lightDir);
shader->SetUniform("viewPos", arg->canvas.GetViewingVolume()->GetPosition());
shader->ApplyShader();
}
node->CalcLOD(arg->canvas.GetViewingVolume());
IndicesPtr indices = node->GetIndices();
if (bufferSupport) glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, indices->GetID());
// Replace with a patch iterator
node->Render(*arg);
if (shader){
shader->ReleaseShader();
this->currentShader.reset();
}
if (bufferSupport) glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
if (renderTangent)
node->RenderBoundingGeometry();
node->VisitSubNodes(*this);
}
template <typename PointT> void
pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedian (
const PointCloudConstPtr &cloud, const IndicesPtr &indices, Eigen::Vector4f &median)
{
// Copy the values to vectors for faster sorting
std::vector<float> x (indices->size ());
std::vector<float> y (indices->size ());
std::vector<float> z (indices->size ());
for (size_t i = 0; i < indices->size (); ++i)
{
x[i] = cloud->points[(*indices)[i]].x;
y[i] = cloud->points[(*indices)[i]].y;
z[i] = cloud->points[(*indices)[i]].z;
}
std::sort (x.begin (), x.end ());
std::sort (y.begin (), y.end ());
std::sort (z.begin (), z.end ());
int mid = indices->size () / 2;
if (indices->size () % 2 == 0)
{
median[0] = (x[mid-1] + x[mid]) / 2;
median[1] = (y[mid-1] + y[mid]) / 2;
median[2] = (z[mid-1] + z[mid]) / 2;
}
else
{
median[0] = x[mid];
median[1] = y[mid];
median[2] = z[mid];
}
median[3] = 0;
}
template <typename PointT> double
pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedianAbsoluteDeviation (
const PointCloudConstPtr &cloud, const IndicesPtr &indices, double sigma)
{
std::vector<double> distances (indices->size ());
Eigen::Vector4f median;
// median (dist (x - median (x)))
computeMedian (cloud, indices, median);
for (size_t i = 0; i < indices->size (); ++i)
{
pcl::Vector4fMapConst pt = cloud->points[(*indices)[i]].getVector4fMap ();
Eigen::Vector4f ptdiff = pt - median;
ptdiff[3] = 0;
distances[i] = ptdiff.dot (ptdiff);
}
std::sort (distances.begin (), distances.end ());
double result;
int mid = indices->size () / 2;
// Do we have a "middle" point or should we "estimate" one ?
if (indices->size () % 2 == 0)
result = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
else
result = sqrt (distances[mid]);
return (sigma * result);
}
void
jsk_pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0)
return;
// If cloud is given, check if it's valid
if (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
return;
}
/// DEBUG
if (indices)
NODELET_DEBUG ("[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
///
// Check whether the user has given a different input TF frame
tf_input_orig_frame_ = cloud->header.frame_id;
PointCloud2::ConstPtr cloud_tf;
if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
{
NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
// Save the original frame ID
// Convert the cloud into the different frame
PointCloud2 cloud_transformed;
tf_listener_.waitForTransform(cloud->header.frame_id, tf_input_frame_, cloud->header.stamp, ros::Duration(5.0));
if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_))
{
NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
return;
}
cloud_tf = boost::make_shared<PointCloud2> (cloud_transformed);
}
else
cloud_tf = cloud;
// Need setInputCloud () here because we have to extract x/y/z
IndicesPtr vindices;
if (indices)
vindices.reset (new std::vector<int> (indices->indices));
computePublish (cloud_tf, vindices);
}
template <typename PointT> void
pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::extractExtrema (std::list<IndicesPtr> &rois)
{
std::vector<std::vector<bool> > is_min (scale_values_.size ()),
is_max (scale_values_.size ());
// for each point, check if it is a local extrema on each scale
for (size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
{
std::vector<bool> is_min_scale (input_->points.size ()),
is_max_scale (input_->points.size ());
for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
{
std::vector<int> nn_indices;
geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices);
bool is_max_point = true, is_min_point = true;
for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
if (F_scales_[scale_i][point_i] < F_scales_[scale_i][*nn_it])
is_max_point = false;
else
is_min_point = false;
is_min_scale[point_i] = is_min_point;
is_max_scale[point_i] = is_max_point;
}
is_min[scale_i] = is_min_scale;
is_max[scale_i] = is_max_scale;
}
// look for points that are min/max over three consecutive scales
for (size_t scale_i = 1; scale_i < scale_values_.size () - 1; ++scale_i)
{
for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
if ((is_min[scale_i - 1][point_i] && is_min[scale_i][point_i] && is_min[scale_i + 1][point_i]) ||
(is_max[scale_i - 1][point_i] && is_max[scale_i][point_i] && is_max[scale_i + 1][point_i]))
{
// add the point to the result vector
IndicesPtr region (new std::vector<int>);
region->push_back (static_cast<int> (point_i));
// and also add its scale-sized geodesic neighborhood
std::vector<int> nn_indices;
geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices);
region->insert (region->end (), nn_indices.begin (), nn_indices.end ());
rois.push_back (region);
}
}
}
template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg)
{
this->addPointIdx (pointIdx_arg);
if (indices_arg)
indices_arg->push_back (pointIdx_arg);
}
void
UniformSamplingWrapper::compute(PointCloudInPtr input, PointCloudOut &output)
{
float keypoint_separation = 0.01f;
IndicesPtr indices (new std::vector<int>());
PointCloud<int> leaves;
UniformSampling<PointNormal> us;
us.setRadiusSearch(keypoint_separation);
us.setInputCloud(input);
us.compute(leaves);
// Copy point cloud and return
indices->assign(leaves.points.begin(), leaves.points.end()); // can't use operator=, probably because of different allocators
copyPointCloud(*input, *indices, output);
}
template <typename PointT> void
pcl::MaximumLikelihoodSampleConsensus<PointT>::getMinMax (
const PointCloudConstPtr &cloud, const IndicesPtr &indices, Eigen::Vector4f &min_p, Eigen::Vector4f &max_p)
{
min_p.setConstant (FLT_MAX);
max_p.setConstant (-FLT_MAX);
min_p[3] = max_p[3] = 0;
for (size_t i = 0; i < indices->size (); ++i)
{
if (cloud->points[(*indices)[i]].x < min_p[0]) min_p[0] = cloud->points[(*indices)[i]].x;
if (cloud->points[(*indices)[i]].y < min_p[1]) min_p[1] = cloud->points[(*indices)[i]].y;
if (cloud->points[(*indices)[i]].z < min_p[2]) min_p[2] = cloud->points[(*indices)[i]].z;
if (cloud->points[(*indices)[i]].x > max_p[0]) max_p[0] = cloud->points[(*indices)[i]].x;
if (cloud->points[(*indices)[i]].y > max_p[1]) max_p[1] = cloud->points[(*indices)[i]].y;
if (cloud->points[(*indices)[i]].z > max_p[2]) max_p[2] = cloud->points[(*indices)[i]].z;
}
}
pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::MLSVoxelGrid (PointCloudInConstPtr& cloud,
IndicesPtr &indices,
float voxel_size) :
voxel_grid_ (), bounding_min_ (), bounding_max_ (), data_size_ (), voxel_size_ (voxel_size)
{
pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
// Put initial cloud in voxel grid
data_size_ = static_cast<uint64_t> (1.5 * max_size / voxel_size_);
for (unsigned int i = 0; i < indices->size (); ++i)
if (pcl_isfinite (cloud->points[(*indices)[i]].x))
{
Eigen::Vector3i pos;
getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos);
uint64_t index_1d;
getIndexIn1D (pos, index_1d);
Leaf leaf;
voxel_grid_[index_1d] = leaf;
}
}
void
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0)
return;
// If cloud+normals is given, check if it's valid
if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
{
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
emptyPublish (cloud);
return;
}
// If surface is given, check if it's valid
if (cloud_surface && !isValid (cloud_surface, "surface"))
{
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
emptyPublish (cloud);
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices))
{
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
emptyPublish (cloud);
return;
}
/// DEBUG
if (cloud_surface)
if (indices)
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
else
if (indices)
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
///
if ((int)(cloud->width * cloud->height) < k_)
{
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height));
emptyPublish (cloud);
return;
}
// If indices given...
IndicesPtr vindices;
if (indices && !indices->header.frame_id.empty ())
vindices.reset (new std::vector<int> (indices->indices));
computePublish (cloud, cloud_normals, cloud_surface, vindices);
}
void generateIndices(size_t step = 100)
{
indices->clear();
for(size_t i = 0; i < cloud->points.size(); i += step)
indices->push_back(i);
}
