static gboolean
ensure_slices (GstVaapiEncoderMpeg2 * encoder, GstVaapiEncPicture * picture)
{
g_assert (picture);
if (!fill_slices (encoder, picture))
return FALSE;
return TRUE;
}
void loop() {
Timer loop_t = Timer("TOTAL");
cv::Mat depth, obstacle, clear;
cv_bridge::CvImagePtr img;
{
Timer setup_t = Timer("setup");
/* Grab a pair of images */
sensor_msgs::Image::ConstPtr img_msg;
stereo_msgs::DisparityImage::ConstPtr disp_msg;
get_images(img_msg, disp_msg);
if (img_msg == NULL || disp_msg == NULL) {
loop_t.disable();
setup_t.disable();
//ROS_INFO("No images");
return;
}
/* We don't want to waste time calculating the
same image twice.
*/
unsigned int seq = disp_msg->image.header.seq;
if (last_seq == seq && !last_msg) {
ROS_INFO("Old message #%d", seq);
loop_t.disable();
setup_t.disable();
last_msg = true;
return;
}
last_seq = seq;
last_msg = false;
/* Display raw image */
img = cv_bridge::toCvCopy(img_msg,
sensor_msgs::image_encodings::BGR8);
/* Get disparity data */
cv_bridge::CvImagePtr disp = cv_bridge::toCvCopy(disp_msg->image,
"32FC1");
float focal = disp_msg->f; float base = disp_msg->T;
/* Generate depth image */
/* Why are these so inaccurate? Calibration issue?
float min_depth = (focal * base) / disp_msg->min_disparity;
float max_depth = (focal * base) / disp_msg->max_disparity;
*/
cv::Mat full_depth = (focal * base) / disp->image;
/* Not be necessary if downscale = 1 */
cv::resize(full_depth, depth, cv::Size(IMG_WIDTH, IMG_HEIGHT));
/* Display value-scaled depth image */
#ifdef CV_OUTPUT
cv::Mat scaled_depth = depth / RANGE_MAX;
cv::imshow(DEPTH_WINDOW, scaled_depth);
#endif
/* Create empty obstacle map */
obstacle = cv::Mat::zeros(IMG_HEIGHT, IMG_WIDTH, CV_32F);
clear = cv::Mat::zeros(IMG_HEIGHT, IMG_WIDTH, CV_32F);
}
float pct_good;
/* Find and display obstacles */
{
Timer obs_t = Timer("find_obstacles");
pct_good = find_obstacles(depth, obstacle, RANGE_MIN, 100.0);
}
#ifdef CV_OUTPUT
cv::Mat scaled_obs = obstacle / RANGE_MAX;
cv::imshow(OBS_WINDOW, scaled_obs);
#endif
std::vector<Slice> slices;
std::vector<RectList> slice_bboxes;
{
Timer slice_t = Timer("calc slicing");
/* Set up slices */
{
Timer init_t = Timer("init", 1);
init_slices(slices);
}
{
Timer fill_t = Timer("fill", 1);
fill_slices(obstacle, slices, RANGE_MAX);
}
{
Timer noise_t = Timer("noise", 1);
for (int i = 0; i < slices.size(); i++) {
remove_noise(slices[i].mat);
}
}
{
Timer bbox_t = Timer("bbox", 1);
/* Calculate bounding box on each slice */
for (int i = 0; i < slices.size(); i++) {
slice_bboxes.push_back(calc_bboxes(slices[i].mat));
}
}
}
cv::Mat boxes_image, final_image;
{
Timer disp_t = Timer("disp slicing");
/* Display bounding boxes on image */
boxes_image = img->image.clone();
/* Convert box image to HSV */
cv::cvtColor(boxes_image, boxes_image, cv::COLOR_BGR2HSV);
/* Loop backwards-- farthest first, panter's algorithm */
for (int i = slice_bboxes.size()-1; i >= 0; i--) {
/* Calculate hue */
int hue = (int)(((float)i)/((float)slice_bboxes.size())*120.0);
cv::Scalar color = cv::Scalar(hue, 255, 255);
for (int j = 0; j < slice_bboxes[i].size(); j++) { //TODO: Iterators???
/* Get / resize boxes */
cv::Rect bbox = slice_bboxes[i][j];
bbox.x *= DOWNSCALE; bbox.y *= DOWNSCALE;
bbox.width *= DOWNSCALE; bbox.height *= DOWNSCALE;
/* Draw boxes */
cv::rectangle(boxes_image, bbox, color, -1);
}
}
/* Convert back to RGB */
cv::cvtColor(boxes_image, boxes_image, cv::COLOR_HSV2BGR);
/* Combine with image */
cv::addWeighted(boxes_image, 0.3, img->image, 0.7, 0.0, final_image);
}
/* Generate top-down image */
cv::Mat top;
top = cv::Mat::zeros(TOP_SIZE, TOP_SIZE, CV_8UC1);
Grid grid = Grid(GRID_WIDTH, GRID_HEIGHT, RANGE_MAX, RANGE_MAX);
{
Timer top_t = Timer("topdown");
/* Init top-down image */
calc_topdown_grid(grid, slices, slice_bboxes, RANGE_MAX);
publish_grid(grid, pct_good);
draw_grid(grid, top);
calc_topdown(top, slices, slice_bboxes, RANGE_MAX);
}
#ifdef CV_OUTPUT
cv::imshow(TOP_WINDOW, top);
cv::imshow(IMAGE_WINDOW, final_image);
#endif
#if defined(__SLICE_DEBUG) && defined(CV_OUTPUT)
for (int i = 0; i < NUM_SLICES; i++) {
std::string s = "a";
s[0] = 'a'+i;
cv::imshow(s, slices[i]);
}
#endif
}
