static void drawRects(IplImage *Image, std::vector<autoware_msgs::ImageRectRanged> objects, CvScalar color, int threshold_height) { unsigned int object_num = objects.size(); for(unsigned int i = 0; i < object_num; i++) { if (objects.at(i).rect.y > threshold_height && !isNearlyNODATA(objects.at(i).range)) { // temporal way to avoid drawing detections in the sky CvPoint p1=cvPoint(objects.at(i).rect.x, objects.at(i).rect.y); CvPoint p2=cvPoint(objects.at(i).rect.x + objects.at(i).rect.width, objects.at(i).rect.y + objects.at(i).rect.height); cvRectangle(Image,p1,p2,color,OBJ_RECT_THICKNESS); } } }
void showRects(IplImage *Image, std::vector<autoware_msgs::image_rect_ranged> objects, double ratio, CvScalar col) { unsigned int object_num = objects.size(); for(unsigned int i = 0; i < object_num; i++) { if (!isNearlyNODATA(objects.at(i).range)) { CvPoint p1=cvPoint(objects.at(i).rect.x, objects.at(i).rect.y); CvPoint p2=cvPoint(objects.at(i).rect.x + objects.at(i).rect.width, objects.at(i).rect.y + objects.at(i).rect.height); cvRectangle(Image,p1,p2,col,OBJ_RECT_THICKNESS); } } }
static void showImage() { IplImage* image_clone = cvCloneImage(image); char distance_string[32]; CvFont dfont; float hscale = 0.7f; float vscale = 0.7f; float italicscale = 0.0f; int thickness = 1; std::string objectLabel; CvFont dfont_label; float hscale_label = 0.5f; float vscale_label = 0.5f; CvSize text_size; int baseline = 0; cvInitFont(&dfont_label, CV_FONT_HERSHEY_COMPLEX, hscale_label, vscale_label, italicscale, thickness, CV_AA); objectLabel = car_fused_objects.type; cvGetTextSize(objectLabel.data(), &dfont_label, &text_size, &baseline); /* * Plot obstacle frame */ showRects(image_clone, car_fused_objects.obj, ratio, cvScalar(255.0,255.0,0.0)); showRects(image_clone, pedestrian_fused_objects.obj, ratio, cvScalar(0.0,255.0,0.0)); /* * Plot car distance data on image */ for (unsigned int i = 0; i < car_fused_objects.obj.size(); i++) { if(!isNearlyNODATA(car_fused_objects.obj.at(i).range)) { int rect_x = car_fused_objects.obj.at(i).rect.x; int rect_y = car_fused_objects.obj.at(i).rect.y; int rect_width = car_fused_objects.obj.at(i).rect.width; int rect_height = car_fused_objects.obj.at(i).rect.height; float range = car_fused_objects.obj.at(i).range; /* put label */ CvPoint labelOrg = cvPoint(rect_x - OBJ_RECT_THICKNESS, rect_y - baseline - OBJ_RECT_THICKNESS); cvRectangle(image_clone, cvPoint(labelOrg.x + 0, labelOrg.y + baseline), cvPoint(labelOrg.x + text_size.width, labelOrg.y - text_size.height), CV_RGB(0, 0, 0), // label background color is black -1, 8, 0 ); cvPutText(image_clone, objectLabel.data(), labelOrg, &dfont_label, CV_RGB(255, 255, 255) // label text color is white ); /* put distance data */ cvRectangle(image_clone, cv::Point(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 45), rect_y + rect_height + 5), cv::Point(rect_x + (rect_width/2) + (((int)log10(range/100)+1) * 8 + 38), rect_y + rect_height + 30), cv::Scalar(255,255,255), -1); cvInitFont (&dfont, CV_FONT_HERSHEY_COMPLEX , hscale, vscale, italicscale, thickness, CV_AA); sprintf(distance_string, "%.2f m", range / 100); //unit of length is meter cvPutText(image_clone, distance_string, cvPoint(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 40), rect_y + rect_height + 25), &dfont, CV_RGB(255, 0, 0)); } } objectLabel = pedestrian_fused_objects.type; cvGetTextSize(objectLabel.data(), &dfont_label, &text_size, &baseline); /* * Plot pedestrian distance data on image */ for (unsigned int i = 0; i < pedestrian_fused_objects.obj.size(); i++) { if(!isNearlyNODATA(pedestrian_fused_objects.obj.at(i).range)) { int rect_x = pedestrian_fused_objects.obj.at(i).rect.x; int rect_y = pedestrian_fused_objects.obj.at(i).rect.y; int rect_width = pedestrian_fused_objects.obj.at(i).rect.width; int rect_height = pedestrian_fused_objects.obj.at(i).rect.height; float range = pedestrian_fused_objects.obj.at(i).range; /* put label */ CvPoint labelOrg = cvPoint(rect_x - OBJ_RECT_THICKNESS, rect_y - baseline - OBJ_RECT_THICKNESS); cvRectangle(image_clone, cvPoint(labelOrg.x + 0, labelOrg.y + baseline), cvPoint(labelOrg.x + text_size.width, labelOrg.y - text_size.height), CV_RGB(0, 0, 0), // label background color is black -1, 8, 0 ); cvPutText(image_clone, objectLabel.data(), labelOrg, &dfont_label, CV_RGB(255, 255, 255) // label text color is white ); /* put distance data */ cvRectangle(image_clone, cv::Point(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 45), rect_y + rect_height + 5), cv::Point(rect_x + (rect_width/2) + (((int)log10(range/100)+1) * 8 + 38), rect_y + rect_height + 30), cv::Scalar(255,255,255), -1); cvInitFont (&dfont, CV_FONT_HERSHEY_COMPLEX , hscale, vscale, italicscale, thickness, CV_AA); sprintf(distance_string, "%.2f m", range / 100); //unit of length is meter cvPutText(image_clone, distance_string, cvPoint(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 40), rect_y + rect_height + 25), &dfont, CV_RGB(255, 0, 0)); } } /* * Show image */ if (cvGetWindowHandle(window_name.c_str()) != NULL) // Guard not to write destroyed window by using close button on the window { cvShowImage(window_name.c_str(), image_clone); cvWaitKey(2); } cvReleaseImage(&image_clone); }
static void putDistance(IplImage *Image, std::vector<autoware_msgs::ImageRectRanged> objects, int threshold_height, const char* objectLabel) { char distance_string[32]; CvFont dfont; float hscale = 0.7f; float vscale = 0.7f; float italicscale = 0.0f; int thickness = 1; CvFont dfont_label; float hscale_label = 0.5f; float vscale_label = 0.5f; CvSize text_size; int baseline = 0; cvInitFont(&dfont_label, CV_FONT_HERSHEY_COMPLEX, hscale_label, vscale_label, italicscale, thickness, CV_AA); cvGetTextSize(objectLabel, &dfont_label, &text_size, &baseline); for (unsigned int i=0; i<objects.size(); i++) { if (objects.at(i).rect.y > threshold_height) // temporal way to avoid drawing detections in the sky { if (!isNearlyNODATA(objects.at(i).range)) { /*put label */ CvPoint labelOrg = cvPoint(objects.at(i).rect.x - OBJ_RECT_THICKNESS, objects.at(i).rect.y - baseline - OBJ_RECT_THICKNESS); cvRectangle(Image, cvPoint(labelOrg.x + 0, labelOrg.y + baseline), cvPoint(labelOrg.x + text_size.width, labelOrg.y - text_size.height), CV_RGB(0, 0, 0), // label background is black -1, 8, 0 ); cvPutText(Image, objectLabel, labelOrg, &dfont_label, CV_RGB(255, 255, 255) // label text color is white ); /* put distance data */ cvRectangle(Image, cv::Point(objects.at(i).rect.x + (objects.at(i).rect.width/2) - (((int)log10(objects.at(i).range/100)+1) * 5 + 45), objects.at(i).rect.y + objects.at(i).rect.height + 5), cv::Point(objects.at(i).rect.x + (objects.at(i).rect.width/2) + (((int)log10(objects.at(i).range/100)+1) * 8 + 38), objects.at(i).rect.y + objects.at(i).rect.height + 30), cv::Scalar(255,255,255), -1); cvInitFont (&dfont, CV_FONT_HERSHEY_COMPLEX, hscale, vscale, italicscale, thickness, CV_AA); sprintf(distance_string, "%.2f m", objects.at(i).range / 100); //unit of length is meter cvPutText(Image, distance_string, cvPoint(objects.at(i).rect.x + (objects.at(i).rect.width/2) - (((int)log10(objects.at(i).range/100)+1) * 5 + 40), objects.at(i).rect.y + objects.at(i).rect.height + 25), &dfont, CV_RGB(255, 0, 0)); } } } }