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_;
}
}
Cloud Background_substraction::removeBackground(const Cloud& current, float min_dist, float max_dist, std::vector<cv::Point2i>* valids){
Cloud result;
// assert(cloud.size() == reference.size());
// for (uint i=0; i<cloud.size(); ++i){
ROS_INFO("min: %f, max: %f", min_dist,max_dist);
if (min_dist == max_dist){
ROS_WARN("removeBackground called with min = %f and max = %f", min_dist, max_dist);
return result;
}
uint invalid = 0;
for (uint x=0; x<current.width; ++x)
for (uint y=0; y<current.height; ++y){
pcl_Point c = current.at(x,y);
if (c.x != c.x) { invalid++; continue;}
if (mask.at<uchar>(y,x) == 0) continue;
float d = norm(c);
float mean = means.at<float>(y,x);
// ROS_INFO("Norm: %f, mean: %f", d, mean);
if ( mean - min_dist > d && d > mean - max_dist){
result.push_back(c);
if (valids) valids->push_back(cv::Point2i(x,y));
}
}
// ROS_INFO("BG: %i invalid", invalid);
return result;
}
// untested
Cloud Background_substraction::applyMask(Cloud& current){
if (uint(mask.cols) != current.width){
ROS_WARN("no background computed!");
return Cloud();
}
Cloud result;
result.reserve(current.size());
for (uint x=0; x<current.width; ++x)
for (uint y=0; y<current.height; ++y){
if (mask.at<uchar>(y,x) == 255)
result.push_back(current.at(x,y));
}
return result;
}
