void VoxelCalculator::compute(Cloud &cloud) { AccumulatorMap accumulatorMap; float inverseResolution = 1.0f / _resolution; for(size_t i = 0; i < cloud.points().size(); i++) { const Point &point = cloud.points()[i]; IndexComparator s; s.indeces[0] = (int) (point[0] * inverseResolution); s.indeces[1] = (int) (point[1] * inverseResolution); s.indeces[2] = (int) (point[2] * inverseResolution); AccumulatorMap::iterator it = accumulatorMap.find(s); if(it == accumulatorMap.end()) { VoxelAccumulator voxelAccumulator; voxelAccumulator.accumulator = point; voxelAccumulator.numPoints = 1; voxelAccumulator.index = i; accumulatorMap.insert(make_pair(s, voxelAccumulator)); } else { VoxelAccumulator &voxelAccumulator = it->second; voxelAccumulator.add(point); } } std::cout << "Voxelization resized the cloud from " << cloud.points().size() << " to "; // HAKKE // cloud.clear(); Cloud tmpCloud; tmpCloud.clear(); for(AccumulatorMap::iterator it = accumulatorMap.begin(); it != accumulatorMap.end(); it++) { VoxelAccumulator &voxelAccumulator = it->second; // HAKKE // Point average = voxelAccumulator.average(); // cloud.points().push_back(average); tmpCloud.points().push_back(cloud.points()[voxelAccumulator.index]); tmpCloud.normals().push_back(cloud.normals()[voxelAccumulator.index]); tmpCloud.stats().push_back(cloud.stats()[voxelAccumulator.index]); if(cloud.pointInformationMatrix().size() == cloud.points().size() && cloud.normalInformationMatrix().size() == cloud.points().size()) { tmpCloud.pointInformationMatrix().push_back(cloud.pointInformationMatrix()[voxelAccumulator.index]); tmpCloud.normalInformationMatrix().push_back(cloud.normalInformationMatrix()[voxelAccumulator.index]); } // if(cloud.traversabilityVector().size() == cloud.points().size()) { // tmpCloud.traversabilityVector().push_back(cloud.traversabilityVector()[voxelAccumulator.index]); // } if(cloud.gaussians().size() == cloud.points().size()) { tmpCloud.gaussians().push_back(cloud.gaussians()[voxelAccumulator.index]); } } // HAKKE cloud.clear(); cloud = tmpCloud; std::cout << cloud.points().size() << " points" << std::endl; }
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); } }
void PixelEnvironmentModel::getCloudRepresentation(const Cloud& model, Cloud& result, float N){ result.clear(); uint8_t r = 255, g = 0, b = 0; uint32_t rgb_red = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b); r = 0; g = 255; uint32_t rgb_green = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b); pcl_Point nap; nap.x = std::numeric_limits<float>::quiet_NaN(); for (int y = 0; y < height_; ++y) for (int x = 0; x < width_; ++x){ if ((mask_set && mask_.at<uchar>(y,x) == 0) || !gaussians[y][x].initialized){ if (N<0) result.push_back(nap); continue; } float mean_dist = gaussians[y][x].mean; // add point with same color but with norm = mean pcl_Point p = model.at(x,y); if (p.x != p.x) { if (N<0) // keep cloud organized if no sigma-points are included result.push_back(nap); continue; } pcl_Point mean = setLength(p,mean_dist); mean.rgb = p.rgb; result.push_back(mean); if (N > 0 && gaussians[y][x].initialized){ float sigma = gaussians[y][x].sigma(); pcl_Point near = setLength(p,mean_dist-N*sigma); near.rgb = *reinterpret_cast<float*>(&rgb_green); result.push_back(near); pcl_Point far = setLength(p,mean_dist+N*sigma); far.rgb = *reinterpret_cast<float*>(&rgb_red); result.push_back(far); } } if (N<0){ assert(int(result.size()) == width_*height_); result.width = width_; result.height = height_; } }
void MultiProjector::unproject(Cloud& cloud, const RawDepthImage& depth_image, const RGBImage& rgb_image){ Cloud temp; cloud.clear(); for (size_t i = 0; i<_projectors.size(); i++) { if (!_projectors[i]) { continue; } unsigned short * zb= const_cast<unsigned short*>(depth_image.ptr<const unsigned short>(_mono_rows*i)); RGBImage mono_rgb; RawDepthImage mono_zbuffer(_mono_rows, _image_cols,zb); if (rgb_image.rows && rgb_image.cols) { cv::Vec3b * rgbb= const_cast<cv::Vec3b*>(rgb_image.ptr<const cv::Vec3b>(_mono_rows*i)); mono_rgb = RGBImage(_mono_rows, _image_cols, rgbb); } _projectors[i]->unproject(temp, mono_zbuffer, mono_rgb); cloud.add(temp); } cloud.transformInPlace(_camera_info->offset()); }