void show(void)
{
if(!existImage || !existPoints){
return;
}
const auto& encoding = sensor_msgs::image_encodings::BGR8;
cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image_msg, encoding);
IplImage frame = cv_image->image;
cv::Mat matImage = cv::cvarrToMat(&frame);//(&frame, false);
/* DRAW RECTANGLES of detected objects */
#if 0
for(std::size_t i=0; i<cars.size();i++) {
if(cars[i].y > matImage.rows*.3) { //temporal way to avoid drawing detections in the sky
cvRectangle( &frame,
cvPoint(cars[i].x, cars[i].y),
cvPoint(cars[i].x+cars[i].width, cars[i].y+cars[i].height),
_colors[0], 3, 8,0 );
}
}
for(std::size_t i=0; i<peds.size();i++) {
if(peds[i].y > matImage.rows*.3) {
cvRectangle( &frame,
cvPoint(peds[i].x, peds[i].y),
cvPoint(peds[i].x+peds[i].width, peds[i].y+peds[i].height),
_colors[1], 3, 8,0 );
}
}
#else
drawRects(&frame,
car_fused_objects.obj,
cvScalar(255.0, 255.0, 0,0),
matImage.rows*.10);
drawRects(&frame,
pedestrian_fused_objects.obj,
cvScalar(0.0, 255.0, 0,0),
matImage.rows*.10);
#endif
/* PUT DISTANCE text on image */
putDistance(&frame,
car_fused_objects.obj,
matImage.rows*.10,
car_fused_objects.type.c_str());
putDistance(&frame,
pedestrian_fused_objects.obj,
matImage.rows*.10,
pedestrian_fused_objects.type.c_str());
/* DRAW POINTS of lidar scanning */
int w = matImage.size().width;
int h = matImage.size().height;
int n = w * h;
float min_d = 1<<16, max_d = -1;
// int min_i = 1<<8, max_i = -1;
for(int i=0; i<n; i++){
int di = points_msg->distance[i];
max_d = di > max_d ? di : max_d;
min_d = di < min_d ? di : min_d;
// int it = points_msg->intensity[i];
// max_i = it > max_i ? it : max_i;
// min_i = it < min_i ? it : min_i;
}
float wid_d = max_d - min_d;
for(int y=0; y<h; y++){
for(int x=0; x<w; x++){
int j = y * w + x;
double distance = points_msg->distance[j];
if(distance == 0){
continue;
}
int colorid= wid_d ? ( (distance - min_d) * 255 / wid_d ) : 128;
cv::Vec3b color=colormap.at<cv::Vec3b>(colorid);
int g = color[1];
int b = color[2];
int r = color[0];
cvRectangle(&frame, cvPoint(x, y), cvPoint(x+1, y+1), CV_RGB(r, g, b));
}
}
if (cvGetWindowHandle(window_name) != NULL) // Guard not to write destroyed window by using close button on the window
{
cvShowImage(window_name, &frame);
cvWaitKey(2);
}
}
static void show()
{
if(!exist_image || !exist_scan){
return;
}
IplImage* image_view = cvCreateImage(cvGetSize(&image), image.depth, image.nChannels);
cvCopy(&image, image_view);
float min_d, max_d;
min_d = max_d = scan_image.distance.at(0);
for(int i = 1; i < IMAGE_WIDTH * IMAGE_HEIGHT; i++){
float di = scan_image.distance.at(i);
max_d = di > max_d ? di : max_d;
min_d = di < min_d ? di : min_d;
}
float wid_d = max_d - min_d;
/*
* Plot depth points on an image
*/
CvPoint pt;
int height, width;
for(int i = 0; i < (int)scan_image.distance.size(); i++) {
height = (int)(i % IMAGE_HEIGHT);
width = (int)(i / IMAGE_HEIGHT);
if(scan_image.distance.at(i) != 0.0) {
pt.x = width;
pt.y = height;
int colorid= wid_d ? ( (scan_image.distance.at(i) - min_d) * 255 / wid_d ) : 128;
cv::Vec3b color=colormap.at<cv::Vec3b>(colorid);
int g = color[1];
int b = color[2];
int r = color[0];
cvCircle(image_view, pt, 2, CV_RGB (r, g, b), CV_FILLED, 8, 0);
}
}
drawRects(image_view,
car_fused_objects.obj,
cvScalar(255.0, 255.0, 0,0),
(image_view->height)*.3);
drawRects(image_view,
pedestrian_fused_objects.obj,
cvScalar(0.0, 255.0, 0,0),
(image_view->height)*.3);
/* PUT DISTANCE text on image */
putDistance(image_view,
car_fused_objects.obj,
(image_view->height)*.3,
car_fused_objects.type.c_str());
putDistance(image_view,
pedestrian_fused_objects.obj,
(image_view->height)*.3,
pedestrian_fused_objects.type.c_str());
/*
* Show image
*/
cvShowImage(window_name, image_view);
cvWaitKey(2);
cvReleaseImage(&image_view);
}
