/// initialize tof camera viewer.
/// @return <code>false</code> on failure, <code>true</code> otherwise
bool init()
{
/// Create viewer windows
cvStartWindowThread();
cvNamedWindow("z data");
cvNamedWindow("gray data");
xyz_image_subscriber_ = image_transport_.subscribe("camera/xyz_tof_data", 1, &CobTofCameraViewerNode::xyzImageCallback, this);
grey_image_subscriber_ = image_transport_.subscribe("camera/grey_tof_data", 1, &CobTofCameraViewerNode::greyImageCallback, this);
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "image_listener");
ros::NodeHandle nh;
init();
cvNamedWindow("Lines");
cvStartWindowThread();
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("segmented", 1, imageCallback);
pub = nh.advertise<std_msgs::Int32MultiArray>("corners", 1);
ros::spin();
cvDestroyWindow("Lines");
}
int main(int argc, char *argv[]){
ros::init(argc, argv, "vision_analyzer");
FeatureTracker analyzer;
cvStartWindowThread();
/* cvNamedWindow("rectified image");
cvNamedWindow("calib image");
cvNamedWindow("threshold");
cvNamedWindow("edges");*/
ros::spin();
/* cvDestroyWindow("rectified image");
cvDestroyWindow("calib image");
cvDestroyWindow("threshold");
cvDestroyWindow("edges");*/
return 0;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "harris_corners");
dynamic_reconfigure::Server<harris_waitinglist::HarrisConfig> srv;
dynamic_reconfigure::Server<harris_waitinglist::HarrisConfig>::CallbackType f;
f = boost::bind(&callback, _1, _2);
srv.setCallback(f);
ros::NodeHandle nodeHandle("~");
nodeHandle.param("template_size",templateSize,int(3));
nodeHandle.param("threshold",threshold,double(0.1));
cvNamedWindow("Harris Corner Detection");
cvStartWindowThread();
image_transport::ImageTransport it(nodeHandle);
image_transport::Subscriber sub = it.subscribe("/image_original", 1, imageCallback);
ros::spin();
cvDestroyWindow("Harris Corner Detection");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "image_listener");
ros::NodeHandle nh;
ros::Rate loop_rate(1);
cvNamedWindow("view");
//while(nh.ok())
//{
ROS_INFO("getting image");
cvStartWindowThread();
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
ros::spin();
cvDestroyWindow("view");
loop_rate.sleep();
//}
return 0;
}
int main(int argc, char **argv)
{
if ( argc < 2 ) {
printf( "Usage: imagedisplay <topic>\n" );
return 0;
}
ros::init(argc, argv, "imagedisplay");
ros::NodeHandle nh;
cvNamedWindow("view");
cvStartWindowThread();
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe(argv[1], 1, imageCallback);
ros::spin();
cvDestroyWindow("view");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "evaluate_overlay_utias_dataset");
if (argc != 2)
{
ROS_WARN("WARNING: you should specify images folder path (on which GT and estimated poses/positions will be overlaid)! which might be %s\n, respectively",argv[1]);
}
else
{
printf("INFO: you have set camera images folder path: %s\n",argv[1]);
}
cvStartWindowThread();
ImageOverlayer node(argv[1]);
ros::spin();
return 0;
}
void Tools::enabledebugging()
{
debuggingmode = true;
cvStartWindowThread();
cvNamedWindow("HSV_Image", cv::WINDOW_AUTOSIZE);
cvNamedWindow("Editted_Image", cv::WINDOW_AUTOSIZE);
cvNamedWindow("Adjustors",0);
cvCreateTrackbar( "H_MIN", "Adjustors", &H_MIN, 256, 0 );
cvCreateTrackbar( "H_MAX", "Adjustors", &H_MAX, 256, 0 );
cvCreateTrackbar( "S_MIN", "Adjustors", &S_MIN, 256, 0 );
cvCreateTrackbar( "S_MAX", "Adjustors", &S_MAX, 256, 0 );
cvCreateTrackbar( "V_MIN", "Adjustors", &V_MIN, 256, 0 );
cvCreateTrackbar( "V_MAX", "Adjustors", &V_MAX, 256, 0 );
cvCreateTrackbar( "dp", "Adjustors", &dp, 5, 0 );
cvCreateTrackbar( "min_dist", "Adjustors", &min_dist, 400, 0 );
cvCreateTrackbar( "param_1", "Adjustors", ¶m_1, 300, 0 );
cvCreateTrackbar( "param_2", "Adjustors", ¶m_2, 300, 0 );
cvCreateTrackbar( "min_radius", "Adjustors", &min_radius, 100, 0 );
cvCreateTrackbar( "max_radius", "Adjustors", &max_radius, 400, 0 );
}
int main(int argc, char **argv)
{
/* create resizable window */
cvNamedWindow(window_name, CV_WINDOW_NORMAL);
cvStartWindowThread();
ros::init(argc, argv, "vscan_image_viewer");
ros::NodeHandle n;
ros::Subscriber sub_image = n.subscribe("image_raw", 1, image_cb);
ros::Subscriber sub_points = n.subscribe("vscan_image", 1, points_cb);
cv::Mat grayscale(256,1,CV_8UC1);
for(int i=0;i<256;i++)
{
grayscale.at<uchar>(i)=i;
}
cv::applyColorMap(grayscale,colormap,cv::COLORMAP_JET);
ros::spin();
cvDestroyWindow(window_name);
return 0;
}
DEFINE_THREAD_ROUTINE(video_stage, data)
{
C_RESULT res;
vp_api_io_pipeline_t pipeline;
vp_api_io_data_t out;
vp_api_io_stage_t stages[NB_STAGES];
vp_api_picture_t picture;
video_com_config_t icc;
vlib_stage_decoding_config_t vec;
vp_stages_yuv2rgb_config_t yuv2rgbconf;
#ifdef RECORD_VIDEO
video_stage_recorder_config_t vrc;
#endif
/// Picture configuration
picture.format = PIX_FMT_YUV420P;
picture.width = QVGA_WIDTH;
picture.height = QVGA_HEIGHT;
picture.framerate = 30;
picture.y_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT );
picture.cr_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 );
picture.cb_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 );
picture.y_line_size = QVGA_WIDTH;
picture.cb_line_size = QVGA_WIDTH / 2;
picture.cr_line_size = QVGA_WIDTH / 2;
vp_os_memset(&icc, 0, sizeof( icc ));
vp_os_memset(&vec, 0, sizeof( vec ));
vp_os_memset(&yuv2rgbconf, 0, sizeof( yuv2rgbconf ));
icc.com = COM_VIDEO();
icc.buffer_size = 100000;
icc.protocol = VP_COM_UDP;
COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip);
vec.width = QVGA_WIDTH;
vec.height = QVGA_HEIGHT;
vec.picture = &picture;
vec.block_mode_enable = TRUE;
vec.luma_only = FALSE;
yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24;
#ifdef RECORD_VIDEO
vrc.fp = NULL;
#endif
pipeline.nb_stages = 0;
stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET;
stages[pipeline.nb_stages].cfg = (void *)&icc;
stages[pipeline.nb_stages].funcs = video_com_funcs;
pipeline.nb_stages++;
#ifdef RECORD_VIDEO
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void*)&vrc;
stages[pipeline.nb_stages].funcs = video_recorder_funcs;
pipeline.nb_stages++;
#endif // RECORD_VIDEO
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void*)&vec;
stages[pipeline.nb_stages].funcs = vlib_decoding_funcs;
pipeline.nb_stages++;
stages[pipeline.nb_stages].type = VP_API_FILTER_YUV2RGB;
stages[pipeline.nb_stages].cfg = (void*)&yuv2rgbconf;
stages[pipeline.nb_stages].funcs = vp_stages_yuv2rgb_funcs;
pipeline.nb_stages++;
stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL;
stages[pipeline.nb_stages].cfg = NULL;
stages[pipeline.nb_stages].funcs = vp_stages_output_gtk_funcs;
pipeline.nb_stages++;
pipeline.stages = &stages[0];
/* Processing of a pipeline */
if( !ardrone_tool_exit() )
{
PRINT("Video stage thread initialization\n");
res = vp_api_open(&pipeline, &pipeline_handle);
// PRINT("VP_API OPENED\n");
if( SUCCEED(res) )
{
int loop = SUCCESS;
out.status = VP_API_STATUS_PROCESSING;
cvStartWindowThread();
#define frontWindow "DroneView"
cvNamedWindow(frontWindow, CV_WINDOW_AUTOSIZE);
frontImgStream = cvCreateImage(cvSize(picture.width, picture.height), IPL_DEPTH_8U, 3);
#define bottomWindow "BomberView"
if(extractBottomImage)
cvNamedWindow(bottomWindow, CV_WINDOW_AUTOSIZE);
bottomImgStream = cvCreateImage(cvSize(bottomWidth, bottomHeight), IPL_DEPTH_8U, 3);
IplImage *frame;
CvCapture *capture;
if(USEWEBCAM)
{
capture = cvCaptureFromCAM(WEBCAMINDEX);
if(!capture)
{
printf("ERROR: Cannot Initialize Webcam.\n");
loop = !SUCCESS;
}
}
while( !ardrone_tool_exit() && (loop == SUCCESS) )
{
if(!USEWEBCAM)
{
if( SUCCEED(vp_api_run(&pipeline, &out)) ) {
if (vec.controller.num_frames==0) continue;
/*int i;
for(i = 0; i < (picture.width)*(picture.height); i++)
{
frontImgStream->imageData[i*3] = picture.y_buf[i];
frontImgStream->imageData[i*3+1] = picture.y_buf[i];
frontImgStream->imageData[i*3+2] = picture.y_buf[i];
}
*/
cvCvtColor(rgbHeader, frontImgStream, CV_RGB2BGR); //[for colour]
if(extractBottomImage)
{
int j = 0, i;
for(i = 0; j < bottomHeight*bottomWidth; i = i%picture.width >= bottomWidth-1 ? i - (i%picture.width) + picture.width : i+1)
{
bottomImgStream->imageData[j*3] = frontImgStream->imageData[i*3];
bottomImgStream->imageData[j*3+1] = frontImgStream->imageData[i*3+1];
bottomImgStream->imageData[j*3+2] = frontImgStream->imageData[i*3+2];
frontImgStream->imageData[i*3] = 0;
frontImgStream->imageData[i*3+1] = 0;
frontImgStream->imageData[i*3+2] = 0;
j++;
}
}
//cvLine(frontImgStream, cvPoint(picture.width/2, 0), cvPoint(picture.width/2, picture.height), CV_RGB(0,255,0), 1, CV_AA, 0 );
cvShowImage(frontWindow, frontImgStream);
if(extractBottomImage)
cvShowImage(bottomWindow, bottomImgStream);
if(CAPTUREIMAGESTREAM)
{
char filename[256];
struct timeval t;
gettimeofday(&t, NULL);
sprintf(filename, "%d.%06d.jpg", (int)t.tv_sec, (int)t.tv_usec);
if(frontImgStream != NULL && cvSaveImage(filename, cvCloneImage(frontImgStream), 0))
printf("Image dumped to %s\n", filename);
else
printf("Error dumping image...\n");
if(extractBottomImage)
{
sprintf(filename, "%d.%06dbottom.jpg", (int)t.tv_sec, (int)t.tv_usec);
if(bottomImgStream != NULL && cvSaveImage(filename, cvCloneImage(bottomImgStream), 0))
printf("Bottom Image dumped to %s\n", filename);
else
printf("Error dumping bottom image...\n");
}
}
}
else loop = -1;
}
else //use webcam
{
frame = cvQueryFrame(capture);
if(!frame) break;
cvResize(frame, frontImgStream, CV_INTER_LINEAR);
cvShowImage(frontWindow, frontImgStream);
cvWaitKey(1);
}
}
cvDestroyWindow(frontWindow);
if(extractBottomImage)
cvDestroyWindow(bottomWindow);
//cvReleaseImage(&imgBottom);
//cvDestroyWindow(bottomWindow);
cvReleaseImage(&frontImgStream);
vp_api_close(&pipeline, &pipeline_handle);
}
}
PRINT(" Video stage thread ended\n\n");
return (THREAD_RET)0;
}
// main method
int main(int argc, char** argv) {
if (argc < 3) {
std::cout << "Usage: MapMerger <filename_1> <filename_2>\n";
exit(1);
}
std::cout << "----------MapsMerger---------\n";
// load maps
std::vector<Eigen::Vector3d> map1;
std::vector<Eigen::Vector3d> map2;
if (!loadMaps(argv[1], argv[2], &map1, &map2)) {
std::cout << "problems loading the map files\nExiting...\n";
exit(1);
}
// std::cout << "Map1:\n";
// for(unsigned int i=0; i<map1.size(); i++){
// std::cout << map1[i][0] << " " << map1[i][1] << "\n";
// }
std::cout << "Maps loaded\n";
double distance_threshold = computeDistanceTrigger(&map1, &map2);
std::cout << "distance threshold computed: " << distance_threshold << "\n";
std::vector<Eigen::Vector3d> totalmap;
Eigen::Matrix3d transformation;
unsigned int offset_index = mergemaps(&map1, &map2, &totalmap, &transformation, distance_threshold);
std::cout << "resulting POIs are in:\n";
for (unsigned int i = 0; i < totalmap.size(); i++) {
std::cout << totalmap[i][0] << " " << totalmap[i][1] << "\n";
}
// show images
cvStartWindowThread();
const char * map1_window_name = "MAP1";
const char * map2_window_name = "MAP2";
const char * totalmap_window_name = "TOTALMAP";
const char * associations_window_name = "ASSOCIATIONS";
cv::namedWindow(map1_window_name, CV_WINDOW_NORMAL);
cv::namedWindow(map2_window_name, CV_WINDOW_NORMAL);
cv::namedWindow(totalmap_window_name, CV_WINDOW_NORMAL);
cv::namedWindow(associations_window_name, CV_WINDOW_NORMAL);
cv::Mat * map1_image = drawPOI(&map1);
cv::Mat * map2_image = drawPOI(&map2);
cv::Mat * totalmap_image = drawPOI(&totalmap);
cv::Mat * associations_image = createAssociationsImage(map1_image, map2_image,
&map1, &map2, &transformation, distance_threshold, offset_index);
cv::imshow(map1_window_name, *map1_image);
cv::imshow(map2_window_name, *map2_image);
cv::imshow(totalmap_window_name, *totalmap_image);
cv::imshow(associations_window_name, *associations_image);
std::cout << "Displaying results, press ESC or Q to quit";
int button = cvWaitKey(0);
while((button!=27) && ((char)button!='q')){
button = cvWaitKey(0);
}
// delete transformation;
// delete map1_image;
// delete map2_image;
// delete totalmap_image;
// delete associations_image;
exit(0);
}
int cv::startWindowThread()
{
return cvStartWindowThread();
}
int main(int argc, char **argv)
{
/* create resizable window */
cvNamedWindow(window_name, CV_WINDOW_NORMAL);
cvStartWindowThread();
ros::init(argc, argv, "points_image_d_viewer");
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
std::string image_topic_name;
std::string car_node;
std::string pedestrian_node;
std::string points_node;
if (private_nh.getParam("image_raw_topic", image_topic_name)) {
ROS_INFO("Setting image topic to %s", image_topic_name.c_str());
} else {
ROS_INFO("No image topic received, defaulting to image_raw, you can use _image_raw_topic:=YOUR_NODE");
image_topic_name = "/image_raw";
}
if (private_nh.getParam("car_node", car_node)) {
ROS_INFO("Setting car positions node to %s", car_node.c_str());
} else {
ROS_INFO("No car positions node received, defaulting to car_pixel_xyz, you can use _car_node:=YOUR_TOPIC");
car_node = "/obj_car/image_obj_ranged";
}
if (private_nh.getParam("pedestrian_node", pedestrian_node)) {
ROS_INFO("Setting pedestrian positions node to %s", pedestrian_node.c_str());
} else {
ROS_INFO("No pedestrian positions node received, defaulting to pedestrian_pixel_xyz, you can use _pedestrian_node:=YOUR_TOPIC");
pedestrian_node = "/obj_person/image_obj_ranged";
}
if (private_nh.getParam("points_node", points_node)) {
ROS_INFO("Setting pedestrian positions node to %s", points_node.c_str());
} else {
ROS_INFO("No points node received, defaulting to points_image, you can use _points_node:=YOUR_TOPIC");
points_node = "/points_image";
}
#if (CV_MAJOR_VERSION == 3)
generateColors(_colors, 25);
#else
cv::generateColors(_colors, 25);
#endif
ros::Subscriber scriber = n.subscribe(image_topic_name, 1,
image_cb);
ros::Subscriber scriber_car = n.subscribe(car_node, 1,
car_updater_callback);
ros::Subscriber scriber_ped = n.subscribe(pedestrian_node, 1,
ped_updater_callback);
ros::Subscriber scriber_points = n.subscribe(points_node, 1,
points_cb);
cv::Mat grayscale(256,1,CV_8UC1);
for(int i=0;i<256;i++) {
grayscale.at<uchar>(i)=i;
}
cv::applyColorMap(grayscale,colormap,cv::COLORMAP_JET);
ros::spin();
cvDestroyWindow(window_name);
return 0;
}
int main(int argc, char* argv[])
{
// necessary variables..
int erode_level = 11;
char key='q';
char s_erode_level[20] ;
char img_text[]="erode level: ";
// image container...
IplImage* img;
// Font obj for setting text font...
CvFont font;
// initialize font object...
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX,
1.0, 1.0, 0, 1, CV_AA);
// starts the window thread...
// is necessary to destroy window...
cvStartWindowThread();
// do...while loop starts here
do{
img = cvLoadImage("03.jpg", CV_LOAD_IMAGE_UNCHANGED);
if (!img) {
printf("Error: Could not open the image file! \n");
exit(1);
}
// get erode level from user
printf("Enter the level of eroding: ");
scanf("%d", &erode_level);
// set limits 0-120
if(erode_level<=0)
erode_level=1;
else if(erode_level>120)
erode_level=119;
// erode the image...
cvErode(img, img, NULL,erode_level);
/*
*
* Similarly, dilation can be perfomed...
* cvDilate(img,img,NULL,erode_level);
*
*/
// write erode_level integer to String
sprintf(s_erode_level, "%d", erode_level) ;
// Concat "erode Level: " and s_erode_level(actual value)...
strcpy(img_text,"erode level: ");
strcat(img_text,s_erode_level);
// write text to image
cvPutText(img, img_text, cvPoint(10, 50),
&font, cvScalar(255, 255, 255, 0));
// display image in the "Eroded" window...
cvNamedWindow("Eroded", CV_WINDOW_AUTOSIZE);
cvShowImage("Eroded", img);
// save to eroded.jpg...
cvSaveImage("eroded.jpg", img, 0);
// get input from user
key = cvWaitKey(0);
// destroy window and release image...
cvDestroyWindow("Eroded");
cvReleaseImage( &img );
// if 'q' is pressed by user (key)
// break out of loop
// else continue...
}while(key!='q');
// end of do...while loop
return 0;
}// end of main
int cv::startWindowThread()
{
CV_TRACE_FUNCTION();
return cvStartWindowThread();
}
int main(int argc, char* argv[])
{
IplImage* src = cvLoadImage(argv[1],CV_LOAD_IMAGE_UNCHANGED);
// get HSV form of image
IplImage* hsvSrc = cvCreateImage(cvGetSize(src),8,3);
cvCvtColor(src,hsvSrc,CV_BGR2HSV);
// initialize Histogram
int numBins = 256;
CvHistogram *hist = cvCreateHist(1,&numBins,CV_HIST_ARRAY,NULL,1);
cvClearHist(hist);
// Separate hsv image into 3 channels
IplImage* hueCh = cvCreateImage(cvGetSize(hsvSrc),8,1);
IplImage* satCh = cvCreateImage(cvGetSize(hsvSrc),8,1);
IplImage* valCh = cvCreateImage(cvGetSize(hsvSrc),8,1);
cvSplit(hsvSrc,hueCh,satCh,valCh,NULL);
// **** Rendering Histogram ****
// --- Hue Channel ---
cvCalcHist(&hueCh,hist, 0, NULL);
IplImage* imgHistHue = drawHistogram(hist);
cvClearHist(hist);
// --- Sat Channel ---
cvCalcHist(&satCh, hist, 0, NULL);
IplImage* imgHistSat = drawHistogram(hist);
cvClearHist(hist);
// --- Val Channel ---
cvCalcHist(&valCh, hist, 0, NULL);
IplImage* imgHistVal = drawHistogram(hist);
cvClearHist(hist);
cvStartWindowThread();
// display histogram
cvNamedWindow("Hue",CV_WINDOW_NORMAL);
cvNamedWindow("Sat",CV_WINDOW_NORMAL);
cvNamedWindow("Val",CV_WINDOW_NORMAL);
cvShowImage("Hue",imgHistHue);
cvShowImage("Sat",imgHistSat);
cvShowImage("Val",imgHistVal);
// wait for key press
cvWaitKey(0);
// release memory
cvDestroyAllWindows();
cvReleaseImage(&src);
cvReleaseImage(&hueCh);
cvReleaseImage(&satCh);
cvReleaseImage(&valCh);
cvReleaseImage(&imgHistHue);
cvReleaseImage(&imgHistSat);
cvReleaseImage(&imgHistVal);
return 0;
}// end of main
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
//bridge that will transform the message (image) from ROS code back to "image" code
sensor_msgs::CvBridge bridge;
//publish data (obstacle waypoints) back to the boat
ros::NodeHandle n;
//std_msgs::Float32 xWaypoint_msg; // X coordinate obstacle message
//std_msgs::Float32 yWaypoint_msg; // Y coordinate obstacle message
//publish the waypoint data
//ros::Publisher waypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//ros::Publisher Ywaypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//std::stringstream ss;
/***********************************************************************/
//live image coming streamed straight from the boat's camera
IplImage* boatFront = bridge.imgMsgToCv(msg, "bgr8");
IplImage* backUpImage = bridge.imgMsgToCv(msg, "bgr8");
boatFront->origin = IPL_ORIGIN_TL; //sets image origin to top left corner
//Crop the image to the ROI
cvSetImageROI(boatFront, cvRect(0,0,boatFront->height/0.5,boatFront->width/1.83));
int X = boatFront->height;
int Y = boatFront->width;
/***********************************************************************/
//boat's edge distance from the camera. This is used for visual calibration
//to know the distance from the boat to the nearest obstacles.
//With respect to the mounted camera, distance is 21 inches (0.5334 m) side to side
//and 15 inches (0.381 m).
//float boatFrontDistance = 0.381; //distance in meters
//float boatSideDistance = 0.5334; //distance in meters
// These variables tell the distance from the center bottom of the image
// (the camera) to the square surrounding a the obstacle
float xObstacleDistance = 0.0;
float yObstacleDistance = 0.0;
float obstacleDistance = 0.0;
int pixelsNumber = 6; //number of pixels for an n x n matrix and # of neighbors
const int arraySize = pixelsNumber;
const int threeArraySize = pixelsNumber;
//if n gets changed, then the algorithm might have to be
//recalibrated. Try to keep it constant
//these variables are used for the k nearest neighbors
//int accuracy;
//reponses for each of the classifications
float responseWaterH, responseWaterS, responseWaterV;
float responseGroundH, responseGroundS, responseGroundV;
//float responseSkyH, responseSkyS, responseSkyV;
float averageHue = 0.0;
float averageSat = 0.0;
float averageVal = 0.0;
CvMat* trainClasses = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
CvMat* trainClasses2 = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
for (int i = 0; i < pixelsNumber/2; i++)
{
cvmSet(trainClasses, i,0,1);
cvmSet(trainClasses2, i,0,1);
}
for (int i = pixelsNumber/2; i < pixelsNumber; i++)
{
cvmSet(trainClasses, i,0,2);
cvmSet(trainClasses2, i,0,2);
}
//for (int i =0; i<pixelsNumber;i++)
//{
// cout << cvmGet(trainClasses,i,0);
// cout << cvmGet(trainClasses2,i,0);
//}
//CvMat sample = cvMat( 1, 2, CV_32FC1, _sample );
//used with the classifier
CvMat* nearestWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* nearestSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* nearestSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* nearestSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//Distance
//CvMat* distanceWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//these variables are use to traverse the picture by blocks of n x n pixels at
//a time.
//Index(0,0) does not exist, so make sure kj and ki start from 1 (in the
//right way, of course)
//x and y are the dimensions of the local patch of pixels
int x = (boatFront->height)/2.5 + pixelsNumber + 99;
int y = pixelsNumber-1;
int ix = 0;
int iy = 0;
int skyX = 0;
int skyY = 0;
//M controls the x axis (up and down); N controls the y axis (left and
//right)
int Mw = -550;
int Nw = 1300;
int Mg = -350;
int Ng = 700;
int row1 = 0;
int column1 = 0;
int row2 = 0;
int column2 = 0;
//ground sample
CvMat* groundTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* groundTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* groundTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//water sample
CvMat* waterTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//n x n sample patch taken from the picture
CvMat* sampleHue = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleSat = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleVal = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* resampleHue = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* resampleSat = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* resampleVal = cvCreateMat(arraySize,arraySize,CV_32FC1);
//sky training sample
CvMat* skyTrainingHue = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingSat = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingVal = cvCreateMat(arraySize,arraySize,CV_32FC1);
//initialize each matrix element to zero for ease of use
cvZero(groundTrainingHue);
cvZero(groundTrainingSat);
cvZero(groundTrainingVal);
cvZero(waterTrainingHue);
cvZero(waterTrainingSat);
cvZero(waterTrainingVal);
cvZero(sampleHue);
cvZero(sampleSat);
cvZero(sampleVal);
cvZero(resampleHue);
cvZero(resampleSat);
cvZero(resampleVal);
cvZero(skyTrainingHue);
cvZero(skyTrainingSat);
cvZero(skyTrainingVal);
//Stores the votes for each channel (whether it belongs to water or not
//1 is part of water, 0 not part of water
//if sum of votes is bigger than 1/2 the number of elements, then it belongs to water
int votesSum = 0;
int comparator[3]; //used when only three votes are needed
//int comparatorTwo [3][3]; //used when six votes are needed
//initial sum of votes is zero
//Error if initialize both matrices inside a single for loop. Dont know why
//for(int i = 0; i < 3; i++)
//{
//comparator[i] = 0;
// for(int j = 0; j < 3; j++)
// {
// comparatorTwo[i][j] = 0;
// }
//}
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
/***********************************************************************/
//Convert from RGB to HSV to control the brightness of the objects.
//work with reflexion
/*Sky recognition. Might be useful for detecting reflexion on the water. If
the sky is detected, and the reflection has the same characteristics of
something below the horizon, that "something" might be water. Assume sky
wont go below the horizon
*/
//convert from RGB to HSV
cvCvtColor(boatFront, boatFront, CV_BGR2HSV);
cvCvtColor(backUpImage, backUpImage, CV_BGR2HSV);
HsvImage I(boatFront);
HsvImage IBackUp(backUpImage);
//Sky detection
for (int i=0; i<boatFront->height/3;i++)
{
for (int j=0; j<boatFront->width;j++)
{
//if something is bright enough, consider it sky and store the
//value. HSV values go from 0 to 180 ... RGB goes from 0 to 255
if (((I[i][j].v >= 180) && (I[i][j].s <= 16)))
// && ((I[i][j].h >=10)))) //&& (I[i][j].h <= 144))))
{
//The HSV values vary between 0 and 1
cvmSet(skyTrainingHue,skyX,skyY,I[i][j].h);
cvmSet(skyTrainingSat,skyX,skyY,I[i][j].s);
cvmSet(skyTrainingVal,skyX,skyY,I[i][j].v);
I[i][j].h = 0.3*180; //H (color)
I[i][j].s = 0.3*180; //S (color intensity)
I[i][j].v = 0.6*180; //V (brightness)
if (skyY == pixelsNumber-1)
{
if (skyX == pixelsNumber-1)
skyX = 1;
else
skyX = skyX + 1;
skyY = 1;
}
else
skyY = skyY + 1;
}
}
}
/***********************************************************************/
//offline input pictures. Samples of water properties are taken from these
//pictures to get a range of values for H, S, V that will be stored into a
//pre-defined classifier
IplImage* imageSample1 = cvLoadImage("bigObstacle.jpg");
cvSetImageROI(imageSample1, cvRect(0,0,imageSample1->height/0.5,imageSample1->width/1.83));
cvCvtColor(imageSample1, imageSample1, CV_BGR2HSV);
HsvImage I1(imageSample1);
IplImage* imageSample2 = cvLoadImage("bigObstacle2.jpg");
cvSetImageROI(imageSample2, cvRect(0,0,imageSample2->height/0.5,imageSample2->width/1.83));
cvCvtColor(imageSample2, imageSample2, CV_BGR2HSV);
HsvImage I2(imageSample2);
IplImage* imageSample3 = cvLoadImage("bigObstacle3.jpg");
cvSetImageROI(imageSample3, cvRect(0,0,imageSample3->height/0.5,imageSample3->width/1.83));
cvCvtColor(imageSample3, imageSample3, CV_BGR2HSV);
HsvImage I3(imageSample3);
IplImage* imageSample4 = cvLoadImage("river.jpg");
cvSetImageROI(imageSample4, cvRect(0,0,imageSample4->height/0.5,imageSample4->width/1.83));
cvCvtColor(imageSample4, imageSample4, CV_BGR2HSV);
HsvImage I4(imageSample4);
IplImage* imageSample5 = cvLoadImage("river2.jpg");
cvSetImageROI(imageSample5, cvRect(0,0,imageSample5->height/0.5,imageSample5->width/1.83));
cvCvtColor(imageSample5, imageSample5, CV_BGR2HSV);
HsvImage I5(imageSample5);
IplImage* imageSample6 = cvLoadImage("roundObstacle4.jpg");
cvSetImageROI(imageSample6, cvRect(0,0,imageSample6->height/0.5,imageSample6->width/1.83));
cvCvtColor(imageSample6, imageSample6, CV_BGR2HSV);
HsvImage I6(imageSample6);
IplImage* imageSample7 = cvLoadImage("farm.jpg");
cvSetImageROI(imageSample7, cvRect(0,0,imageSample7->height/0.5,imageSample7->width/1.83));
cvCvtColor(imageSample7, imageSample7, CV_BGR2HSV);
HsvImage I7(imageSample7);
IplImage* imageSample8 = cvLoadImage("bigObstacle4.jpg");
cvSetImageROI(imageSample8, cvRect(0,0,imageSample8->height/0.5,imageSample8->width/1.83));
cvCvtColor(imageSample8, imageSample8, CV_BGR2HSV);
HsvImage I8(imageSample8);
IplImage* imageSample9 = cvLoadImage("roundObstacle6.jpg");
cvSetImageROI(imageSample9, cvRect(0,0,imageSample9->height/0.5,imageSample9->width/1.83));
cvCvtColor(imageSample9, imageSample9, CV_BGR2HSV);
HsvImage I9(imageSample9);
IplImage* imageSample10 = cvLoadImage("roundObstacle.jpg");
cvSetImageROI(imageSample10, cvRect(0,0,imageSample10->height/0.5,imageSample10->width/1.83));
cvCvtColor(imageSample10, imageSample10, CV_BGR2HSV);
HsvImage I10(imageSample10);
//grab water samples from each picture
for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
row1 = ceil(X/1.2866)+ceil(X/5.237)+i+Mw;
column1 = ceil(Y/7.0755)+ceil(Y/21.01622)+j+Nw;
averageHue = (I1[row1][column1].h + I2[row1][column1].h + I3[row1][column1].h + I4[row1][column1].h + I5[row1][column1].h +
I6[row1][column1].h + I7[row1][column1].h + I8[row1][column1].h + I9[row1][column1].h + I10[row1][column1].h) / 10;
averageSat = (I1[row1][column1].s + I2[row1][column1].s + I3[row1][column1].s + I4[row1][column1].s + I5[row1][column1].s +
I6[row1][column1].s + I7[row1][column1].s + I8[row1][column1].s + I9[row1][column1].s + I10[row1][column1].s) / 10;
averageVal = (I1[row1][column1].v + I2[row1][column1].v + I3[row1][column1].v + I4[row1][column1].v + I5[row1][column1].v +
I6[row1][column1].v + I7[row1][column1].v + I8[row1][column1].v + I9[row1][column1].v + I10[row1][column1].v) / 10;
//water patch sample (n X n matrix)
cvmSet(waterTrainingHue,i,j,averageHue);
cvmSet(waterTrainingSat,i,j,averageSat);
cvmSet(waterTrainingVal,i,j,averageVal);
//patch is red (this is for me to know where the ground patch sample is)
//I[row1][column1].h = 0;
//I[row1][column1].s = 255;
//I[row1][column1].v = 255;
}
}
//order the water samples in ascending order on order to know a range
cvSort(waterTrainingHue, waterTrainingHue, CV_SORT_ASCENDING);
cvSort(waterTrainingSat, waterTrainingSat, CV_SORT_ASCENDING);
cvSort(waterTrainingVal, waterTrainingVal, CV_SORT_ASCENDING);
// find the maximum and minimum values in the array to create a range
int maxH = cvmGet(waterTrainingHue,0,0);
int maxS = cvmGet(waterTrainingSat,0,0);
int maxV = cvmGet(waterTrainingVal,0,0);
int minH = cvmGet(waterTrainingHue,0,0);
int minS = cvmGet(waterTrainingSat,0,0);
int minV = cvmGet(waterTrainingVal,0,0);
for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
if (cvmGet(waterTrainingHue,i,j) > maxH)
maxH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) > maxS)
maxS = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingVal,i,j) > maxV)
maxV = cvmGet(waterTrainingVal,i,j);
if (cvmGet(waterTrainingHue,i,j) < minH)
minH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) < minS)
minS = cvmGet(waterTrainingSat,i,j);
if (cvmGet(waterTrainingVal,i,j) < minV)
minV = cvmGet(waterTrainingVal,i,j);
}
}
/***********************************************************************/
//Grab a random patch of water below the horizon and compare every other
//pixel against it
//The results of the water detection depend on where in the picture the
//training samples are located. Maybe adding more training samples will
//help improve this?
/*
for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
row2 = ceil(X/4.7291)+ceil(X/8.3176)+i+Mg;
column2 = ceil(Y/7.78378)+ceil(Y/16.54468)+j+Ng;
//ground patch sample (n X n matrix)
//Detecting the horizon in the picture might be an excellent visual aid to
//choose where (above the horizon) you can take a ground training(1:3*n,1:n)g sample
//from. The ground pixel sample can be at a constant distance from the
//horizon
cvmSet(groundTrainingHue,i,j,I[row2][column2].h);
cvmSet(groundTrainingSat,i,j,I[row2][column2].s);
cvmSet(groundTrainingVal,i,j,I[row2][column2].v);
//patch is red (this is for me to know where the ground patch sample is)
I[row2][column2].h = 60;
I[row2][column2].s = 180;
I[row2][column2].v = 90;
}
}
//order the water samples in ascending order on order to know a range
cvSort(groundTrainingHue, groundTrainingHue, CV_SORT_ASCENDING);
cvSort(groundTrainingSat, groundTrainingSat, CV_SORT_ASCENDING);
cvSort(groundTrainingVal, groundTrainingVal, CV_SORT_ASCENDING);
*/
// Main loop. It traverses through the picture
//skyX = 0;
//skyY = 0;
while (x < boatFront->height/1.158)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i<pixelsNumber;i++)
{
cvmSet(sampleHue,0,i,I[x][y].h);
cvmSet(sampleSat,0,i,I[x][y].s);
cvmSet(sampleVal,0,i,I[x][y].v);
}
//Find the shortest distance between a pixel and the neighbors from each of
//the training samples (sort of inefficient, but might do the job...sometimes)
//if (ix == pixelsNumber-1)
//{
//HSV for water sample
// learn classifier
//CvKNearest knn(trainData, trainClasses, 0, false, itemsNumber);
//CvKNearest knnWaterHue(waterTrainingHue, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnWaterSat(waterTrainingSat, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnWaterVal(waterTrainingVal, trainClasses, 0, false, pixelsNumber);
//HSV for ground sample
//CvKNearest knnGroundHue(groundTrainingHue, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundSat(groundTrainingSat, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundVal(groundTrainingVal, trainClasses2, 0, false, pixelsNumber);
//HSV for sky sample
//if (cvmGet(skyTrainingHue,0,0)!=0.0 && cvmGet(skyTrainingSat,0,0)!=0.0 && cvmGet(skyTrainingVal,0,0)!=0.0)
//{
// CvKNearest knnSkyHue(skyTrainingHue, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnSkySat(skyTrainingSat, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnSkyVal(skyTrainingVal, trainClasses, 0, false, pixelsNumber);
//}
//scan nearest neighbors to each pixel
//responseWaterH = knnWaterHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestWaterH,0);
//responseWaterS = knnWaterSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestWaterS,0);
//responseWaterV = knnWaterVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestWaterV,0);
//responseGroundH = knnGroundHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestGroundH,0);
//responseGroundS = knnGroundSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestGroundS,0);
//responseGroundV = knnGroundVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestGroundV,0);
for (int i=0;i<pixelsNumber;i++)
{
for (int j=0;j<pixelsNumber;j++)
{
if ((minH <= cvmGet(sampleHue,0,j)) || (maxH >= cvmGet(sampleHue,0,j)))
//mark water samples as green
comparator[0] = 1;
else
comparator[0] = 0;
if (((minS <= cvmGet(sampleSat,0,j)) || (maxS <= cvmGet(sampleSat,0,j))))
//mark water samples as green
comparator[1] = 1;
else
comparator[1] = 0;
if ((minV <= cvmGet(sampleVal,0,j)) || (maxV <= cvmGet(sampleVal,0,j)))
//mark water samples as green
comparator[2] = 1;
else
comparator[2] = 0;
//count votes
for (int i3=0; i3 < 3; i3++)
votesSum = votesSum + comparator[i3];
//sky detection
if (votesSum > 1) //&& ((sampleSat[i][j] - sampleVal[i][j]) <= 0.1*180)
{
// classify pixel as water
I[x-pixelsNumber+i][y-pixelsNumber+j].h = 0;
I[x-pixelsNumber+i][y-pixelsNumber+j].s = 255;
I[x-pixelsNumber+i][y-pixelsNumber+j].v = 255;
}
votesSum = 0;
}
}
if (y < Y-1)
y = y + pixelsNumber-1;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
x = x + pixelsNumber-1;
y = pixelsNumber-1;
}
//ix = 0;
}
//traverse through the image one more time, divide the image in grids of
// 500x500 pixels, and see how many pixels of water are in each grid. If
// most of the pixels are labeled water, then mark all the other pixels
// as water as well
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
//int counter = 0;
int xDivisor = 20;
int yDivisor = 20;
votesSum = 0;
column1 = 0;
row1 = 0;
x = ceil(boatFront->height/2.5);
obstacleDistance = x;
y = 0;
int counter = 0;
while (x < boatFront->height/1.2)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i < ceil(boatFront->height/xDivisor); i++)
{
for(int j = 0; j < ceil(boatFront->width/yDivisor); j++)
{
cvmSet(resampleHue,i,j,I[x+i][y+j].h);
cvmSet(resampleSat,i,j,I[x+i][y+j].s);
cvmSet(resampleVal,i,j,I[x+i][y+j].v);
if(cvmGet(resampleHue,i,j)==0 && cvmGet(resampleSat,i,j)==255 && cvmGet(resampleVal,i,j)==255)
{
votesSum++;
}
}
}
if (votesSum > ((boatFront->height/xDivisor)*(boatFront->width/yDivisor)*(8.9/9)))
{
// if bigger than 4/5 the total number of pixels in a square, then consider the entire thing as water
// We might need to use other smaller quantities (like 5/6 maybe?)
for (int i = 0; i < ceil(boatFront->height/xDivisor);i++)
{
for (int j = 0; j < ceil(boatFront->width/yDivisor); j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
I[row1][column1].h = 0;
I[row1][column1].s = 255;
I[row1][column1].v = 255;
}
}
}
else
{
// If not water, eliminate all red pixels and turn those pixels
// back to the original color. These pixels shall, then, be marked
// as obstacles
for (int i = 0; i < ceil(boatFront->height/xDivisor);i++)
{
for (int j = 0; j < ceil(boatFront->width/yDivisor); j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
//the darker the color, the closer the object to the boat
I[row1][column1].h = 128;
I[row1][column1].s = 255;
I[row1][column1].v = 255 - counter;
//I[row1][column1].h = IBackUp[row1][column1].h;
//I[row1][column1].s = IBackUp[row1][column1].s;
//I[row1][column1].v = IBackUp[row1][column1].v;
//counter = counter + 20;
}
}
//The distance formula calculated by plotting points is given by:
/*********** distance = (1.76e-11)*pow(pixels,3.99) *****************/
/*********** pixel = (513.9332077469)pow(distance,0.240675506 *****************/
// Convert from pixel distance to normal distance in meters
if(obstacleDistance > sqrt(pow(xObstacleDistance,2) + pow(yObstacleDistance,2)))
{
// x,y coordinates of the obstacle
xObstacleDistance = (1.76e-11)*pow(((boatFront->height/xDivisor)+x)/2, 3.99) ;
yObstacleDistance = (1.76e-11)*pow(((boatFront->width/yDivisor)+y)/2, 3.99);
//xWaypoint_msg = xObstacleDistance;
//yWaypoint_msg = yObstacleDistance;
//publish position data
//waypoint_info_pub.publish(xWaypoint_msg);
//waypoint_info_pub.publish(yWaypoint_msg);
ROS_INFO("Obstacle coordinates: X = %f meters, Y = %f meters", xObstacleDistance, yObstacleDistance);
obstacleDistance = sqrt(pow(xObstacleDistance,2) + pow(yObstacleDistance,2));
ROS_INFO("Obstacle distance from: %f", obstacleDistance);
}
//cout << "Distance to Obstacle is: " << obstacleDistance << endl << endl;
}
y = y + boatFront->width/xDivisor;
if (y > Y-1)
{
x = x + boatFront->height/yDivisor;
y = 0;
counter = counter + 30;
}
votesSum = 0;
}
cvCvtColor(boatFront, boatFront, CV_HSV2BGR);
cvCvtColor(backUpImage, backUpImage, CV_HSV2BGR);
/**************************************************************************/
try
{
cvStartWindowThread();
cvNamedWindow("Boat Front",0); //0 to maintains sizes regardless of image size
cvResizeWindow("Boat Front",400,250); // new width/heigh in pixels
cvShowImage("Boat Front", boatFront);
cvResetImageROI(boatFront);
cvReleaseImage(&boatFront);
cvResetImageROI(backUpImage);
cvReleaseImage(&backUpImage);
cvReleaseMat(&trainClasses);
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line. For programmatic
* remappings you can use a different version of init() which takes remappings
* directly, but for most command-line programs, passing argc and argv is the easiest
* way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "image_d_viewer");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
/**
* The subscribe() call is how you tell ROS that you want to receive messages
* on a given topic. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. Messages are passed to a callback function, here
* called Callback. subscribe() returns a Subscriber object that you
* must hold on to until you want to unsubscribe. When all copies of the Subscriber
* object go out of scope, this callback will automatically be unsubscribed from
* this topic.
*
* The second parameter to the subscribe() function is the size of the message
* queue. If messages are arriving faster than they are being processed, this
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
std::string image_topic;
std::string car_topic;
std::string person_topic;
if (private_nh.getParam("image_raw_topic", image_topic)) {
ROS_INFO("Setting image topic to %s", image_topic.c_str());
} else {
ROS_INFO("No image topic received, defaulting to image_raw, you can use _image_raw_topic:=YOUR_NODE");
image_topic = "/image_raw";
}
if (!private_nh.getParam("car_topic", car_topic)) {
car_topic = "/obj_car/image_obj_ranged";
}
if (!private_nh.getParam("person_topic", person_topic)) {
person_topic = "/obj_person/image_obj_ranged";
}
std::string name_space_str = ros::this_node::getNamespace();
if (name_space_str != "/") {
window_name = std::string(window_name_base) + " (" + ros::this_node::getNamespace() + ")";
}
else {
window_name = std::string(window_name_base);
}
cvNamedWindow(window_name.c_str(), 2);
cvStartWindowThread();
image = NULL;
car_fused_objects.obj.clear();
pedestrian_fused_objects.obj.clear();
ros::Subscriber image_sub = n.subscribe(image_topic, 1, imageCallback);
ros::Subscriber obj_car_sub = n.subscribe(car_topic, 1, obj_carCallback);
ros::Subscriber obj_person_sub = n.subscribe(person_topic, 1, obj_personCallback);
/**
* ros::spin() will enter a loop, pumping callbacks. With this version, all
* callbacks will be called from within this thread (the main one). ros::spin()
* will exit when Ctrl-C is pressed, or the node is shutdown by the master.
*/
ros::spin();
cvDestroyWindow(window_name.c_str());
return 0;
}
int hue_circles_detector(const sensor_msgs::ImageConstPtr&
InputImage, std::vector<Circle> &myCircles)
{
IplImage *myImage = NULL;
cv::Mat inputImage;
cv::Mat hsvImage;
cv::Mat hueImage;
cv::Mat satImage;
cv::Mat binaryImage;
cv::Mat outputImage;
sensor_msgs::CvBridge bridge;
try
{
myImage = bridge.imgMsgToCv(InputImage, "bgr8");
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", InputImage->encoding.c_str());
return -1;
}
cvNamedWindow("Input to hue circles detector");
cvNamedWindow("Circles");
cvNamedWindow("Working image");
cvStartWindowThread();
cvShowImage("Input to hue circles detector", myImage);
IplImage* CirclesImage = cvCloneImage(myImage); // just used to display our found circles
// create memory storage that will contain all the dynamic data
CvMemStorage* storage = 0;
storage = cvCreateMemStorage(0);
// Convert IplImage to cv::Mat
inputImage = cv::Mat (myImage).clone ();
// output = input
outputImage = inputImage.clone ();
// Convert Input image from BGR to HSV
cv::cvtColor (inputImage, hsvImage, CV_BGR2HSV);
// Zero Matrices
hueImage = cv::Mat::zeros(hsvImage.rows, hsvImage.cols, CV_8U);
satImage = cv::Mat::zeros(hsvImage.rows, hsvImage.cols, CV_8U);
binaryImage = cv::Mat::zeros(hsvImage.rows, hsvImage.cols, CV_8U);
// HSV Channel 0 -> hueImage & HSV Channel 1 -> satImage
int from_to[] = { 0,0, 1,1};
cv::Mat img_split[] = { hueImage, satImage};
cv::mixChannels(&hsvImage, 3,img_split,2,from_to,2);
// ****************************************************
// NOTE that i is ROWS and j is COLS
// This is not x,y, it is rows and cols
// 0,0 is upper left; 0, max is upper right; max, 0 is lower left; max,max is lower right
// ****************************************************
for(int i = 0; i < outputImage.rows; i++)
{
for(int j = 0; j < outputImage.cols; j++)
{
// The output pixel is white if the input pixel
// hue is green and saturation is reasonable
// in low light the tennis ball has a hue around 160 - 180, saturations around 30 to 40
// in normal light it has a hue around 20 - 40, saturations around 80 - 150
// in very high light, the tennis ball center goes full white, so saturation drops to <10
// in general, the background doesn't seem to come up too much with a very loose constraint
// on saturation, so it works to allow mostly any saturation value.
if( (hueImage.at<uchar>(i,j) > 20 && hueImage.at<uchar>(i,j) < 40 && satImage.at<uchar>(i,j) > 5))
// || (hueImage.at<uchar>(i,j) > 160 && hueImage.at<uchar>(i,j) < 180 && satImage.at<uchar>(i,j) > 20 ))
binaryImage.at<uchar>(i,j) = 255;
else {
binaryImage.at<uchar>(i,j) = 0;
// Clear pixel blue output channel
outputImage.at<uchar>(i,j*3+0) = 0;
// Clear pixel green output channel
outputImage.at<uchar>(i,j*3+1) = 0;
// Clear pixel red output channel
outputImage.at<uchar>(i,j*3+2) = 0;
}
}
}
cv::Size strel_size;
strel_size.width = 3;
strel_size.height = 3;
cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE,strel_size);
cv::morphologyEx(binaryImage, binaryImage,cv::MORPH_OPEN,strel,cv::Point(-1, -1),3);
// Convert White on Black to Black on White by inverting the image
cv::bitwise_not(binaryImage,binaryImage);
// Blur the image to improve detection
cv::GaussianBlur(binaryImage, binaryImage, cv::Size(7, 7), 2, 2 );
cv::imshow ("Input to hue circles detector", inputImage);
// Display Binary Image
cv::imshow ("Working image", binaryImage);
// Display segmented image
//cv::imshow ("Circles", outputImage);
//start drawing all the circles
IplImage InputToHough = binaryImage;
CvSeq* circles =
cvHoughCircles( &InputToHough, storage, CV_HOUGH_GRADIENT, 1, 70, 140, 15, 20, 400);
//cvHoughCircles( &InputToHough, storage, CV_HOUGH_GRADIENT, 2, InputToHough.height/50, MIN_RADIUS, MAX_RADIUS );
//output all the circles detected
int NumCircles = circles->total;
//cout << "\n\nFound " << NumCircles << " circles" << endl;
if (NumCircles > MAX_CIRCLES) NumCircles = MAX_CIRCLES + 1; // so we don't print out too many
for( int i = 0; i < NumCircles; i++ ){
float* p = (float*)cvGetSeqElem( circles, i );
//cout << "x = " << p[0] << ", y = " << p[1]
// << ", radius = " << p[2] << endl;
cvCircle(CirclesImage,
cvPoint(cvRound(p[0]),
cvRound(p[1])),
cvRound(p[2]),
CV_RGB(0,255,0), 2, 8, 0 );
Circle myLocalCircle;
int myColor = 0, actualThreshold = 100;
myLocalCircle.setValues(cvRound(p[0]), cvRound(p[1]), cvRound(p[2]), myColor, actualThreshold);
myCircles.push_back(myLocalCircle);
} // ends for NumCircles
cvShowImage("Circles", CirclesImage);
return NumCircles;
}
