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