void calc_topdown(cv::Mat &top, const std::vector<Slice> &slices, const std::vector<RectList> &bboxes, float max_dist) { float ppm = (float)TOP_SIZE / max_dist; /* Pixels per meter */ for (int i = 0; i < slices.size(); i++) { const RectList& boxes = bboxes[i]; /* Calculate Y position for this slice */ int max_y = TOP_SIZE - TOP_SIZE*(slices[i].min/max_dist); int min_y = TOP_SIZE - TOP_SIZE*(slices[i].max/max_dist); int y = min_y; int height = max_y-min_y; /* Get avg depth for this slice */ float depth = (slices[i].min + slices[i].max) / 2.0; float scale = get_depth_scale(depth); cv::Scalar color = cv::Scalar(255, 255, 255); for (int j = 0; j < boxes.size(); j++) { /* Get distance (IN METERS) of bbox from center */ int dx = boxes[j].x - (IMG_WIDTH/2); float dx_m = dx / scale; /* Get width (IN METERS) of bbox */ float width_m = boxes[j].width / scale; /* Convert to pixels on top-down view */ int x = ppm * dx_m + (TOP_SIZE/2); int width = ppm * width_m; /* Draw */ cv::Rect r = cv::Rect(x, y, width, height); cv::rectangle(top, r, color, -1); } } }
//TODO: mostly copied from above void calc_topdown_grid(Grid &grid, const std::vector<Slice> &slices, const std::vector<RectList> &bboxes, float max_dist) { for (int i = 0; i < slices.size(); i++) { const RectList& boxes = bboxes[i]; /* Get avg depth and scale for this slice */ float depth = (slices[i].min + slices[i].max) / 2.0; float scale = get_depth_scale(depth); int y = (int)((depth/max_dist)*grid.height()); if (y < 0 || y >= grid.height()) continue; cv::Scalar color = cv::Scalar(255, 255, 255); for (int j = 0; j < boxes.size(); j++) { /* Get distance (IN METERS) of bbox from center */ int dx = boxes[j].x - (IMG_WIDTH/2); float dx_m = (dx / scale)+(max_dist/2); /* Get width (IN METERS) of bbox */ float width_m = boxes[j].width / scale; int min_x = (dx_m/max_dist)*grid.width(); int max_x = ((dx_m+width_m)/max_dist)*grid.width(); for (int i = min_x; i <= max_x; i++ ) { if (i < 0 || i >= grid.width()) continue; grid[i][y] = 1; } } } }
float find_obstacles(const cv::Mat& depth_img, cv::Mat& obstacle_img, float min, float max) { int total_px = depth_img.rows * depth_img.cols; int good_px = 0; for (int row = depth_img.rows-1; row >= 0; row--) { const float *d = (const float*)depth_img.ptr(row); float *o = (float*)obstacle_img.ptr(row); for (int col = depth_img.cols-1; col >= 0; col--) { float depth = d[col]; if (depth <= min /*|| depth >= max*/) continue; /* out of range */ good_px++; if (o[col] > 0) continue; /* Already an obstacle? Skip. */ /* Valid for examination */ float scale = get_depth_scale(depth); int min_row = row - (int)std::max(MIN_H * scale, 1.0); int max_row = row - (int)std::max(MAX_H * scale, 1.0); /* Make sure we don't fall off the image! */ min_row = std::max(min_row, 0); max_row = std::max(max_row, 0); /* Loop over relevant image rows to search for obstacle */ bool obstacle = false; for(int subrow = min_row; subrow > max_row; subrow--) { int dx = (int)(tan(THETA) * (float)(row-subrow)); int min_col = std::max(col - dx, 0); int max_col = std::min(col + dx, IMG_WIDTH); const float *sd = (const float*)depth_img.ptr(subrow); float *so = (float*)obstacle_img.ptr(subrow); for(int subcol = min_col; subcol < max_col; subcol++) { float subdepth = sd[subcol]; if (subdepth <= 0) continue; float dz = ((float)dx) / scale; //dz is in meters if (depth - dz < subdepth && subdepth < depth + dz) { obstacle = true; so[subcol] = subdepth; } } } if (obstacle) o[col] = depth; } } return (float)good_px/(float)total_px; }
float rs_get_device_depth_scale(const rs_device * device, rs_error ** error) try { VALIDATE_NOT_NULL(device); return device->get_depth_scale(); }