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;
+}