heat_detection.cpp
Go to the documentation of this file.
00001 #include "heat_detection.h"
00002 
00003 
00004 HeatDetection::HeatDetection(){
00005     image_count_ = 0;
00006 
00007     ros::NodeHandle n;
00008     ros::NodeHandle p_n("~");//private nh
00009     image_transport::ImageTransport it(n);
00010     image_transport::ImageTransport p_it(p_n);
00011 
00012     mappingDefined_ = false;
00013     sub_ = it.subscribeCamera("thermal/image", 1, &HeatDetection::imageCallback,this);
00014 
00015     sub_mapping_ = n.subscribe("thermal/mapping",1, &HeatDetection::mappingCallback,this);
00016 
00017     dyn_rec_server_.setCallback(boost::bind(&HeatDetection::dynRecParamCallback, this, _1, _2));
00018 
00019     pub_ = n.advertise<hector_worldmodel_msgs::ImagePercept>("image_percept",20);
00020     pub_detection_ = p_it.advertiseCamera("image", 10);
00021 }
00022 
00023 HeatDetection::~HeatDetection(){}
00024 
00025 
00026 void HeatDetection::imageCallback(const sensor_msgs::ImageConstPtr& img, const sensor_msgs::CameraInfoConstPtr& info){
00027 //if(debug_){
00028 //    ROS_INFO("count: %i", ++image_count_);
00029 //}
00030     if (!mappingDefined_){
00031         ROS_WARN("Error: Mapping undefined -> cannot perform detection");
00032     }else{
00033 
00034      //Read image with cvbridge
00035      cv_bridge::CvImageConstPtr cv_ptr;
00036      cv_ptr = cv_bridge::toCvShare(img, sensor_msgs::image_encodings::MONO8);
00037      cv::Mat img_filtered(cv_ptr->image);
00038 
00039      if ((img_thres_.rows != static_cast<int>(img->height)) || (img_thres_.cols != static_cast<int>(img->width))){
00040        img_thres_min_ = cv::Mat (img->height, img->width,CV_8UC1,1);
00041        img_thres_max_ = cv::Mat (img->height, img->width,CV_8UC1,1);
00042        img_thres_ = cv::Mat (img->height, img->width,CV_8UC1,1);
00043     }
00044 
00045 
00046    //Perform thresholding
00047 
00048    //Define image thresholds for victim detection
00049    int minThreshold = (int)std::max(std::min(((minTempVictim_ - mapping_.minTemp) *(256.0/( mapping_.maxTemp -  mapping_.minTemp))),255.0),0.0);
00050    int maxThreshold = (int)std::max(std::min(((maxTempVictim_ - mapping_.minTemp) *(256.0/( mapping_.maxTemp -  mapping_.minTemp))),255.0),0.0);
00051 
00052    cv::threshold(img_filtered,img_thres_min_,minThreshold,1,cv::THRESH_BINARY);
00053    cv::threshold(img_filtered,img_thres_max_,maxThreshold,1,cv::THRESH_BINARY_INV);
00054 
00055    //Element-wise multiplication to obtain an image with respect to both thresholds
00056    IplImage img_thres_min_ipl = img_thres_min_;
00057    IplImage img_thres_max_ipl = img_thres_max_;
00058    IplImage img_thres_ipl = img_thres_;
00059 
00060    cvMul(&img_thres_min_ipl, &img_thres_max_ipl, &img_thres_ipl, 255);
00061 
00062    //Perform blob detection
00063    cv::SimpleBlobDetector::Params params;
00064    params.filterByColor = true;
00065    params.blobColor = 255;
00066    params.minDistBetweenBlobs = minDistBetweenBlobs_;
00067    params.filterByArea = true;
00068    params.minArea = minAreaVictim_;
00069    params.maxArea = img_filtered.rows * img_filtered.cols;
00070    params.filterByCircularity = false;
00071    params.filterByColor = false;
00072    params.filterByConvexity = false;
00073    params.filterByInertia = false;
00074 
00075    cv::SimpleBlobDetector blob_detector(params);
00076    std::vector<cv::KeyPoint> keypoints;
00077    keypoints.clear();
00078 
00079    blob_detector.detect(img_thres_,keypoints);
00080 
00081    //Publish results
00082    hector_worldmodel_msgs::ImagePercept ip;
00083 
00084    ip.header= img->header;
00085    ip.info.class_id = perceptClassId_;
00086    ip.info.class_support = 1;
00087    ip.camera_info =  *info;
00088 
00089    for(unsigned int i=0; i<keypoints.size();i++)
00090    {
00091        ip.x = keypoints.at(i).pt.x;
00092        ip.y = keypoints.at(i).pt.y;
00093        pub_.publish(ip);
00094        ROS_DEBUG("Heat blob found at image coord: (%f, %f)", ip.x, ip.y);
00095    }
00096 
00097 
00098    if(pub_detection_.getNumSubscribers() > 0){
00099 
00100    //Create image with detection frames
00101        int width = 3;
00102        int height = 3;
00103 
00104        IplImage ipl_img = img_filtered;
00105 
00106       //Display Keypoints
00107        for(unsigned int i = 0; i < keypoints.size(); i++){
00108            if (keypoints.at(i).size > 1){
00109 
00110                //Write rectangle into image
00111                width = (int)(keypoints.at(i).size );
00112                height = (int)(keypoints.at(i).size );
00113                for(int j = -width; j <= width;j++){
00114                     if ((keypoints.at(i).pt.x + j) >= 0  &&  (keypoints.at(i).pt.x + j) < ipl_img.width){
00115                        //Draw upper line
00116                        if ((keypoints.at(i).pt.y - height) >= 0){
00117                             cvSet2D(&ipl_img,(int)(keypoints.at(i).pt.y - height), (int)(keypoints.at(i).pt.x + j),cv::Scalar(0));
00118                        }
00119                        //Draw lower line
00120                        if ((keypoints.at(i).pt.y + height) < ipl_img.height){
00121                             cvSet2D(&ipl_img,(int)(keypoints.at(i).pt.y + height), (int)(keypoints.at(i).pt.x + j),cv::Scalar(0));
00122                        }
00123                     }
00124                }
00125 
00126                for(int k = -height; k <= height;k++){
00127                    if ((keypoints.at(i).pt.y + k) >= 0  &&  (keypoints.at(i).pt.y + k) < ipl_img.height){
00128                        //Draw left line
00129                        if ((keypoints.at(i).pt.x - width) >= 0){
00130                             cvSet2D(&ipl_img,(int)(keypoints.at(i).pt.y +k), (int)(keypoints.at(i).pt.x - width),cv::Scalar(0));
00131                        }
00132                         //Draw right line
00133                        if ((keypoints.at(i).pt.x + width) < ipl_img.width){
00134                             cvSet2D(&ipl_img,(int)(keypoints.at(i).pt.y +k), (int)(keypoints.at(i).pt.x + width),cv::Scalar(0));
00135                        }
00136                    }
00137                }
00138            }
00139        }
00140 
00141        //cv::imshow("Converted Image",img_filtered);
00142        //cv::waitKey(20);
00143 
00144        cv_bridge::CvImage cvImg;
00145        cvImg.image = img_filtered;
00146 
00147 
00148 
00149        cvImg.header = img->header;
00150        cvImg.encoding = sensor_msgs::image_encodings::MONO8;
00151        pub_detection_.publish(cvImg.toImageMsg(),info);
00152     }
00153 }
00154 }
00155 
00156 void HeatDetection::mappingCallback(const thermaleye_msgs::Mapping& mapping){
00157    mapping_ = mapping;
00158    mappingDefined_ = true;
00159    ROS_INFO("Mapping received");
00160 }
00161 
00162 void HeatDetection::dynRecParamCallback(HeatDetectionConfig &config, uint32_t level)
00163 {
00164   minTempVictim_ = config.min_temp_detection;
00165   maxTempVictim_ = config.max_temp_detection;
00166   minAreaVictim_ = config.min_area_detection;
00167   minDistBetweenBlobs_ = config.min_dist_between_blobs;
00168   perceptClassId_ = config.percept_class_id;
00169 }
00170 
00171 int main(int argc, char **argv)
00172 {
00173 
00174  // cv::namedWindow("Converted Image");
00175 
00176   ros::init(argc, argv, "heat_detection");
00177 
00178   HeatDetection hd;
00179 
00180   ros::spin();
00181 
00182   return 0;
00183 }
00184 


hector_heat_detection
Author(s): Konstantin Fuchs
autogenerated on Mon Jan 18 2016 13:24:03