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