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