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