$search
00001 #include <ros/ros.h> 00002 #include <std_msgs/String.h> 00003 #include <sensor_msgs/Image.h> 00004 #include <sensor_msgs/image_encodings.h> 00005 #include <cv_bridge/CvBridge.h> 00006 #include <vision_msgs/cop_answer.h> 00007 #include "odu_finder.h" 00008 00009 class ODUFinderNode : public ODUFinder 00010 { 00011 private: 00012 // NODE 00013 ros::NodeHandle node_handle; 00014 ros::Subscriber image_subscriber; 00015 ros::Publisher template_publisher; 00016 ros::Publisher image_pub_; 00017 // VISUALIZATION 00018 sensor_msgs::CvBridge bridge; 00019 std::string image_topic, template_topic; 00020 std::string output_image_topic_; 00021 public: 00022 ODUFinderNode(ros::NodeHandle &anode) 00023 : node_handle(anode) 00024 { 00025 //load or build database, equivalent to detect or build 00026 node_handle.param ("command", command, std::string("/load")); 00027 //path to database 00028 node_handle.param ("database_location", database_location, std::string("database/germandeli")); 00029 //path to training images 00030 node_handle.param ("images_folder", images_directory, std::string("data/germandeli")); 00031 //path to visualization images 00032 node_handle.param ("images_for_visualization_directory", images_for_visualization_directory, std::string("")); 00033 //input image topic 00034 node_handle.param ("image_topic", image_topic, std::string("/narrow_stereo/left/image_rect")); 00035 //template publisher for http://www.ros.org/wiki/cop 00036 node_handle.param ("template_topic", template_topic, std::string("TemplateName")); 00037 //how many best votes do we consider 00038 node_handle.param ("votes_count", votes_count, 10); 00039 //vocabulary trees parameters 00040 node_handle.param ("tree_k", tree_k, 5); 00041 node_handle.param ("tree_levels", tree_levels, 5); 00042 //min size of features when clustering 00043 node_handle.param ("min_cluster_size", min_cluster_size, 30); 00044 //when is object uknown (score goes from 0 (best) - 2 (worst) 00045 node_handle.param ("unknown_object_threshold", unknown_object_threshold, 0.3); 00046 //wait for #sliding_window_size frames before deciding on the classification result 00047 node_handle.param ("sliding_window_size", sliding_window_size, 10); 00048 //clustering yes or no 00049 node_handle.param ("enable_clustering", enable_clustering, 1); 00050 //acquire complete appearance models (suitable for e.g. images from germandeli) 00051 node_handle.param ("enable_incremental_learning", enable_incremental_learning, 0); 00052 //enable visualization 00053 node_handle.param ("enable_visualization", enable_visualization, 1); 00054 set_visualization(enable_visualization); 00055 //object id for http://www.ros.org/wiki/cop 00056 node_handle.param ("object_id", object_id, 700000); 00057 //radius adaptation parameters for clustering 00058 node_handle.param ("radius_adaptation_r_min", radius_adaptation_r_min, 200.0); 00059 node_handle.param ("radius_adaptation_r_max", radius_adaptation_r_max, 600.9); 00060 node_handle.param ("radius_adaptation_A", radius_adaptation_A, 800.0); 00061 node_handle.param ("radius_adaptation_K", radius_adaptation_K, 0.02); 00062 image_subscriber = node_handle.subscribe(image_topic, 100, &ODUFinderNode::image_callback, this); 00063 00064 //publish data for http://www.ros.org/wiki/cop 00065 template_publisher = node_handle.advertise<vision_msgs::cop_answer>(template_topic, 1); 00066 node_handle.param ("output_image_topic" , output_image_topic_ , std::string("object_found")); 00067 image_pub_ = node_handle.advertise<sensor_msgs::Image>(output_image_topic_,10); 00068 00069 //shall we extract roi around the keypoints?? 00070 node_handle.param ("extract_roi" , extract_roi_, false); 00071 00072 // number of templates to show in the visualisation 00073 node_handle.param ("templates_to_show", templates_to_show, 4); 00074 00075 // if logging of statistics shall be enabled 00076 node_handle.param ("enable_logging", enable_logging_, true); 00077 00078 if (enable_visualization) 00079 { 00080 cvNamedWindow("visualization", CV_WINDOW_AUTOSIZE); 00081 cvStartWindowThread(); 00082 } 00083 //for visualization of feature points 00084 color_table[0] = cvScalar(255, 0, 0); 00085 color_table[1] = cvScalar(0, 255, 0); 00086 color_table[2] = cvScalar(0, 0, 255); 00087 color_table[3] = cvScalar(255, 255, 0); 00088 color_table[4] = cvScalar(255, 0, 255); 00089 color_table[5] = cvScalar(0, 255, 255); 00090 color_table[6] = cvScalar(255, 255, 255); 00091 color_table[7] = cvScalar(125, 255, 255); 00092 color_table[8] = cvScalar(255, 125, 255); 00093 color_table[9] = cvScalar(255, 255, 125); 00094 color_table[10] = cvScalar(125, 125, 255); 00095 color_table[11] = cvScalar(255, 125, 125); 00096 color_table[12] = cvScalar(125, 255, 125); 00097 00098 } 00099 00100 private: 00101 void image_callback (const sensor_msgs::ImageConstPtr& received_image) 00102 { 00103 //if image is not de-bayered yet do it (needed for Prosilica image on PR2 robot) 00104 if (received_image->encoding.find("bayer") != std::string::npos) 00105 { 00106 cv::Mat tmpImage; 00107 const std::string& raw_encoding = received_image->encoding; 00108 int raw_type = CV_8UC1; 00109 if (raw_encoding == sensor_msgs::image_encodings::BGR8 || raw_encoding == sensor_msgs::image_encodings::RGB8) 00110 raw_type = CV_8UC3; 00111 const cv::Mat raw(received_image->height, received_image->width, raw_type, 00112 const_cast<uint8_t*>(&received_image->data[0]), received_image->step); 00113 // Convert to color BGR 00114 int code = 0; 00115 if (received_image->encoding == sensor_msgs::image_encodings::BAYER_RGGB8) 00116 code = CV_BayerBG2BGR; 00117 else if (received_image->encoding == sensor_msgs::image_encodings::BAYER_BGGR8) 00118 code = CV_BayerRG2BGR; 00119 else if (received_image->encoding == sensor_msgs::image_encodings::BAYER_GBRG8) 00120 code = CV_BayerGR2BGR; 00121 else if (received_image->encoding == sensor_msgs::image_encodings::BAYER_GRBG8) 00122 code = CV_BayerGB2BGR; 00123 else 00124 { 00125 ROS_ERROR("[image_proc] Unsupported encoding '%s'", received_image->encoding.c_str()); 00126 return; 00127 } 00128 cv::cvtColor(raw, tmpImage, code); 00129 00130 cv::Mat tmp2; 00131 cv::cvtColor(tmpImage, tmp2, CV_BGR2GRAY); 00132 camera_image = cvCreateImage(cvSize(800, 600), IPL_DEPTH_8U, 1); 00133 IplImage ti; 00134 ti = tmp2; 00135 cvSetImageROI(&ti, cvRect(300, 300, 1848, 1450)); 00136 cvResize(&ti, camera_image); 00137 } 00138 else 00139 { 00140 camera_image = bridge.imgMsgToCv(received_image, "mono8"); 00141 } 00142 printf("\n\n"); 00143 ROS_INFO("Image received!"); 00144 //call main processing function 00145 00146 if (camera_image->width == 2) 00147 { 00148 //if (visualization_mode_ == SEQUENCES && sequence_buffer.size() == 0) 00149 //return; 00150 00151 ROS_INFO("Special image for end of sequence detected! Switching to SEQUENCE mode!\n"); 00152 visualization_mode_ = SEQUENCES; 00153 00154 //std::sort(sequence_buffer.begin(), sequence_buffer.end()); 00155 00156 visualize(camera_image, NULL, NULL); 00157 00158 clear_sequence_buffer(); 00159 return; 00160 } 00161 00162 std::string result = process_image(camera_image); 00163 00164 //image_pub_.publish(bridge.cvToImgMsg(image_roi)); 00165 image_pub_.publish(bridge.cvToImgMsg(camera_image)); 00166 ROS_INFO("[ODUFinder:] image published to %s", output_image_topic_.c_str()); 00167 if (image_roi != NULL) 00168 cvReleaseImage(&image_roi); 00169 00170 //msg.data = name.substr(0, name.find_last_of('.')).c_str(); 00171 //publish the result to http://www.ros.org/wiki/cop 00172 if (result.length() > 0) 00173 { 00174 vision_msgs::cop_answer msg; 00175 vision_msgs::cop_descriptor cop_descriptor; 00176 vision_msgs::aposteriori_position aposteriori_position; 00177 msg.found_poses.push_back(aposteriori_position); 00178 msg.found_poses[0].models.push_back(cop_descriptor); 00179 //TODO: only one needed probably, ask Moritz 00180 msg.found_poses[0].models[0].type = "ODUFinder"; 00181 msg.found_poses[0].models[0].sem_class = result; 00182 msg.found_poses[0].models[0].object_id = ++object_id; 00183 msg.found_poses[0].position = 0; 00184 template_publisher.publish(msg); 00185 } 00186 } 00187 }; 00188 int main(int argc, char** argv) 00189 { 00190 ros::init(argc, argv, "odu_finder"); 00191 ros::NodeHandle node_handle("~"); 00192 ODUFinderNode odu_finder(node_handle); 00193 int result = odu_finder.start(); 00194 if(result != -1) 00195 ros::spin(); 00196 }