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); }