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