7 #include <sensor_msgs/Image.h> 8 #include <sensor_msgs/CameraInfo.h> 14 #if ( CV_MAJOR_VERSION >= 4) 15 #include <opencv2/opencv.hpp> 33 calc_flow_node () : nh_(), it_(nh_), flow(0, 0, CV_8UC1), prevImg(0, 0, CV_8UC1) {
37 result_pub_ = nh_.
advertise<sensor_msgs::Image> (
"flow_image", 1);
50 bool prevImg_update_required =
false;
51 if((flow.cols != (
int)img->width) ||
52 (flow.rows != (
int)img->height)) {
54 cv_ptr->image.copyTo(flow);
55 prevImg_update_required =
true;
57 if(prevImg_update_required) {
58 cv_ptr->image.copyTo(prevImg);
59 prevImg_update_required =
false;
67 cv::Mat nextImg(img->height, img->width, CV_8UC1);
69 cv_ptr->image.copyTo(nextImg);
71 cv::calcOpticalFlowFarneback(prevImg, nextImg, flow,
72 0.5, 3, 15, 3, 5, 1.2, 0 );
77 nextImg.copyTo(prevImg);
80 result.header = img->header;
81 result.width = flow.cols;
82 result.height = flow.rows;
83 result.encoding =
"mono8";
84 result.step = flow.cols;
85 result.data.resize(flow.cols * flow.rows);
86 CvPoint2D32f *ptr = (CvPoint2D32f *)flow.data;
87 for(
int i = 0; i<result.data.size(); i++) {
90 int val = 10 *
sqrt(ptr[i].
x * ptr[i].
x + ptr[i].
y * ptr[i].
y);
91 result.data[i] = 255>val?val:255;
97 int main(
int argc,
char **argv)
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
void publish(const boost::shared_ptr< M > &message) const
std::string resolveName(const std::string &name, bool remap=true) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
image_transport::Subscriber sub_
ROSCPP_DECL void spin(Spinner &spinner)
image_transport::ImageTransport it_
void imageCB(const sensor_msgs::ImageConstPtr &img)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher result_pub_