void Elevation_map::getModel(Cloud& model){ model.clear(); pcl_Point p; model.resize(cell_cnt_y*cell_cnt_x); int pos = 0; for (int y = 0; y < cell_cnt_y; ++y){ p.y = y_min_+(y+0.5)*cell_size_; for (int x = 0; x < cell_cnt_x; ++x) { p.x = x_min_+(x+0.5)*cell_size_; p.z = mean.at<float>(y,x); // model.push_back(p); model[pos++] = p; } } model.width = cell_cnt_x; model.height = cell_cnt_y; if (int(model.size()) != cell_cnt_x*cell_cnt_y){ ROS_INFO("size: %zu, %i % i", model.size(),cell_cnt_x,cell_cnt_y); // assert(int(model.size()) == cell_cnt_x*cell_cnt_y); } }
template<typename PointT, typename PointNT> void pcl::CovarianceSampling<PointT, PointNT>::applyFilter (Cloud &output) { std::vector<int> sampled_indices; applyFilter (sampled_indices); output.resize (sampled_indices.size ()); output.header = input_->header; output.height = 1; output.width = uint32_t (output.size ()); output.is_dense = true; for (size_t i = 0; i < sampled_indices.size (); ++i) output[i] = (*input_)[sampled_indices[i]]; }