diff --git a/jsk_perception/CMakeLists.txt b/jsk_perception/CMakeLists.txt index 8fb61c56ba..3827518ebf 100644 --- a/jsk_perception/CMakeLists.txt +++ b/jsk_perception/CMakeLists.txt @@ -45,6 +45,7 @@ rosbuild_add_executable(point_pose_extractor src/point_pose_extractor.cpp) rosbuild_add_executable(white_balance_converter src/white_balance_converter.cpp) rosbuild_add_executable(hough_lines src/hough_lines.cpp) rosbuild_add_executable(rectangle_detector src/rectangle_detector.cpp) +rosbuild_add_executable(calc_flow src/calc_flow.cpp) target_link_libraries(camshiftdemo ${OpenCV_LIBRARIES}) target_link_libraries(virtual_camera_mono ${OpenCV_LIBRARIES} rospack) @@ -52,6 +53,7 @@ target_link_libraries(point_pose_extractor ${OpenCV_LIBRARIES} rospack) rosbuild_link_boost(point_pose_extractor filesystem system) target_link_libraries(hough_lines ${OpenCV_LIBRARIES}) target_link_libraries(rectangle_detector ${OpenCV_LIBRARIES}) +target_link_libraries(calc_flow ${OpenCV_LIBRARIES}) rosbuild_link_boost(rectangle_detector signals) macro(jsk_perception_nodelet _nodelet_cpp _nodelet_class _single_nodelet_exec_name) diff --git a/jsk_perception/catkin.cmake b/jsk_perception/catkin.cmake index 8af019559b..ffe00653ca 100644 --- a/jsk_perception/catkin.cmake +++ b/jsk_perception/catkin.cmake @@ -22,7 +22,7 @@ generate_messages( catkin_package( CATKIN_DEPENDS std_msgs sensor_msgs geometry_msgs message_runtime jsk_pcl_ros - DEPENDS + DEPENDS pcl OpenCV INCLUDE_DIRS LIBRARIES ) @@ -34,6 +34,7 @@ add_executable(point_pose_extractor src/point_pose_extractor.cpp) add_executable(white_balance_converter src/white_balance_converter.cpp) add_executable(hough_lines src/hough_lines.cpp) add_executable(rectangle_detector src/rectangle_detector.cpp) +add_executable(calc_flow src/calc_flow.cpp) macro(jsk_perception_nodelet _nodelet_cpp _nodelet_class _single_nodelet_exec_name) list(APPEND jsk_perception_nodelet_sources ${_nodelet_cpp}) @@ -70,6 +71,7 @@ target_link_libraries(edge_detector ${catkin_LIBRARIES} ${OpenCV_LIBR target_link_libraries(white_balance_converter ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) target_link_libraries(hough_lines ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) target_link_libraries(rectangle_detector ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(calc_flow ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) add_dependencies(camshiftdemo ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_gencpp) add_dependencies(virtual_camera_mono ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_gencpp) @@ -84,7 +86,7 @@ add_dependencies(rectangle_detector ${PROJECT_NAME}_gencfg ${PROJECT_NAME} # COMMAND ${PROJECT_SOURCE_DIR}/src/eusmodel_template_gen.sh) #add_custom_target(eusmodel_template ALL DEPENDS ${PROJECT_SOURCE_DIR}/template) -install(TARGETS camshiftdemo virtual_camera_mono point_pose_extractor white_balance_converter hough_lines rectangle_detector ${PROJECT_NAME} +install(TARGETS camshiftdemo virtual_camera_mono point_pose_extractor white_balance_converter hough_lines rectangle_detector calc_flow ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/jsk_perception/package.xml b/jsk_perception/package.xml index 837674fa3f..839807723a 100644 --- a/jsk_perception/package.xml +++ b/jsk_perception/package.xml @@ -26,6 +26,7 @@ image_transport driver_base dynamic_reconfigure + pcl pcl_ros eigen roscpp @@ -49,6 +50,7 @@ image_transport driver_base dynamic_reconfigure + pcl pcl_ros eigen roscpp diff --git a/jsk_perception/src/calc_flow.cpp b/jsk_perception/src/calc_flow.cpp new file mode 100644 index 0000000000..7bfdf7aa21 --- /dev/null +++ b/jsk_perception/src/calc_flow.cpp @@ -0,0 +1,103 @@ +// +// JSK calc_flow +// +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +class calc_flow_node { +private: + ros::NodeHandle nh_; + image_transport::ImageTransport it_; + image_transport::CameraSubscriber camera_sub; + ros::Publisher result_pub_; + std::string namespace_; + cv::Mat prevImg; + cv::Mat flow; + //cv::Mat *nextImg; +public: + calc_flow_node () : nh_(), it_(nh_), flow(0, 0, CV_8UC1), prevImg(0, 0, CV_8UC1) { + //flow = new cv::Mat(0, 0, CV_8UC1); + + namespace_ = nh_.resolveName("camera"); + camera_sub = it_.subscribeCamera(namespace_ + "/image", 10, &calc_flow_node::cameraCB, this); + result_pub_ = nh_.advertise ("flow_image", 1); + //result_pub_ = nh_.advertise ("flow_vector", 1); + } + + void cameraCB(const sensor_msgs::ImageConstPtr &img, + const sensor_msgs::CameraInfoConstPtr &info) { + //IplImage *ipl_ = new IplImage(); + //ipl_ = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 1); + //sensor_msgs::CvBridge bridge; + cv_bridge::CvImagePtr cv_ptr; + //cvResize(bridge.imgMsgToCv(img, "mono8"), ipl_); + //IplImage *ipl_ = bridge.imgMsgToCv(img, "mono8"); + //cv_ptr = cv_bridge::toCvCopy(img, "mono8"); + cv_ptr = cv_bridge::toCvCopy(img, "mono8"); + bool prevImg_update_required = false; + if((flow.cols != (int)img->width) || + (flow.rows != (int)img->height)) { + ROS_INFO("make flow"); + cv_ptr->image.copyTo(flow); + prevImg_update_required = true; + } + if(prevImg_update_required) { + cv_ptr->image.copyTo(prevImg); + prevImg_update_required = false; + ROS_INFO("return"); + return; + } + // + //ROS_INFO("subscribe image"); + //prevImg. + //cv::Mat *nextImg = new cv::Mat(ipl_); + cv::Mat nextImg(img->height, img->width, CV_8UC1); + //memcpy(nextImg->data, ipl_->imageData, img->height*img->width); + cv_ptr->image.copyTo(nextImg); + + cv::calcOpticalFlowFarneback(prevImg, nextImg, flow, + 0.5, 3, 15, 3, 5, 1.2, 0 ); + // 0.5, 2, + // 16, 4, + // 5, 1.1, 0); + //cv::OPTFLOW_USE_INITIAL_FLOW); + nextImg.copyTo(prevImg); + + sensor_msgs::Image result; + result.header = img->header; + result.width = flow.cols; + result.height = flow.rows; + result.encoding = "mono8"; + result.step = flow.cols; + result.data.resize(flow.cols * flow.rows); + CvPoint2D32f *ptr = (CvPoint2D32f *)flow.data; + for(int i = 0; i result + //result.data[i] = ; + int val = 10 * sqrt(ptr[i].x * ptr[i].x + ptr[i].y * ptr[i].y); + result.data[i] = 255>val?val:255; + } + result_pub_.publish(result); + } +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "calc_flow"); + //cv::namedWindow(std::string("window"), ); + calc_flow_node flow_node; + + ros::spin(); + return 0; +}