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 #include <jsk_topic_tools/log_utils.h>
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.hpp>
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::Subscriber 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     result_pub_ = nh_.advertise<sensor_msgs::Image> ("flow_image", 1);
00034     sub_ = it_.subscribe(namespace_ + "/image", 10, &calc_flow_node::imageCB, this);
00035   }
00036 
00037   void imageCB(const sensor_msgs::ImageConstPtr &img) {
00038     //IplImage *ipl_ = new IplImage();
00039     //ipl_ = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 1);
00040     //sensor_msgs::CvBridge bridge;
00041     cv_bridge::CvImagePtr cv_ptr;
00042     //cvResize(bridge.imgMsgToCv(img, "mono8"), ipl_);
00043     //IplImage *ipl_ = bridge.imgMsgToCv(img, "mono8");
00044     //cv_ptr = cv_bridge::toCvCopy(img, "mono8");
00045     cv_ptr = cv_bridge::toCvCopy(img, "mono8");
00046     bool prevImg_update_required = false;
00047     if((flow.cols != (int)img->width) ||
00048        (flow.rows != (int)img->height)) {
00049       ROS_INFO("make flow");
00050       cv_ptr->image.copyTo(flow);
00051       prevImg_update_required = true;
00052     }
00053     if(prevImg_update_required) {
00054       cv_ptr->image.copyTo(prevImg);
00055       prevImg_update_required = false;
00056       ROS_INFO("return");
00057       return;
00058     }
00059     //
00060     //ROS_INFO("subscribe image");
00061     //prevImg.
00062     //cv::Mat *nextImg = new cv::Mat(ipl_);
00063     cv::Mat nextImg(img->height, img->width, CV_8UC1);
00064     //memcpy(nextImg->data, ipl_->imageData, img->height*img->width);
00065     cv_ptr->image.copyTo(nextImg);
00066     
00067     cv::calcOpticalFlowFarneback(prevImg, nextImg, flow,
00068                                  0.5, 3, 15, 3, 5, 1.2, 0 );
00069                                  // 0.5, 2,
00070                                  // 16, 4,
00071                                  // 5, 1.1, 0);
00072     //cv::OPTFLOW_USE_INITIAL_FLOW);
00073     nextImg.copyTo(prevImg);
00074 
00075     sensor_msgs::Image result;
00076     result.header = img->header;
00077     result.width  = flow.cols;
00078     result.height = flow.rows;
00079     result.encoding = "mono8";
00080     result.step   = flow.cols;
00081     result.data.resize(flow.cols * flow.rows);
00082     CvPoint2D32f *ptr = (CvPoint2D32f *)flow.data;
00083     for(int i = 0; i<result.data.size(); i++) {
00084       // copy flow -> result
00085       //result.data[i] = ;
00086       int val = 10 * sqrt(ptr[i].x * ptr[i].x + ptr[i].y * ptr[i].y);
00087       result.data[i] = 255>val?val:255;
00088     }
00089     result_pub_.publish(result);
00090   }
00091 };
00092 
00093 int main(int argc, char **argv)
00094 {
00095   ros::init(argc, argv, "calc_flow");
00096   //cv::namedWindow(std::string("window"), );
00097   calc_flow_node flow_node;
00098 
00099   ros::spin();
00100   return 0;
00101 }


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Sun Oct 8 2017 02:43:23