motion_detection.cpp
Go to the documentation of this file.
00001 #include "motion_detection.h"
00002 
00003 MotionDetection::MotionDetection()
00004 {
00005   ros::NodeHandle nh;
00006   ros::NodeHandle p_nh("~"); //private nh
00007 
00008   image_transport::ImageTransport it(nh);
00009   image_transport::ImageTransport image_motion_it(p_nh);
00010   image_transport::ImageTransport image_detected_it(p_nh);
00011 
00012   // subscribe topics
00013   image_sub_ = it.subscribe("opstation/rgb/image_color", 1, &MotionDetection::imageCallback, this);
00014   //image_sub_ = it.subscribe("/openni/rgb/image_color", 1, &MotionDetection::imageCallback, this);
00015 
00016   // advertise topics
00017   image_percept_pub_ = nh.advertise<hector_worldmodel_msgs::ImagePercept>("image_percept", 20);
00018   image_motion_pub_ = image_motion_it.advertiseCamera("image_motion", 10);
00019   image_detected_pub_ = image_detected_it.advertiseCamera("image_detected", 10);
00020 
00021   // dynamic reconfigure
00022   dyn_rec_server_.setCallback(boost::bind(&MotionDetection::dynRecParamCallback, this, _1, _2));
00023 
00024   update_timer = nh.createTimer(ros::Duration(0.04), &MotionDetection::update, this);
00025 }
00026 
00027 MotionDetection::~MotionDetection()
00028 {
00029 }
00030 
00031 void MotionDetection::colorizeDepth(const cv::Mat& gray, cv::Mat& rgb) const
00032 {
00033   double maxDisp = 255;
00034   float S= 1.0f;
00035   float V= 1.0f ;
00036 
00037   rgb.create(gray.size(), CV_8UC3);
00038   rgb = cv::Scalar::all(0);
00039 
00040   if (maxDisp < 1)
00041     return;
00042 
00043   for (int y = 0; y < gray.rows; y++)
00044   {
00045     for (int x = 0; x < gray.cols; x++)
00046     {
00047       uchar d = gray.at<uchar>(y,x);
00048       unsigned int H = 255 - ((uchar)maxDisp - d) * 280/ (uchar)maxDisp;
00049       unsigned int hi = (H/60) % 6;
00050 
00051       float f = H/60.f - H/60;
00052       float p = V * (1 - S);
00053       float q = V * (1 - f * S);
00054       float t = V * (1 - (1 - f) * S);
00055 
00056       cv::Point3f res;
00057 
00058       if (hi == 0) //R = V,  G = t,  B = p
00059         res = cv::Point3f( p, t, V );
00060       if (hi == 1) // R = q, G = V,  B = p
00061         res = cv::Point3f( p, V, q );
00062       if (hi == 2) // R = p, G = V,  B = t
00063         res = cv::Point3f( t, V, p );
00064       if (hi == 3) // R = p, G = q,  B = V
00065         res = cv::Point3f( V, q, p );
00066       if (hi == 4) // R = t, G = p,  B = V
00067         res = cv::Point3f( V, p, t );
00068       if (hi == 5) // R = V, G = p,  B = q
00069         res = cv::Point3f( q, p, V );
00070 
00071       uchar b = (uchar)(std::max(0.f, std::min (res.x, 1.f)) * 255.f);
00072       uchar g = (uchar)(std::max(0.f, std::min (res.y, 1.f)) * 255.f);
00073       uchar r = (uchar)(std::max(0.f, std::min (res.z, 1.f)) * 255.f);
00074 
00075       rgb.at<cv::Point3_<uchar> >(y,x) = cv::Point3_<uchar>(b, g, r);
00076     }
00077   }
00078 }
00079 
00080 void MotionDetection::drawOpticalFlowVectors(cv::Mat& img, const cv::Mat& optical_flow, int step, const cv::Scalar& color) const
00081 {
00082   for (int y = 0; y < optical_flow.rows; y += step)
00083   {
00084     for (int x = 0; x < optical_flow.cols; x += step)
00085     {
00086       const cv::Point2f& fxy = optical_flow.at<cv::Point2f>(y, x);
00087       line(img, cv::Point(x,y), cv::Point(cvRound(x+fxy.x), cvRound(y+fxy.y)), color);
00088       circle(img, cv::Point(cvRound(x+fxy.x), cvRound(y+fxy.y)), 1, color, -1);
00089     }
00090   }
00091 }
00092 
00093 void MotionDetection::computeOpticalFlow(const cv::Mat& prev_img, const cv::Mat& cur_img, cv::Mat& optical_flow, bool use_initial_flow, bool filter) const
00094 {
00095   // compute optical flow
00096   int flags = 0;
00097   if (use_initial_flow && optical_flow.rows == prev_img.rows && optical_flow.cols == prev_img.cols && optical_flow.type() == CV_32FC2)
00098     flags = cv::OPTFLOW_USE_INITIAL_FLOW;
00099   else
00100     optical_flow = cv::Mat(prev_img.rows, prev_img.cols, CV_32FC2);
00101 
00102   calcOpticalFlowFarneback(prev_img, cur_img, optical_flow, 0.5, 2, 15, 3, 5, 1.2, flags);
00103 
00104   if (filter)
00105   {
00106     // zero-mean data
00107     cv::Point2f mean;
00108     mean.x = 0;
00109     mean.y = 0;
00110     for(int y = 0; y < optical_flow.rows; y++)
00111     {
00112       for(int x = 0; x < optical_flow.cols; x++)
00113         mean += optical_flow.at<cv::Point2f>(y, x);
00114     }
00115 
00116     mean.x /= static_cast<float>(optical_flow.rows*optical_flow.cols);
00117     mean.y /= static_cast<float>(optical_flow.rows*optical_flow.cols);
00118     float mean_length = std::sqrt(mean.x*mean.x + mean.y*mean.y);
00119 
00120     float max_cos = cos(M_PI_2);
00121     for(int y = 0; y < optical_flow.rows; y++)
00122     {
00123       for(int x = 0; x < optical_flow.cols; x++)
00124       {
00125         cv::Point2f& p = optical_flow.at<cv::Point2f>(y, x);
00126 
00127         float cos_pm = (p.x*mean.x+p.y*mean.y) / (std::sqrt(p.x*p.x + p.y*p.y)*mean_length);
00128         if (cos_pm > max_cos)
00129           p -= mean;
00130       }
00131     }
00132   }
00133 }
00134 
00135 void MotionDetection::computeOpticalFlowMagnitude(const cv::Mat& optical_flow, cv::Mat& optical_flow_mag) const
00136 {
00137   optical_flow_mag = cv::Mat(optical_flow.size(), CV_8UC1);
00138 
00139   for (int y = 0; y < optical_flow.rows; y++)
00140   {
00141     for (int x = 0; x < optical_flow.cols; x++)
00142     {
00143       const cv::Point2f& fxy = optical_flow.at<cv::Point2f>(y, x);
00144       double magnitude = std::min(std::sqrt(fxy.x*fxy.x+fxy.y*fxy.y)/motion_detect_inv_sensivity_, 1.0);
00145       optical_flow_mag.at<uchar>(y, x) = static_cast<uchar>(magnitude*255);
00146     }
00147   }
00148 }
00149 
00150 void MotionDetection::drawBlobs(cv::Mat& img, const KeyPoints& keypoints, double scale) const
00151 {
00152   if (img.rows == 0 || img.cols == 0)
00153     return;
00154 
00155   double line_width = 2.0*scale;
00156   for (std::vector<cv::KeyPoint>::const_iterator itr = keypoints.begin(); itr != keypoints.end(); itr++)
00157   {
00158     const cv::KeyPoint& keypoint = *itr;
00159 
00160     if (keypoint.size > 1)
00161     {
00162       float width = keypoint.size;
00163       float height = keypoint.size;
00164 
00165       const cv::Point2f& p = keypoint.pt;
00166       cv::rectangle(img, cv::Rect(cv::Point(static_cast<int>((p.x-0.5*width-line_width)*scale), static_cast<int>((p.y-0.5*height-line_width)*scale)), cv::Point(static_cast<int>((p.x+0.5*width+line_width)*scale), static_cast<int>((p.y+0.5*height+line_width)*scale))), CV_RGB(255,0,0), line_width);
00167     }
00168   }
00169 }
00170 
00171 void MotionDetection::detectBlobs(const cv::Mat& img, KeyPoints& keypoints) const
00172 {
00173   // Perform thresholding
00174   cv::Mat img_thresh;
00175   cv::threshold(img, img_thresh, motion_detect_threshold_, 255, CV_THRESH_BINARY);
00176 
00177   // Dilate detected area
00178   cv::Mat kernel_dil = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(motion_detect_dilation_size_, motion_detect_dilation_size_));
00179   cv::dilate(img_thresh, img_thresh, kernel_dil);
00180 
00181   // Perform blob detection
00182   cv::SimpleBlobDetector::Params params;
00183   params.minDistBetweenBlobs = motion_detect_min_blob_dist_;
00184   params.filterByArea = true;
00185   params.minArea = motion_detect_min_area_;
00186   params.maxArea = img_thresh.rows * img_thresh.cols;
00187   params.filterByCircularity = false;
00188   params.filterByColor = true;
00189   params.blobColor = 255;
00190   params.filterByConvexity = false;
00191   params.filterByInertia = false;
00192 
00193   cv::SimpleBlobDetector blob_detector(params);
00194   keypoints.clear();
00195   blob_detector.detect(img_thresh, keypoints);
00196 
00197 #ifdef DEBUG
00198   cv::imshow("blob_detector_input", img_thresh);
00199 #endif
00200 }
00201 
00202 void MotionDetection::update(const ros::TimerEvent& /*event*/)
00203 {
00204   if (!last_img)
00205     return;
00206 
00207   // get image
00208   cv_bridge::CvImageConstPtr img_next_ptr(cv_bridge::toCvShare(last_img, sensor_msgs::image_encodings::MONO8));
00209   cv_bridge::CvImageConstPtr img_next_col_ptr(cv_bridge::toCvShare(last_img, sensor_msgs::image_encodings::BGR8));
00210 
00211   last_img.reset();
00212 
00213   if (img_prev_ptr_ && img_prev_col_ptr_)
00214   {
00215     cv::Mat img_prev;
00216     cv::Mat img_next;
00217     cv::Mat img_next_col;
00218 
00219     cv::Size img_size(img_next_ptr->image.cols / motion_detect_downscale_factor_, img_next_ptr->image.rows / motion_detect_downscale_factor_);
00220     cv::resize(img_prev_ptr_->image, img_prev, img_size);
00221     cv::resize(img_next_ptr->image, img_next, img_size);
00222     cv::resize(img_next_col_ptr->image, img_next_col, img_size);
00223 
00224     computeOpticalFlow(img_prev, img_next, optical_flow, motion_detect_use_initial_flow_, motion_detect_image_flow_filter_);
00225 
00226     cv::Mat optical_flow_mag;
00227     computeOpticalFlowMagnitude(optical_flow, optical_flow_mag);
00228 
00229 #ifdef DEBUG
00230     cv::Mat optical_flow_img;
00231     img_next_col.copyTo(optical_flow_img);
00232     drawOpticalFlowVectors(optical_flow_img, optical_flow);
00233     cv::resize(optical_flow_img, optical_flow_img, img_prev_ptr_->image.size());
00234     cv::imshow("flow", optical_flow_img);
00235 #endif
00236 
00237     cv::Mat optical_flow_mag_img;
00238     colorizeDepth(optical_flow_mag, optical_flow_mag_img);
00239 
00240 #ifdef DEBUG
00241     cv::resize(optical_flow_mag_img, optical_flow_mag_img, img_prev_ptr_->image.size());
00242     cv::imshow("magnitude", optical_flow_mag_img);
00243 #endif
00244 
00245     cv::Mat total_flow;
00246     optical_flow_mag.copyTo(total_flow);
00247 
00248     if (flow_history.size() < static_cast<size_t>(motion_detect_flow_history_size_))
00249     {
00250       flow_history.push_front(optical_flow_mag);
00251       return;
00252     }
00253     else
00254     {
00255       for (std::list<cv::Mat>::const_iterator itr = flow_history.begin(); itr != flow_history.end(); itr++)
00256       {
00257         cv::Mat img_thresh;
00258         cv::threshold(*itr, img_thresh, motion_detect_threshold_, 255, CV_THRESH_BINARY);
00259         cv::bitwise_or(total_flow, img_thresh, total_flow);
00260       }
00261 
00262       flow_history.push_front(optical_flow_mag);
00263 
00264       while (flow_history.size() > static_cast<size_t>(motion_detect_flow_history_size_))
00265         flow_history.pop_back();
00266     }
00267 
00268     KeyPoints keypoints;
00269     detectBlobs(total_flow, keypoints);
00270 
00271     // generate image where the detected movement is encircled by a rectangle
00272     cv::Mat img_detected;
00273     img_next_col_ptr->image.copyTo(img_detected);
00274     drawBlobs(img_detected, keypoints, motion_detect_downscale_factor_);
00275 
00276 #ifdef DEBUG
00277     cv::imshow("view", img_detected);
00278 #endif
00279 
00280     sensor_msgs::CameraInfo::Ptr info;
00281     info.reset(new sensor_msgs::CameraInfo());
00282     info->header = img_next_ptr->header;
00283 
00284     if (image_motion_pub_.getNumSubscribers() > 0)
00285     {
00286       cv_bridge::CvImage cvImg;
00287       optical_flow_mag_img.copyTo(cvImg.image);
00288       cvImg.header = img_next_ptr->header;
00289       cvImg.encoding = sensor_msgs::image_encodings::BGR8;
00290       image_motion_pub_.publish(cvImg.toImageMsg(), info);
00291     }
00292 
00293     if (image_detected_pub_.getNumSubscribers() > 0)
00294     {
00295       cv_bridge::CvImage cvImg;
00296       img_detected.copyTo(cvImg.image);
00297       cvImg.header = img_next_ptr->header;
00298       cvImg.encoding = sensor_msgs::image_encodings::BGR8;
00299       image_detected_pub_.publish(cvImg.toImageMsg(), info);
00300     }
00301   }
00302 
00303   // shift image buffers
00304   img_prev_ptr_= img_next_ptr;
00305   img_prev_col_ptr_ = img_next_col_ptr;
00306 }
00307 
00308 void MotionDetection::imageCallback(const sensor_msgs::ImageConstPtr& img)
00309 {
00310   last_img = img;
00311 }
00312 
00313 void MotionDetection::dynRecParamCallback(MotionDetectionConfig& config, uint32_t level)
00314 {
00315   motion_detect_downscale_factor_ = config.motion_detect_downscale_factor;
00316   motion_detect_inv_sensivity_ = config.motion_detect_inv_sensivity;
00317   motion_detect_use_initial_flow_ = config.motion_detect_use_initial_flow;
00318   motion_detect_image_flow_filter_ = config.motion_detect_image_flow_filter;
00319   motion_detect_threshold_ = config.motion_detect_threshold;
00320   motion_detect_min_area_ = config.motion_detect_min_area;
00321   motion_detect_min_blob_dist_ = config.motion_detect_min_blob_dist;
00322   motion_detect_dilation_size_ = config.motion_detect_dilation_size;
00323   motion_detect_flow_history_size_ = config.motion_detect_flow_history_size;
00324   percept_class_id_ = config.percept_class_id;
00325 
00326   flow_history.clear();
00327 }
00328 
00329 int main(int argc, char **argv)
00330 {
00331 #ifdef DEBUG
00332   cv::namedWindow("view");
00333   cv::namedWindow("flow");
00334   cv::namedWindow("magnitude");
00335   cv::namedWindow("blob_detector_input");
00336   cv::startWindowThread();
00337 #endif
00338 
00339   ros::init(argc, argv, "motion_detection");
00340   MotionDetection md;
00341   ros::spin();
00342 
00343 #ifdef DEBUG
00344   cv::destroyWindow("view");
00345   cv::destroyWindow("flow");
00346   cv::destroyWindow("magnitude");
00347   cv::destroyWindow("blob_detector_input");
00348 #endif
00349 
00350   return 0;
00351 }
00352 


hector_motion_detection
Author(s): Alexander Stumpf
autogenerated on Mon Jan 18 2016 13:24:11