/**
* update each gaussian with the norm of the corresponding point
* points with frame_mask > 0 are not updated. This can be used to only update gaussians with
* background pixels. (use getForeground_* to compute such a mask)
*/
void PixelEnvironmentModel::update(const Cloud& cloud, cv::Mat* frame_mask, int step){
assert(int(cloud.width) == width_ && int(cloud.height) == height_);
timing_start("up_pixel");
update_cnt++;
if (mask_set){
// int cnt = cv::countNonZero(mask_);
// ROS_INFO("Update: mask has %i pixels",cnt);
}else{
ROS_INFO("No mask defined");
}
for (int y=0; y<height_; y += step)
for (int x=0; x<width_; x += step){
if (mask_set && mask_.at<uchar>(y,x) == 0) continue;
if (frame_mask && frame_mask->at<uchar>(y,x) > 0) continue;
double length = norm(cloud.at(x,y));
if (length > 0) // check for nans
gaussians[y][x].update(length);
}
timing_end("up_pixel");
}
/// point is foreground if it is not within N sigma
void PixelEnvironmentModel::getForeground_prob(const Cloud& cloud, float N, cv::Mat& foreground){
if (foreground.rows != height_ || foreground.cols != width_ || foreground.type() != CV_8UC1){
foreground = cv::Mat(height_,width_,CV_8UC1);
}
foreground.setTo(0);
for (int y=0; y<height_; ++y)
for (int x=0; x<width_; ++x){
if (mask_set && mask_.at<uchar>(y,x) == 0) continue;
bool inited = gaussians[y][x].initialized;
float current = norm(cloud.at(x,y));
if (current < 0) continue; // nan point
if (!inited || (current < gaussians[y][x].mean && !gaussians[y][x].isWithinNSigma(current,N))){
foreground.at<uchar>(y,x) = 255;
}
}
cv::medianBlur(foreground,foreground,3);
// cv::erode(foreground,foreground,cv::Mat(),cv::Point(-1,-1),2);
// cv::dilate(foreground,foreground,cv::Mat(),cv::Point(-1,-1),2);
}
/// lock all cells on which the marker points in the cloud are projected into
void Elevation_map::lockCells(const cv::Mat & mask, const Cloud& current){
if (locked.cols != mean.cols || locked.rows !=mean.rows || locked.type() == CV_8UC1){
locked = cv::Mat(mean.rows,mean.cols, CV_8UC1);
}
locked.setTo(0);
assert(mask.cols == int(current.width) && mask.rows == int(current.height));
for (int x=0; x<mask.cols; ++x)
for (int y=0; y<mask.rows; ++y){
if (mask.at<uchar>(y,x) == 0) continue;
cv::Point pos = grid_pos(current.at(x,y));
if (pos.x < 0) continue;
locked.at<uchar>(pos.y,pos.x) = 255;
}
locking_active = true;
// cv::namedWindow("blocked");
// cv::imshow("blocked",locked);
}
void applyBilateralFilter(const Cloud& in, double diameter, double sigmaDist, double sigmaPixel, Cloud& out){
cv::Mat dist(in.height, in.width, CV_32FC1);
out = in;
for (uint x=0; x<in.width; ++x)
for (uint y=0; y<in.height; ++y){
pcl_Point p = in.at(x,y);
if (p.x != p.x)
dist.at<float>(y,x) = 0;
else
dist.at<float>(y,x) = norm(p);
}
cv::Mat smoothed;
cv::bilateralFilter(dist, smoothed, diameter, sigmaDist, sigmaPixel);
float mean_change = 0;
int cnt= 0;
for (uint x=0; x<in.width; ++x)
for (uint y=0; y<in.height; ++y){
pcl_Point p = in.at(x,y);
float new_dist = smoothed.at<float>(y,x);
if (abs(new_dist) < 0.2) continue;
float old_dist = dist.at<float>(y,x);
mean_change += abs(new_dist-old_dist);
cnt++;
out.at(x,y) = setLength(p,new_dist);
}
mean_change /= cnt;
ROS_INFO("bilateral filter: mean change of %.2f cm", mean_change*100);
}
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::showBackground(const Cloud& cloud){
Cloud result; result = cloud;
for (uint x=0; x<cloud.width; ++x)
for (uint y=0; y<cloud.height; ++y){
float m = means.at<float>(y,x);
if (m > 0){
pcl_Point c = cloud.at(x,y);
if (c.x != c.x) continue;
setLength(c,m);
result.at(x,y) = c;
}
}
return result;
}
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;
}
cv::Mat Elevation_map::getFGMask(const Cloud& cloud, float max_dist, cv::Mat* areaMask){
cv::Mat res(cloud.height, cloud.width, CV_8UC1);
res.setTo(0);
// if (areaMask){
// areaMask = new cv::Mat(cloud.height, cloud.width, CV_8UC1);
// areaMask->setTo(0);
// ROS_INFO("Area: %i %i", areaMask->cols, areaMask->rows);
// }
int step = 1;
for (uint x=0; x<cloud.width; x += step)
for (uint y=0; y<cloud.height; y += step){
pcl_Point p = cloud.at(x,y);
if (p.x!=p.x) continue;
cv::Point pos = grid_pos(p);
if (pos.x < 0) continue;
if (areaMask){
areaMask->at<uchar>(y,x) = 255;
}
float h = mean.at<float>(pos.y,pos.x);
if (p.z > h+max_dist){
res.at<uchar>(y,x) = 255;
}
}
if (step>1){
cv::dilate(res, res, cv::Mat(), cv::Point(-1,-1),floor(step/2));
if (areaMask)
cv::dilate(*areaMask, *areaMask, cv::Mat(), cv::Point(-1,-1),floor(step/2));
}
return res;
}
// 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;
}
void PixelEnvironmentModel::getNorm(const Cloud& current, cv::Mat& norms){
if (norms.rows != height_ || norms.cols != width_ || norms.type() != CV_32FC1){
norms = cv::Mat(height_,width_,CV_32FC1);
}
norms.setTo(0);
for (int y=0; y<height_; ++y)
for (int x=0; x<width_; ++x){
// ROS_INFO("x,y: %i %i",x,y);
if (mask_set && mask_.at<uchar>(y,x) == 0) continue;
if (!gaussians[y][x].initialized) continue;
float d = norm(current.at(x,y));
if (d < 0) continue; // nan point
norms.at<float>(y,x) = d;
}
}
uint Background_substraction::addTrainingFrame(const Cloud& c){
if (dists.size() == 0){
means = cv::Mat(c.height, c.width, CV_32FC1);
std_dev = cv::Mat(c.height, c.width, CV_32FC1);
reset_helper_images();
}
cv::Mat new_dist = cv::Mat(c.height, c.width, CV_32FC1);
new_dist.setTo(0);
for (uint x=0; x<c.width; ++x)
for (uint y=0; y<c.height; ++y){
pcl_Point p = c.at(x,y);
// store distance to object if available, -1 else
new_dist.at<float>(y,x) = (p.x==p.x)?norm(p):-1;
}
dists.push_back(new_dist);
return dists.size();
}
void PixelEnvironmentModel::getForeground_dist(const Cloud& cloud, float max_dist, cv::Mat& foreground, cv::Mat* dists, int step){
// if (foreground.rows != height_ || foreground.cols != width_ || foreground.type() != CV_8UC1){
// foreground = cv::Mat(height_,width_,CV_8UC1);
// }
// ROS_INFO("foreground step = %i",step);
timing_start("GetForeground");
ensureSizeAndType(foreground,width_,height_,CV_8UC1);
if (dists){
ensureSizeAndType(*dists,width_,height_,CV_32FC1);
dists->setTo(0);
assert(dists->cols == foreground.cols);
}
foreground.setTo(0);
// float min_ = 1e6;
// float max_ = -1e6;
// int fg_cnt = 0;
for (int y=0; y<height_; y += step)
for (int x=0; x<width_; x += step){
if (mask_set && mask_.at<uchar>(y,x) == 0) continue;
float mean = gaussians[y][x].mean;
bool inited = gaussians[y][x].initialized;
// bool stable =gaussians[y][x].isStableMeasurement();
float current = norm(cloud.at(x,y));
if (current < 0) continue; // nan point
// foreground if there was no measurement so far or distance to mean value is larger than max_dist
if (!inited || current + max_dist < mean){
foreground.at<uchar>(y,x) = 255;
// fg_cnt++;
if (dists){
float d = mean-current;
dists->at<float>(y,x) = d;
// min_ = min(min_,d);
// max_ = max(max_,d);
}
}
}
// ROS_INFO("getForeground_dist: %f %f (%i fg pixels)", min_,max_,fg_cnt);
timing_start("blur");
//cv::medianBlur(foreground,foreground,3);
cv::dilate(foreground,foreground,cv::Mat(),cv::Point(-1,-1),step);
cv::erode(foreground,foreground,cv::Mat(),cv::Point(-1,-1),step);
// cv::medianBlur(foreground,foreground,3);
if (dists && step > 0){
cv::dilate(*dists,*dists,cv::Mat(),cv::Point(-1,-1),step);
cv::GaussianBlur(*dists,*dists,cv::Size(3,3),3,3);
}
timing_end("blur");
timing_end("GetForeground");
}
