calc_flow.cpp
Go to the documentation of this file.
00001 //
00002 // JSK calc_flow
00003 //
00004 #include <ros/ros.h>
00005 #include <ros/names.h>
00006 
00007 #include <sensor_msgs/Image.h>
00008 #include <sensor_msgs/CameraInfo.h>
00009 #include <sensor_msgs/fill_image.h>
00010 #include <image_transport/image_transport.h>
00011 #include <image_transport/subscriber_filter.h>
00012 
00013 #include <cv.h>
00014 #include <cv_bridge/cv_bridge.h>
00015 
00016 #include <stdlib.h>
00017 
00018 class calc_flow_node {
00019 private:
00020   ros::NodeHandle nh_;
00021   image_transport::ImageTransport it_;
00022   image_transport::CameraSubscriber camera_sub;
00023   ros::Publisher result_pub_;
00024   std::string namespace_;
00025   cv::Mat prevImg;
00026   cv::Mat flow;
00027   //cv::Mat *nextImg;
00028 public:
00029   calc_flow_node () : nh_(), it_(nh_), flow(0, 0, CV_8UC1), prevImg(0, 0, CV_8UC1) {
00030     //flow = new cv::Mat(0, 0, CV_8UC1);
00031 
00032     namespace_ = nh_.resolveName("camera");
00033     camera_sub = it_.subscribeCamera(namespace_ + "/image", 10, &calc_flow_node::cameraCB, this);
00034     result_pub_ = nh_.advertise<sensor_msgs::Image> ("flow_image", 1);
00035     //result_pub_ = nh_.advertise<sensor_msgs::PointCloud2> ("flow_vector", 1);
00036   }
00037 
00038   void cameraCB(const sensor_msgs::ImageConstPtr &img,
00039                 const sensor_msgs::CameraInfoConstPtr &info) {
00040     //IplImage *ipl_ = new IplImage();
00041     //ipl_ = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 1);
00042     //sensor_msgs::CvBridge bridge;
00043     cv_bridge::CvImagePtr cv_ptr;
00044     //cvResize(bridge.imgMsgToCv(img, "mono8"), ipl_);
00045     //IplImage *ipl_ = bridge.imgMsgToCv(img, "mono8");
00046     //cv_ptr = cv_bridge::toCvCopy(img, "mono8");
00047     cv_ptr = cv_bridge::toCvCopy(img, "mono8");
00048     bool prevImg_update_required = false;
00049     if((flow.cols != (int)img->width) ||
00050        (flow.rows != (int)img->height)) {
00051       ROS_INFO("make flow");
00052       cv_ptr->image.copyTo(flow);
00053       prevImg_update_required = true;
00054     }
00055     if(prevImg_update_required) {
00056       cv_ptr->image.copyTo(prevImg);
00057       prevImg_update_required = false;
00058       ROS_INFO("return");
00059       return;
00060     }
00061     //
00062     //ROS_INFO("subscribe image");
00063     //prevImg.
00064     //cv::Mat *nextImg = new cv::Mat(ipl_);
00065     cv::Mat nextImg(img->height, img->width, CV_8UC1);
00066     //memcpy(nextImg->data, ipl_->imageData, img->height*img->width);
00067     cv_ptr->image.copyTo(nextImg);
00068     
00069     cv::calcOpticalFlowFarneback(prevImg, nextImg, flow,
00070                                  0.5, 3, 15, 3, 5, 1.2, 0 );
00071                                  // 0.5, 2,
00072                                  // 16, 4,
00073                                  // 5, 1.1, 0);
00074     //cv::OPTFLOW_USE_INITIAL_FLOW);
00075     nextImg.copyTo(prevImg);
00076 
00077     sensor_msgs::Image result;
00078     result.header = img->header;
00079     result.width  = flow.cols;
00080     result.height = flow.rows;
00081     result.encoding = "mono8";
00082     result.step   = flow.cols;
00083     result.data.resize(flow.cols * flow.rows);
00084     CvPoint2D32f *ptr = (CvPoint2D32f *)flow.data;
00085     for(int i = 0; i<result.data.size(); i++) {
00086       // copy flow -> result
00087       //result.data[i] = ;
00088       int val = 10 * sqrt(ptr[i].x * ptr[i].x + ptr[i].y * ptr[i].y);
00089       result.data[i] = 255>val?val:255;
00090     }
00091     result_pub_.publish(result);
00092   }
00093 };
00094 
00095 int main(int argc, char **argv)
00096 {
00097   ros::init(argc, argv, "calc_flow");
00098   //cv::namedWindow(std::string("window"), );
00099   calc_flow_node flow_node;
00100 
00101   ros::spin();
00102   return 0;
00103 }


jsk_perception
Author(s): Manabu Saito
autogenerated on Mon Oct 6 2014 01:16:59