/** * 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"); }