ros-opencv-apps-1.11.14/000077500000000000000000000000001303612755600147235ustar00rootroot00000000000000ros-opencv-apps-1.11.14/.gitignore000066400000000000000000000001131303612755600167060ustar00rootroot00000000000000build*/ ._* *.pyc .* *~ # ignore png image generated from test test/*.png ros-opencv-apps-1.11.14/.travis.sh000077500000000000000000000057511303612755600166600ustar00rootroot00000000000000#!/bin/bash set -e function travis_time_start { set +x TRAVIS_START_TIME=$(date +%s%N) TRAVIS_TIME_ID=$(cat /dev/urandom | tr -dc 'a-z0-9' | fold -w 8 | head -n 1) TRAVIS_FOLD_NAME=$1 echo -e "\e[0Ktraivs_fold:start:$TRAVIS_FOLD_NAME" echo -e "\e[0Ktraivs_time:start:$TRAVIS_TIME_ID" set -x } function travis_time_end { set +x _COLOR=${1:-32} TRAVIS_END_TIME=$(date +%s%N) TIME_ELAPSED_SECONDS=$(( ($TRAVIS_END_TIME - $TRAVIS_START_TIME)/1000000000 )) echo -e "traivs_time:end:$TRAVIS_TIME_ID:start=$TRAVIS_START_TIME,finish=$TRAVIS_END_TIME,duration=$(($TRAVIS_END_TIME - $TRAVIS_START_TIME))\n\e[0K" echo -e "traivs_fold:end:$TRAVIS_FOLD_NAME" echo -e "\e[0K\e[${_COLOR}mFunction $TRAVIS_FOLD_NAME takes $(( $TIME_ELAPSED_SECONDS / 60 )) min $(( $TIME_ELAPSED_SECONDS % 60 )) sec\e[0m" set -x } apt-get update -qq && apt-get install -y -q wget sudo lsb-release # for docker travis_time_start setup.before_install #before_install: # Install ROS sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main\" > /etc/apt/sources.list.d/ros-latest.list" wget http://packages.ros.org/ros.key -O - | sudo apt-key add - sudo apt-get update -qq # Install ROS sudo apt-get install -y -q python-catkin-pkg python-catkin-tools python-rosdep python-wstool python-rosinstall-generator ros-$ROS_DISTRO-catkin source /opt/ros/$ROS_DISTRO/setup.bash # Setup for rosdep sudo rosdep init rosdep update travis_time_end travis_time_start setup.install #install: mkdir -p ~/catkin_ws/src # Add the package under test to the workspace. cd ~/catkin_ws/src ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace # Install all dependencies, using wstool and rosdep. # wstool looks for a ROSINSTALL_FILE defined in before_install. travis_time_end travis_time_start setup.before_script #before_script: # source dependencies: install using wstool. cd ~/catkin_ws/src wstool init #if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi if [ $OPENCV_VERSION == 3 ]; then rosinstall_generator image_pipeline >> .rosinstall.opencv3; fi # need to recompile image_proc if [ $OPENCV_VERSION == 3 ]; then rosinstall_generator vision_opencv >> .rosinstall.opencv3; fi # need to recompile visoin_opencv if [ $OPENCV_VERSION == 3 ]; then wstool merge .rosinstall.opencv3; fi # need to recompile visoin_opencv wstool up wstool info if [ $OPENCV_VERSION == 3 ]; then sed -i 's@libopencv-dev@opencv3@' */*/package.xml ; fi # package depdencies: install using rosdep. cd ~/catkin_ws rosdep install -q -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO travis_time_end # Compile and test. #script: travis_time_start setup.script source /opt/ros/$ROS_DISTRO/setup.bash cd ~/catkin_ws catkin build -p1 -j1 --no-status catkin run_tests -p1 -j1 --no-status catkin_test_results --verbose build || catkin_test_results --all build catkin clean -b --yes || catkin clean -b -a catkin config --install catkin build -p1 -j1 --no-status travis_time_end ros-opencv-apps-1.11.14/.travis.yml000066400000000000000000000020301303612755600170270ustar00rootroot00000000000000sudo: required dist: trusty language: generic env: - ROS_DISTRO=hydro DOCKER_IMAGE=ubuntu:precise - OPENCV_VERSION=2 ROS_DISTRO=indigo DOCKER_IMAGE=ubuntu:trusty - OPENCV_VERSION=3 ROS_DISTRO=indigo DOCKER_IMAGE=ubuntu:trusty - OPENCV_VERSION=2 ROS_DISTRO=jade DOCKER_IMAGE=ubuntu:trusty - OPENCV_VERSION=3 ROS_DISTRO=jade DOCKER_IMAGE=ubuntu:trusty - ROS_DISTRO=kinetic DOCKER_IMAGE=ubuntu:xenial # Install system dependencies, namely ROS. script: - export CI_SOURCE_PATH=$(pwd) - export REPOSITORY_NAME=${PWD##*/} - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "OPENCV_VERSION=$OPENCV_VERSION" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" after_failure: - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \; - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done ros-opencv-apps-1.11.14/CHANGELOG.rst000066400000000000000000000162341303612755600167520ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package opencv_apps ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.11.14 (2016-09-12) -------------------- * Force convert to bgr for display (`#30 `_) * add include sensor_msgs/image_encodings for old image_encodings * force conver to bgr8 using sensor_msgs::image_encodings::BGR8 * Add more nodes from opencv sample codes * [smoothing] Add smoothing filter sample code, test, launch files (`#32 `_) * [threshold] add threshold sample code (`#27 `_) * [adding_image] add adding_image sample code (`#29 `_) * Add launch files for opencv_apps nodes * separate launch and test files (`#20 `_) * Add hydro travis testing (`#22 `_) * test/CMakeLists.txt : catkin_download_test_data not working with DESTINATION . for hydro * cv_bridge before 1.11.9 does not suport CompressedImage in cv_bridge * lk_flow : need to explicitly include sensor_msgs/image_endcodings.h * simple_compressed_example_nodelet.cpp : need to include sensor_msgs/CompressedImage explicitly on hydro * .travis.yml : add hydro testing * Minor Fixes * update gitignore to avoid test png data (`#28 `_) * fix hough_circles for input frame color (`#13 `_ ) * CMakeLists.txt update list of opencv tutorial codes (`#25 `_) * fix face_detection.launch to accept args for cascade xml for opencv3 (`#20 `_) * CMakeLists.txt : add install rule for launch (`#20 `_) * add launch/*.launch files (from test/*.test) to reuse launch files (`#20 `_) * CMakeLists.txt: on roslaunch 1.11.1, roslaunch_add_file check fails with unsupported doc attributes (`#20 `_) * * Add test for simple_example / simple_compressed_example (`#24 `_) * add retry for simple_example/simple_compressed_example test, not sure why it fails.. on travis * package.xml : add image_transport_plugins to test_depend for republish node in test-simple_compressed_example.test * add test for simple_example/simple_compressed_example * simple_example_nodlet.cpp / simple_compressed_example_nodelet.cpp : support debug_view param * .travis.sh : add catkin_test_results --verbose * Support kinetic on travis (`#15 `_) * test/test-face-detection.test : add haarcascade data from opencv3 package directory * use docekr to run trusty/xenial .travis.sh * Modified enabling use_camera_info by rosparam (`#18 `_) * Enabling dynamic_reconfigure in private nodelet handler * Enable to set min_distance_between_circles param, publish debug message (`#14 `_) * hough_circles : fix to set dp_int to dp * hough_circles : enable to set min_distance_between_circles * hough_circles : add debug_image_publisher * hough_circles : fix bugs on createTrackver uses gaussian_blur_size for sigma x/y * Contributors: Kei Okada, Iori Yanokura 1.11.13 (2016-06-01) -------------------- * Add parameter to people_detector `#9 `_ * hough_circles: enable to set double value to the HoughCircle params `#8 `_ * hough_circle enable to set gaussian_blue_size and kernel sigma from cfg * hough_circles: fix default/min/max value of cfg * hough_circle: enable to set db to 100 * circle_hough: dp, accumrate_threshold, canny_threshold is double, not int * Add parameter to hough_circles_nodelet `#7 `_ * Add parameter to hough_lines_nodelet `#6 `_ * Add parameter to edge_detection_nodelet(canny) `#5 `_ * Simplify source tree by removing duplicated node codes `#4 `_ Closes `#3 `_ * fix .travis file * copy Travis and .gitignore from vision_opencv * geometry_msgs doesn't get used by opencv_apps, but std_msgs does. (`#119 `_) * Contributors: Kei Okada, Kentaro Wada, Lucas Walter, Vincent Rabaud, IorI Yanokura 1.11.12 (2016-03-10) -------------------- * relax test condition * fix test hz to 5 hz, tested on core i7 3.2G * Refactor opencv_apps to remove duplicated codes to handle connection of topics. 1. Add opencv_apps::Nodelet class to handle connection and disconnection of topics. 2. Update nodelets of opencv_apps to inhereit opencv_apps::Nodelet class to remove duplicated codes. * Contributors: Kei Okada, Ryohei Ueda 1.11.11 (2016-01-31) -------------------- * check if opencv_contrib is available * Use respawn instead of watch * Contributors: Kei Okada, trainman419 1.11.10 (2016-01-16) -------------------- * enable simple_flow on opencv3, https://github.com/ros-perception/vision_opencv/commit/8ed5ff5c48b4c3d270cd8216175cf6a8441cb046 can revert https://github.com/ros-perception/vision_opencv/commit/89a933aef7c11acdb75a17c46bfcb60389b25baa * lk_flow_nodeletcpp, fback_flow_nodelet.cpp: need to copy input image to gray * opencv_apps: add test programs, this will generate images for wiki * fix OpenCV3 build * phase_corr: fix display, bigger circle and line * goodfeature_track_nodelet.cpp: publish good feature points as corners * set image encoding to bgr8 * convex_hull: draw hull with width 4 * watershed_segmentatoin_nodelet.cpp: output segmented area as contours and suppot add_seed_points as input of segmentatoin seed * src/nodelet/segment_objects_nodelet.cpp: change output image topic name from segmented to image * Convert rgb image to bgr in lk_flow * [oepncv_apps] fix bug for segment_objects_nodelet.cpp * Contributors: Kei Okada, Kentaro Wada, Shingo Kitagawa, Vincent Rabaud 1.11.9 (2015-11-29) ------------------- * Accept grayscale images as input as well * Add format enum for easy use and choose format. * Contributors: Felix Mauch, talregev 1.11.8 (2015-07-15) ------------------- * simplify the OpenCV3 compatibility * fix image output of fback_flow * fix error: ISO C++ forbids initialization of member for gcc 4.6 * add std_srvs * add std_srvs * fix error: ISO C++ forbids initialization of member for gcc 4.6 * get opencv_apps to compile with OpenCV3 * fix licenses for Kei * add opencv_apps, proposed in `#40 `_ * Contributors: Kei Okada, Vincent Rabaud, Yuto Inagaki ros-opencv-apps-1.11.14/CMakeLists.txt000066400000000000000000000324051303612755600174670ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(opencv_apps) find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp) find_package(OpenCV REQUIRED) message(STATUS "OpenCV Components: ${OpenCV_LIB_COMPONENTS}") if(OpenCV_VERSION VERSION_LESS "3.0" OR TARGET opencv_optflow) set(OPENCV_HAVE_OPTFLOW TRUE) endif() # Supporting CompressedImage in cv_bridge has been started from 1.11.9 (2015-11-29) # note : hydro 1.10.18, indigo : 1.11.13 ... # https://github.com/ros-perception/vision_opencv/pull/70 if(cv_bridge_VERSION VERSION_LESS "1.11.9") add_definitions("-DCV_BRIDGE_COMPRESSED_IMAGE_IS_NOT_SUPPORTED") endif() # generate the dynamic_reconfigure config file generate_dynamic_reconfigure_options( cfg/AddingImages.cfg cfg/Smoothing.cfg cfg/EdgeDetection.cfg cfg/HoughLines.cfg cfg/HoughCircles.cfg cfg/FindContours.cfg cfg/ConvexHull.cfg cfg/GeneralContours.cfg cfg/ContourMoments.cfg cfg/FaceDetection.cfg cfg/GoodfeatureTrack.cfg # cfg/CamShift.cfg cfg/FBackFlow.cfg cfg/LKFlow.cfg cfg/PeopleDetect.cfg cfg/PhaseCorr.cfg cfg/SegmentObjects.cfg cfg/SimpleFlow.cfg cfg/Threshold.cfg cfg/WatershedSegmentation.cfg ) ## Generate messages in the 'msg' folder add_message_files( FILES Point2D.msg Point2DStamped.msg Point2DArray.msg Point2DArrayStamped.msg Rect.msg RectArray.msg RectArrayStamped.msg Flow.msg FlowStamped.msg FlowArray.msg FlowArrayStamped.msg RectArrayStamped.msg Size.msg Face.msg FaceArray.msg FaceArrayStamped.msg Line.msg LineArray.msg LineArrayStamped.msg RotatedRect.msg RotatedRectStamped.msg RotatedRectArray.msg RotatedRectArrayStamped.msg Circle.msg CircleArray.msg CircleArrayStamped.msg Moment.msg MomentArray.msg MomentArrayStamped.msg Contour.msg ContourArray.msg ContourArrayStamped.msg ) ## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES std_msgs ) catkin_package(CATKIN_DEPENDS std_msgs # DEPENDS OpenCV INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} ) include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) ## Declare a cpp library if(OPENCV_HAVE_OPTFLOW) list(APPEND ${PROJECT_NAME}_EXTRA_FILES src/nodelet/simple_flow_nodelet.cpp) endif() ## Macro to add nodelets macro(opencv_apps_add_nodelet node_name nodelet_name nodelet_cppfile) set(NODE_NAME ${node_name}) set(NODELET_NAME ${nodelet_name}) configure_file(src/node/standalone_nodelet_exec.cpp.in ${node_name}.cpp @ONLY) add_executable(${node_name}_exe ${node_name}.cpp) SET_TARGET_PROPERTIES(${node_name}_exe PROPERTIES OUTPUT_NAME ${node_name}) target_link_libraries(${node_name}_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) list(APPEND _opencv_apps_nodelet_cppfiles ${nodelet_cppfile}) list(APPEND _opencv_apps_nodelet_targets ${node_name}_exe) endmacro() # https://github.com/Itseez/opencv/blob/2.4/samples/cpp/ # calib3d # ./tutorial_code/calib3d/camera_calibration/camera_calibration.cpp # ./tutorial_code/calib3d/real_time_pose_estimation/src/main_detection.cpp # ./tutorial_code/calib3d/real_time_pose_estimation/src/main_registration.cpp # ./tutorial_code/calib3d/stereoBM/SBM_Sample.cpp # core opencv_apps_add_nodelet(adding_images adding_images/adding_images src/nodelet/adding_images_nodelet.cpp) # ./tutorial_code/core/AddingImages/AddingImages.cpp # ./tutorial_code/core/discrete_fourier_transform/discrete_fourier_transform.cpp # ./tutorial_code/core/file_input_output/file_input_output.cpp # ./tutorial_code/core/how_to_scan_images/how_to_scan_images.cpp # ./tutorial_code/core/interoperability_with_OpenCV_1/interoperability_with_OpenCV_1.cpp # ./tutorial_code/core/ippasync/ippasync_sample.cpp # ./tutorial_code/core/mat_mask_operations/mat_mask_operations.cpp # ./tutorial_code/core/Matrix/Drawing_1.cpp # ./tutorial_code/core/Matrix/Drawing_2.cpp # ./tutorial_code/core/mat_the_basic_image_container/mat_the_basic_image_container.cpp # features2D # ./tutorial_code/features2D/AKAZE_match.cpp # ./tutorial_code/features2D/AKAZE_tracking/planar_tracking.cpp # ./tutorial_code/gpu/gpu-basics-similarity/gpu-basics-similarity.cpp # highGUi # ./tutorial_code/HighGUI/AddingImagesTrackbar.cpp # ./tutorial_code/HighGUI/BasicLinearTransformsTrackbar.cpp # Histograms_Matching # ./tutorial_code/Histograms_Matching/calcBackProject_Demo1.cpp # ./tutorial_code/Histograms_Matching/calcBackProject_Demo2.cpp # ./tutorial_code/Histograms_Matching/calcHist_Demo.cpp # ./tutorial_code/Histograms_Matching/compareHist_Demo.cpp # ./tutorial_code/Histograms_Matching/EqualizeHist_Demo.cpp # ./tutorial_code/Histograms_Matching/MatchTemplate_Demo.cpp # imagecodecs # ./tutorial_code/imgcodecs/GDAL_IO/gdal-image.cpp # ImgProc # ./tutorial_code/ImgProc/BasicLinearTransforms.cpp # ./tutorial_code/ImgProc/Morphology_1.cpp # ./tutorial_code/ImgProc/Morphology_2.cpp # ./tutorial_code/ImgProc/Morphology_3.cpp # ./tutorial_code/ImgProc/Pyramids.cpp opencv_apps_add_nodelet(smoothing smoothing/smoothing src/nodelet/smoothing_nodelet.cpp) # ./tutorial_code/ImgProc/Smoothing.cpp opencv_apps_add_nodelet(threshold threshold/threshold src/nodelet/threshold_nodelet.cpp) # ./tutorial_code/ImgProc/Threshold.cpp # ./tutorial_code/ImgProc/Threshold_inRange.cpp # ImgTrans opencv_apps_add_nodelet(edge_detection edge_detection/edge_detection src/nodelet/edge_detection_nodelet.cpp) # ./tutorial_code/ImgTrans/CannyDetector_Demo.cpp # ./tutorial_code/ImgTrans/Laplace_Demo.cpp # ./tutorial_code/ImgTrans/Sobel_Demo.cpp opencv_apps_add_nodelet(hough_lines hough_lines/hough_lines src/nodelet/hough_lines_nodelet.cpp) # ./tutorial_code/ImgTrans/HoughLines_Demo.cpp opencv_apps_add_nodelet(hough_circles hough_circles/hough_circles src/nodelet/hough_circles_nodelet.cpp) # ./tutorial_code/ImgTrans/HoughCircle_Demo.cpp # ./tutorial_code/ImgTrans/copyMakeBorder_demo.cpp # ./tutorial_code/ImgTrans/filter2D_demo.cpp # ./tutorial_code/ImgTrans/Geometric_Transforms_Demo.cpp # ./tutorial_code/ImgTrans/imageSegmentation.cpp # ./tutorial_code/ImgTrans/Remap_Demo.cpp # introduction # ./tutorial_code/introduction/display_image/display_image.cpp # ./tutorial_code/introduction/windows_visual_studio_Opencv/introduction_windows_vs.cpp # ml # ./tutorial_code/ml/introduction_to_pca/introduction_to_pca.cpp # ./tutorial_code/ml/introduction_to_svm/introduction_to_svm.cpp # ./tutorial_code/ml/non_linear_svms/non_linear_svms.cpp # objectDetection # ./tutorial_code/objectDetection/objectDetection2.cpp # ./tutorial_code/objectDetection/objectDetection.cpp # photo # ./tutorial_code/photo/decolorization/decolor.cpp # ./tutorial_code/photo/hdr_imaging/hdr_imaging.cpp # ./tutorial_code/photo/non_photorealistic_rendering/npr_demo.cpp # ./tutorial_code/photo/seamless_cloning/cloning_demo.cpp # ./tutorial_code/photo/seamless_cloning/cloning_gui.cpp # ShapeDescriptors opencv_apps_add_nodelet(find_contours find_contours/find_contours src/nodelet/find_contours_nodelet.cpp) # ./tutorial_code/ShapeDescriptors/findContours_demo.cpp opencv_apps_add_nodelet(convex_hull convex_hull/convex_hull src/nodelet/convex_hull_nodelet.cpp) # ./tutorial_code/ShapeDescriptors/hull_demo.cpp opencv_apps_add_nodelet(general_contours general_contours/general_contours src/nodelet/general_contours_nodelet.cpp) # ./tutorial_code/ShapeDescriptors/generalContours_demo2.cpp opencv_apps_add_nodelet(contour_moments contour_moments/contour_moments src/nodelet/contour_moments_nodelet.cpp) # ./tutorial_code/ShapeDescriptors/moments_demo.cpp # ./tutorial_code/ShapeDescriptors/generalContours_demo1.cpp # ./tutorial_code/ShapeDescriptors/pointPolygonTest_demo.cpp # TrackingMotion opencv_apps_add_nodelet(goodfeature_track goodfeature_track/goodfeature_track src/nodelet/goodfeature_track_nodelet.cpp) # ./tutorial_code/TrackingMotion/goodFeaturesToTrack_Demo.cpp # ./tutorial_code/TrackingMotion/cornerDetector_Demo.cpp # ./tutorial_code/TrackingMotion/cornerHarris_Demo.cpp # ./tutorial_code/TrackingMotion/cornerSubPix_Demo.cpp # videoio # ./tutorial_code/video/bg_sub.cpp # ./tutorial_code/videoio/video-input-psnr-ssim/video-input-psnr-ssim.cpp # ./tutorial_code/videoio/video-write/video-write.cpp # viz # ./tutorial_code/viz/creating_widgets.cpp # ./tutorial_code/viz/launching_viz.cpp # ./tutorial_code/viz/transformations.cpp # ./tutorial_code/viz/widget_pose.cpp # xfeature # ./tutorial_code/xfeatures2D/LATCH_match.cpp # ./3calibration.cpp # ./autofocus.cpp # ./bgfg_segm.cpp # ./calibration.cpp opencv_apps_add_nodelet(camshift camshift/camshift src/nodelet/camshift_nodelet.cpp) # ./camshiftdemo.cpp # ./cloning_demo.cpp # ./cloning_gui.cpp # ./connected_components.cpp # ./contours2.cpp # ./convexhull.cpp # ./cout_mat.cpp # ./create_mask.cpp # ./dbt_face_detection.cpp # ./delaunay2.cpp # ./demhist.cpp # ./detect_blob.cpp # ./detect_mser.cpp # ./dft.cpp # ./distrans.cpp # ./drawing.cpp # ./edge.cpp # ./em.cpp # ./example_cmake/example.cpp opencv_apps_add_nodelet(face_detection face_detection/face_detection src/nodelet/face_detection_nodelet.cpp) # ./facedetect.cpp # ./facial_features.cpp opencv_apps_add_nodelet(fback_flow fback_flow/fback_flow src/nodelet/fback_flow_nodelet.cpp) # ./fback.cpp # ./ffilldemo.cpp # ./filestorage_base64.cpp # ./filestorage.cpp # ./fitellipse.cpp # ./grabcut.cpp # ./houghcircles.cpp # ./houghlines.cpp # ./image_alignment.cpp # ./image.cpp # ./imagelist_creator.cpp # ./image_sequence.cpp # ./inpaint.cpp # ./intelperc_capture.cpp # ./kalman.cpp # ./kmeans.cpp # ./laplace.cpp # ./letter_recog.cpp opencv_apps_add_nodelet(lk_flow lk_flow/lk_flow src/nodelet/lk_flow_nodelet.cpp) # ./lkdemo.cpp # ./logistic_regression.cpp # ./lsd_lines.cpp # ./mask_tmpl.cpp # ./matchmethod_orb_akaze_brisk.cpp # ./minarea.cpp # ./morphology2.cpp # ./neural_network.cpp # ./npr_demo.cpp # ./opencv_version.cpp # ./openni_capture.cpp # ./pca.cpp opencv_apps_add_nodelet(people_detect people_detect/people_detect src/nodelet/people_detect_nodelet.cpp) # ./peopledetect.cpp opencv_apps_add_nodelet(phase_corr phase_corr/phase_corr src/nodelet/phase_corr_nodelet.cpp) # ./phase_corr.cpp # ./points_classifier.cpp # ./polar_transforms.cpp opencv_apps_add_nodelet(segment_objects segment_objects/segment_objects src/nodelet/segment_objects_nodelet.cpp) # ./segment_objects.cpp # ./select3dobj.cpp # ./shape_example.cpp # ./smiledetect.cpp # ./squares.cpp # ./starter_imagelist.cpp # ./starter_video.cpp # ./stereo_calib.cpp # ./stereo_match.cpp # ./stitching.cpp # ./stitching_detailed.cpp # ./train_HOG.cpp # ./train_svmsgd.cpp # ./tree_engine.cpp # ./tvl1_optical_flow.cpp # ./videostab.cpp opencv_apps_add_nodelet(watershed_segmentation watershed_segmentation/watershed_segmentation src/nodelet/watershed_segmentation_nodelet.cpp) # ./watershed.cpp # ros examples opencv_apps_add_nodelet(simple_example simple_example/simple_example src/nodelet/simple_example_nodelet.cpp) opencv_apps_add_nodelet(simple_compressed_example simple_compressed_example/simple_compressed_example src/nodelet/simple_compressed_example_nodelet.cpp) # https://github.com/Itseez/opencv/blob/2.4/samples/cpp/simpleflow_demo.cpp # simple flow requires opencv-contrib https://github.com/ros-perception/vision_opencv/issues/108 if(OPENCV_HAVE_OPTFLOW) opencv_apps_add_nodelet(simple_flow simple_flow/simple_flow src/nodelet/simple_flow_nodelet.cpp) endif() # https://github.com/opencv/opencv/blob/2.4/samples/cpp/bgfg_gmg.cpp # https://github.com/opencv/opencv/blob/2.4/samples/cpp/hybridtrackingsample.cpp # https://github.com/opencv/opencv/blob/2.4/samples/cpp/linemod.cpp # https://github.com/opencv/opencv/blob/2.4/samples/cpp/retinaDemo.cpp # https://github.com/opencv/opencv/blob/2.4/samples/cpp/video_dmtx.cpp # https://github.com/opencv/opencv/blob/2.4/samples/cpp/video_homography.cpp # https://github.com/opencv/opencv/blob/2.4/samples/cpp/videocapture_pvapi.cpp add_library(${PROJECT_NAME} SHARED src/nodelet/nodelet.cpp ${_opencv_apps_nodelet_cppfiles} ${${PROJECT_NAME}_EXTRA_FILES} ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_generate_messages_cpp) install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) install(TARGETS ${_opencv_apps_nodelet_targets} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(FILES nodelet_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(DIRECTORY launch test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) ## test if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest roslaunch) if(roslaunch_VERSION VERSION_LESS "1.11.1") message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}") else() file(GLOB LAUNCH_FILES launch/*.launch) foreach(LAUNCH_FILE ${LAUNCH_FILES}) roslaunch_add_file_check(${LAUNCH_FILE}) endforeach() endif() add_subdirectory(test) endif() ros-opencv-apps-1.11.14/cfg/000077500000000000000000000000001303612755600154625ustar00rootroot00000000000000ros-opencv-apps-1.11.14/cfg/AddingImages.cfg000077500000000000000000000010541303612755600204620ustar00rootroot00000000000000#! /usr/bin/env python PACKAGE='adding_images' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("alpha", double_t, 0, "weight of the first array elements.", 0.5, 0.0, 1.0) gen.add("gamma", double_t, 0, "scalar added to each sum.", 0, 0, 255) exit(gen.generate(PACKAGE, "adding_images", "AddingImages")) ros-opencv-apps-1.11.14/cfg/CamShift.cfg000077500000000000000000000041021303612755600176410ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='camshift' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("vmin", int_t, 0, "Vmin", 10, 1, 256) gen.add("vmax", int_t, 0, "Vmax", 256, 1, 256) gen.add("smin", int_t, 0, "Smin", 30, 1, 256) exit(gen.generate(PACKAGE, "camshift", "CamShift")) ros-opencv-apps-1.11.14/cfg/ContourMoments.cfg000077500000000000000000000040341303612755600211430ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='contour_moments' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("canny_low_threshold", int_t, 0, "Canny Edge low Threshold", 100, 1, 255) exit(gen.generate(PACKAGE, "contour_moments", "ContourMoments")) ros-opencv-apps-1.11.14/cfg/ConvexHull.cfg000077500000000000000000000040121303612755600202320ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='convex_hull' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("threshold", int_t, 0, "Detect edges using Threshold", 100, 1, 255) exit(gen.generate(PACKAGE, "convex_hull", "ConvexHull")) ros-opencv-apps-1.11.14/cfg/EdgeDetection.cfg000077500000000000000000000063011303612755600206510ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='edge_detection' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) edge_type = gen.enum([ gen.const("Sobel", int_t, 0, "Sobel Derivatives"), gen.const("Laplace", int_t, 1, "Laplace Operator"), gen.const("Canny", int_t, 2, "Canny Edge Detector")], "An enum for Edge Detection Mehtods") gen.add("edge_type", int_t, 0, "Edge Detection Methods", 0, 0, 2, edit_method=edge_type) gen.add("canny_threshold1", int_t, 0, "First threshold for the hysteresis procedure.", 100, 0, 500) gen.add("canny_threshold2", int_t, 0, "Second threshold for the hysteresis procedure.", 200, 0, 500) gen.add("apertureSize", int_t, 0, "Aperture size for the Sobel() operator.", 3, 1, 10) gen.add("apply_blur_pre", bool_t, 0, "Flag, applying Blur() to input image", True) gen.add("postBlurSize", int_t, 0, "Aperture size for the Blur() operator.", 13, 3, 31) gen.add("postBlurSigma", double_t, 0, "Sigma for the GaussianBlur() operator.", 3.2, 0.0, 10.0) gen.add("apply_blur_post", bool_t, 0, "Flag, applying GaussianBlur() to output(edge) image", False) gen.add("L2gradient", bool_t, 0, "Flag, indicating whether a more accurate L_2 norm should be used to calculate the image gradient magnitude ( L2gradient=true ), or whether the default L_1 norm is enough ( L2gradient=false ).", False) exit(gen.generate(PACKAGE, "edge_detection", "EdgeDetection")) ros-opencv-apps-1.11.14/cfg/FBackFlow.cfg000077500000000000000000000036711303612755600177530ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='fback_flow' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) exit(gen.generate(PACKAGE, "fback_flow", "FBackFlow")) ros-opencv-apps-1.11.14/cfg/FaceDetection.cfg000077500000000000000000000037051303612755600206500ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='face_detection' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) exit(gen.generate(PACKAGE, "face_detection", "FaceDetection")) ros-opencv-apps-1.11.14/cfg/FindContours.cfg000077500000000000000000000040251303612755600205640ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='find_contours' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("canny_low_threshold", int_t, 0, "Canny Edge low Threshold", 10, 1, 255) exit(gen.generate(PACKAGE, "find_contours", "FindContours")) ros-opencv-apps-1.11.14/cfg/GeneralContours.cfg000077500000000000000000000040311303612755600212560ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='general_contours' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("threshold", int_t, 0, "Detect edges using Threshold", 100, 1, 255) exit(gen.generate(PACKAGE, "general_contours", "GeneralContours")) ros-opencv-apps-1.11.14/cfg/GoodfeatureTrack.cfg000077500000000000000000000040251303612755600214000ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='goodfeature_track' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("max_corners", int_t, 0, "Max Number of Corners", 23, 1, 100) exit(gen.generate(PACKAGE, "goodfeature_track", "GoodfeatureTrack")) ros-opencv-apps-1.11.14/cfg/HoughCircles.cfg000077500000000000000000000062101303612755600205240ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='hough_circles' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("canny_threshold", double_t, 0, "Canny threshold", 200, 1, 255) gen.add("accumulator_threshold", double_t, 0, "Accumulator threshold", 50, 1, 200) gen.add("gaussian_blur_size", int_t, 0, "the size of gaussian blur (should be odd number)", 9, 1, 30) gen.add("gaussian_sigma_x", double_t, 0, "sigma x of gaussian kernel", 2, 1, 10) gen.add("gaussian_sigma_y", double_t, 0, "sigma y of gaussian kernel", 2, 1, 10) gen.add("min_distance_between_circles", double_t, 0, "mnimum distance between the centers of the detected circles", 0, 0, 1024) gen.add("dp", double_t, 0, "dp", 2, 0, 100) gen.add("min_circle_radius", int_t, 0, "the minimum size of the circle", 0, 0, 500) gen.add("max_circle_radius", int_t, 0, "the maximum size of the circle", 0, 0, 2000) debug_view_type_enum = gen.enum([ gen.const("Input", int_t, 0, "Input image"), gen.const("Blur", int_t, 1, "GaussianBlur image"), gen.const("Canny", int_t, 2, "Canny edge image"), ], "An enum for debug view") gen.add("debug_image_type", int_t, 0, "Select image type for debug output", 0, 0, 2, edit_method=debug_view_type_enum) exit(gen.generate(PACKAGE, "hough_circles", "HoughCircles")) ros-opencv-apps-1.11.14/cfg/HoughLines.cfg000077500000000000000000000055621303612755600202230ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='hough_lines' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) hough_type = gen.enum([ gen.const("Standard_Hough_Transform", int_t, 0, "Standard Hough Line"), gen.const("Probabilistic_Hough_Transform", int_t, 1, "Probabilistic Hough Line")], "An enum for Hough Transform Mehtods") gen.add("hough_type", int_t, 0, "Hough Line Methods", 0, 0, 1, edit_method=hough_type) gen.add("threshold", int_t, 150, "Hough Line Threshold", 150, 50, 150) gen.add("rho", double_t, 0, "The resolution of the parameter r in pixels. We use 1 pixel.", 1, 1.0, 100.0) gen.add("theta", double_t, 0, "The resolution of the parameter \theta in radians. We use 1 degree (CV_PI/180)", 1, 1.0, 90.0) gen.add("minLineLength", double_t, 0, "The minimum number of points that can form a line. Lines with less than this number of points are disregarded.", 30, 0, 500) gen.add("maxLineGap", double_t, 0, "The maximum gap between two points to be considered in the same line.", 10, 0, 100) exit(gen.generate(PACKAGE, "hough_lines", "HoughLines")) ros-opencv-apps-1.11.14/cfg/LKFlow.cfg000077500000000000000000000036601303612755600173110ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='lk_flow' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) exit(gen.generate(PACKAGE, "lk_flow", "LKFlow")) ros-opencv-apps-1.11.14/cfg/PeopleDetect.cfg000077500000000000000000000051041303612755600205230ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='people_detect' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("hit_threshold", double_t, 150, "Threshold for the distance between features and SVM classifying plane.", 0, 0, 10) gen.add("win_stride", int_t, 8, "Window stride. It must be a multiple of block stride.", 8, 1, 100) gen.add("padding", int_t, 32, "Mock parameter to keep the CPU interface compatibility. It must be (0,0).", 32, 0, 128) gen.add("scale0", double_t, 1.05, "Coefficient of the detection window increase.", 1.05, 1.0, 100.0) gen.add("group_threshold", int_t, 2, "Coefficient to regulate the similarity threshold. When detected, some objects can be covered by many rectangles. 0 means not to perform grouping.", 2, 0, 100) exit(gen.generate(PACKAGE, "people_detect", "PeopleDetect")) ros-opencv-apps-1.11.14/cfg/PhaseCorr.cfg000077500000000000000000000036711303612755600200430ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='phase_corr' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) exit(gen.generate(PACKAGE, "phase_corr", "PhaseCorr")) ros-opencv-apps-1.11.14/cfg/SegmentObjects.cfg000077500000000000000000000037101303612755600210630ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='segment_objects' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) exit(gen.generate(PACKAGE, "segment_objects", "SegmentObjects")) ros-opencv-apps-1.11.14/cfg/SimpleFlow.cfg000077500000000000000000000037531303612755600202370ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='simple_flow' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) gen.add("scale", int_t, 0, "Scale", 4, 1, 24) exit(gen.generate(PACKAGE, "simple_flow", "SimpleFlow")) ros-opencv-apps-1.11.14/cfg/Smoothing.cfg000077500000000000000000000017401303612755600201170ustar00rootroot00000000000000#!/usr/bin/env python PACKAGE = 'smoothing' from dynamic_reconfigure.parameter_generator_catkin import *; gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) filter_type = gen.enum([ gen.const("Homogeneous_Blur", int_t, 0, "Homogeneous blur"), gen.const("Gaussian_Blur", int_t, 1, "Gaussian blur"), gen.const("Median_Blur", int_t, 2, "Median blur"), gen.const("Bilateral_Filter", int_t, 3, "Bilateral Filter")], "An enum for Smoothing Filter Mehtods") gen.add("filter_type", int_t, 0, "Smoothing Filter Methods", 1, 0, 3, edit_method=filter_type) gen.add("kernel_size", int_t, 0, "Size of the kernel (only one because we use a square window). Must be odd.", 7, 1, 31) exit (gen.generate (PACKAGE, "smoothing", "Smoothing")) ros-opencv-apps-1.11.14/cfg/Threshold.cfg000077500000000000000000000022521303612755600201030ustar00rootroot00000000000000#! /usr/bin/env python PACKAGE='threshold' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) threshold_type = gen.enum([gen.const("Binary", int_t, 0, "Binary"), gen.const("Binary_Inverted", int_t, 1, "Binary Inverted"), gen.const("Threshold_Truncated", int_t, 2, "Threshold Truncated"), gen.const("Threshold_To_Zero", int_t, 3, "Threshold to Zero"), gen.const("Threshold_To_Zero_Inverted", int_t, 4, "Threshold to zero inverted"),], "An enum for Threshold Types") gen.add("apply_otsu", bool_t, 0, "Apply otsu threshold", False) gen.add("threshold_type", int_t, 0, "Threshold Type", 0, 0, 4, edit_method=threshold_type) gen.add("threshold", int_t, 0, "Threshold value", 150, 0, 255) gen.add("max_binary", int_t, 0, "Max Binary value", 255, 0, 255) exit(gen.generate(PACKAGE, "threshold", "Threshold")) ros-opencv-apps-1.11.14/cfg/WatershedSegmentation.cfg000077500000000000000000000037351303612755600224620ustar00rootroot00000000000000#! /usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2014, Kei Okada. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Kei Okada nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PACKAGE='watershed_segmentation' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True) exit(gen.generate(PACKAGE, "watershed_segmentation", "WatershedSegmentation")) ros-opencv-apps-1.11.14/include/000077500000000000000000000000001303612755600163465ustar00rootroot00000000000000ros-opencv-apps-1.11.14/include/opencv_apps/000077500000000000000000000000001303612755600206635ustar00rootroot00000000000000ros-opencv-apps-1.11.14/include/opencv_apps/nodelet.h000066400000000000000000000250401303612755600224670ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2016, Ryohei Ueda. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef OPENCV_APPS_NODELET_H_ #define OPENCV_APPS_NODELET_H_ #include #include #include #include namespace opencv_apps { /** @brief * Enum to represent connection status. */ enum ConnectionStatus { NOT_INITIALIZED, NOT_SUBSCRIBED, SUBSCRIBED }; /** @brief * Nodelet to automatically subscribe/unsubscribe * topics according to subscription of advertised topics. * * It's important not to subscribe topic if no output is required. * * In order to watch advertised topics, need to use advertise template method. * And create subscribers in subscribe() and shutdown them in unsubscribed(). * */ class Nodelet: public nodelet::Nodelet { public: Nodelet(): subscribed_(false) { } protected: /** @brief * Initialize nodehandles nh_ and pnh_. Subclass should call * this method in its onInit method */ virtual void onInit(); /** @brief * Post processing of initialization of nodelet. * You need to call this method in order to use always_subscribe * feature. */ virtual void onInitPostProcess(); /** @brief * callback function which is called when new subscriber come */ virtual void connectionCallback(const ros::SingleSubscriberPublisher& pub); /** @brief * callback function which is called when new subscriber come for image * publisher */ virtual void imageConnectionCallback( const image_transport::SingleSubscriberPublisher& pub); /** @brief * callback function which is called when new subscriber come for camera * image publisher */ virtual void cameraConnectionCallback( const image_transport::SingleSubscriberPublisher& pub); /** @brief * callback function which is called when new subscriber come for * camera info publisher */ virtual void cameraInfoConnectionCallback( const ros::SingleSubscriberPublisher& pub); /** @brief * callback function which is called when new subscriber come for camera * image publisher or camera info publisher. * This function is called from cameraConnectionCallback * or cameraInfoConnectionCallback. */ virtual void cameraConnectionBaseCallback(); /** @brief * callback function which is called when walltimer * duration run out. */ virtual void warnNeverSubscribedCallback(const ros::WallTimerEvent& event); /** @brief * This method is called when publisher is subscribed by other * nodes. * Set up subscribers in this method. */ virtual void subscribe() = 0; /** @brief * This method is called when publisher is unsubscribed by other * nodes. * Shut down subscribers in this method. */ virtual void unsubscribe() = 0; /** @brief * Advertise a topic and watch the publisher. Publishers which are * created by this method. * It automatically reads latch boolean parameter from nh and * publish topic with appropriate latch parameter. * * @param nh NodeHandle. * @param topic topic name to advertise. * @param queue_size queue size for publisher. * @param latch set true if latch topic publication. * @return Publisher for the advertised topic. */ template ros::Publisher advertise(ros::NodeHandle& nh, std::string topic, int queue_size) { boost::mutex::scoped_lock lock(connection_mutex_); ros::SubscriberStatusCallback connect_cb = boost::bind(&Nodelet::connectionCallback, this, _1); ros::SubscriberStatusCallback disconnect_cb = boost::bind(&Nodelet::connectionCallback, this, _1); bool latch; nh.param("latch", latch, false); ros::Publisher ret = nh.advertise(topic, queue_size, connect_cb, disconnect_cb, ros::VoidConstPtr(), latch); publishers_.push_back(ret); return ret; } /** @brief * Advertise an image topic and watch the publisher. Publishers which are * created by this method. * It automatically reads latch boolean parameter from nh and it and * publish topic with appropriate latch parameter. * * @param nh NodeHandle. * @param topic topic name to advertise. * @param queue_size queue size for publisher. * @param latch set true if latch topic publication. * @return Publisher for the advertised topic. */ image_transport::Publisher advertiseImage(ros::NodeHandle& nh, const std::string& topic, int queue_size) { boost::mutex::scoped_lock lock(connection_mutex_); image_transport::SubscriberStatusCallback connect_cb = boost::bind(&Nodelet::imageConnectionCallback, this, _1); image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&Nodelet::imageConnectionCallback, this, _1); bool latch; nh.param("latch", latch, false); image_transport::Publisher pub = image_transport::ImageTransport(nh).advertise( topic, 1, connect_cb, disconnect_cb, ros::VoidPtr(), latch); image_publishers_.push_back(pub); return pub; } /** @brief * Advertise an image topic camera info topic and watch the publisher. * Publishers which are * created by this method. * It automatically reads latch boolean parameter from nh and it and * publish topic with appropriate latch parameter. * * @param nh NodeHandle. * @param topic topic name to advertise. * @param queue_size queue size for publisher. * @param latch set true if latch topic publication. * @return Publisher for the advertised topic. */ image_transport::CameraPublisher advertiseCamera(ros::NodeHandle& nh, const std::string& topic, int queue_size) { boost::mutex::scoped_lock lock(connection_mutex_); image_transport::SubscriberStatusCallback connect_cb = boost::bind(&Nodelet::cameraConnectionCallback, this, _1); image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&Nodelet::cameraConnectionCallback, this, _1); ros::SubscriberStatusCallback info_connect_cb = boost::bind(&Nodelet::cameraInfoConnectionCallback, this, _1); ros::SubscriberStatusCallback info_disconnect_cb = boost::bind(&Nodelet::cameraInfoConnectionCallback, this, _1); bool latch; nh.param("latch", latch, false); image_transport::CameraPublisher pub = image_transport::ImageTransport(nh).advertiseCamera(topic, 1, connect_cb, disconnect_cb, info_connect_cb, info_disconnect_cb, ros::VoidPtr(), latch); camera_publishers_.push_back(pub); return pub; } /** @brief * mutex to call subscribe() and unsubscribe() in * critical section. */ boost::mutex connection_mutex_; /** @brief * List of watching publishers */ std::vector publishers_; /** @brief * List of watching image publishers */ std::vector image_publishers_; /** @brief * List of watching camera publishers */ std::vector camera_publishers_; /** @brief * Shared pointer to nodehandle. */ boost::shared_ptr nh_; /** @brief * Shared pointer to private nodehandle. */ boost::shared_ptr pnh_; /** @brief * WallTimer instance for warning about no connection. */ ros::WallTimer timer_; /** @brief * A flag to check if any publisher is already subscribed * or not. */ bool subscribed_; /** @brief * A flag to check if the node has been ever subscribed * or not. */ bool ever_subscribed_; /** @brief * A flag to disable watching mechanism and always subscribe input * topics. It can be specified via ~always_subscribe parameter. */ bool always_subscribe_; /** @brief * Status of connection */ ConnectionStatus connection_status_; /** @brief * true if `~verbose_connection` or `verbose_connection` parameter is true. */ bool verbose_connection_; private: }; } #endif ros-opencv-apps-1.11.14/launch/000077500000000000000000000000001303612755600161755ustar00rootroot00000000000000ros-opencv-apps-1.11.14/launch/adding_images.launch000066400000000000000000000026161303612755600221510ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/camshift.launch000066400000000000000000000025361303612755600211750ustar00rootroot00000000000000 $(arg histogram) ros-opencv-apps-1.11.14/launch/contour_moments.launch000066400000000000000000000016741303612755600226340ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/convex_hull.launch000066400000000000000000000016341303612755600217230ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/edge_detection.launch000066400000000000000000000043061303612755600223360ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/face_detection.launch000066400000000000000000000033151303612755600223270ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/fback_flow.launch000066400000000000000000000014201303612755600214630ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/find_contours.launch000066400000000000000000000017231303612755600222500ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/general_contours.launch000066400000000000000000000016531303612755600227470ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/goodfeature_track.launch000066400000000000000000000020051303612755600230560ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/hough_circles.launch000066400000000000000000000041171303612755600222120ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/hough_lines.launch000066400000000000000000000033301303612755600216740ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/lk_flow.launch000066400000000000000000000014071303612755600210300ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/people_detect.launch000066400000000000000000000032571303612755600222140ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/phase_corr.launch000066400000000000000000000014211303612755600215140ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/segment_objects.launch000066400000000000000000000014371303612755600225510ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/simple_flow.launch000066400000000000000000000016411303612755600217130ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/smoothing.launch000066400000000000000000000022221303612755600213760ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/threshold.launch000066400000000000000000000030571303612755600213720ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/launch/watershed_segmentation.launch000066400000000000000000000014771303612755600241450ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/msg/000077500000000000000000000000001303612755600155115ustar00rootroot00000000000000ros-opencv-apps-1.11.14/msg/Circle.msg000066400000000000000000000000371303612755600174220ustar00rootroot00000000000000Point2D center float64 radius ros-opencv-apps-1.11.14/msg/CircleArray.msg000066400000000000000000000000221303612755600204130ustar00rootroot00000000000000Circle[] circles ros-opencv-apps-1.11.14/msg/CircleArrayStamped.msg000066400000000000000000000000401303612755600217310ustar00rootroot00000000000000Header header Circle[] circles ros-opencv-apps-1.11.14/msg/Contour.msg000066400000000000000000000000211303612755600176430ustar00rootroot00000000000000Point2D[] points ros-opencv-apps-1.11.14/msg/ContourArray.msg000066400000000000000000000000231303612755600206440ustar00rootroot00000000000000Contour[] contours ros-opencv-apps-1.11.14/msg/ContourArrayStamped.msg000066400000000000000000000000421303612755600221630ustar00rootroot00000000000000Header header Contour[] contours ros-opencv-apps-1.11.14/msg/Face.msg000066400000000000000000000000261303612755600170550ustar00rootroot00000000000000Rect face Rect[] eyes ros-opencv-apps-1.11.14/msg/FaceArray.msg000066400000000000000000000000161303612755600200530ustar00rootroot00000000000000Face[] faces ros-opencv-apps-1.11.14/msg/FaceArrayStamped.msg000066400000000000000000000000341303612755600213710ustar00rootroot00000000000000Header header Face[] faces ros-opencv-apps-1.11.14/msg/Flow.msg000066400000000000000000000000371303612755600171300ustar00rootroot00000000000000Point2D point Point2D velocity ros-opencv-apps-1.11.14/msg/FlowArray.msg000066400000000000000000000000141303612755600201220ustar00rootroot00000000000000Flow[] flow ros-opencv-apps-1.11.14/msg/FlowArrayStamped.msg000066400000000000000000000000321303612755600214400ustar00rootroot00000000000000Header header Flow[] flow ros-opencv-apps-1.11.14/msg/FlowStamped.msg000066400000000000000000000000301303612755600204370ustar00rootroot00000000000000Header header Flow flow ros-opencv-apps-1.11.14/msg/Line.msg000066400000000000000000000000311303612755600171020ustar00rootroot00000000000000Point2D pt1 Point2D pt2 ros-opencv-apps-1.11.14/msg/LineArray.msg000066400000000000000000000000151303612755600201030ustar00rootroot00000000000000Line[] lines ros-opencv-apps-1.11.14/msg/LineArrayStamped.msg000066400000000000000000000000331303612755600214210ustar00rootroot00000000000000Header header Line[] lines ros-opencv-apps-1.11.14/msg/Moment.msg000066400000000000000000000006771303612755600174720ustar00rootroot00000000000000# spatial moments float64 m00 float64 m10 float64 m01 float64 m20 float64 m11 float64 m02 float64 m30 float64 m21 float64 m12 float64 m03 # central moments float64 mu20 float64 mu11 float64 mu02 float64 mu30 float64 mu21 float64 mu12 float64 mu03 # central normalized moments float64 nu20 float64 nu11 float64 nu02 float64 nu30 float64 nu21 float64 nu12 float64 nu03 # center of mass m10/m00, m01/m00 Point2D center float64 length float64 area ros-opencv-apps-1.11.14/msg/MomentArray.msg000066400000000000000000000000211303612755600204500ustar00rootroot00000000000000Moment[] moments ros-opencv-apps-1.11.14/msg/MomentArrayStamped.msg000066400000000000000000000000371303612755600217750ustar00rootroot00000000000000Header header Moment[] moments ros-opencv-apps-1.11.14/msg/Point2D.msg000066400000000000000000000000251303612755600174750ustar00rootroot00000000000000float64 x float64 y ros-opencv-apps-1.11.14/msg/Point2DArray.msg000066400000000000000000000000211303612755600204700ustar00rootroot00000000000000Point2D[] points ros-opencv-apps-1.11.14/msg/Point2DArrayStamped.msg000066400000000000000000000000371303612755600220150ustar00rootroot00000000000000Header header Point2D[] points ros-opencv-apps-1.11.14/msg/Point2DStamped.msg000066400000000000000000000000351303612755600210140ustar00rootroot00000000000000Header header Point2D point ros-opencv-apps-1.11.14/msg/Rect.msg000066400000000000000000000001371303612755600171170ustar00rootroot00000000000000# opencv Rect data type, x-y is center point float64 x float64 y float64 width float64 height ros-opencv-apps-1.11.14/msg/RectArray.msg000066400000000000000000000000151303612755600201110ustar00rootroot00000000000000Rect[] rects ros-opencv-apps-1.11.14/msg/RectArrayStamped.msg000066400000000000000000000000351303612755600214310ustar00rootroot00000000000000Header header Rect[] rects ros-opencv-apps-1.11.14/msg/RotatedRect.msg000066400000000000000000000000471303612755600204420ustar00rootroot00000000000000float64 angle Point2D center Size size ros-opencv-apps-1.11.14/msg/RotatedRectArray.msg000066400000000000000000000000241303612755600214340ustar00rootroot00000000000000RotatedRect[] rects ros-opencv-apps-1.11.14/msg/RotatedRectArrayStamped.msg000066400000000000000000000000431303612755600227530ustar00rootroot00000000000000Header header RotatedRect[] rects ros-opencv-apps-1.11.14/msg/RotatedRectStamped.msg000066400000000000000000000000411303612755600217520ustar00rootroot00000000000000Header header RotatedRect rect ros-opencv-apps-1.11.14/msg/Size.msg000066400000000000000000000000361303612755600171320ustar00rootroot00000000000000float64 width float64 height ros-opencv-apps-1.11.14/nodelet_plugins.xml000066400000000000000000000111321303612755600206360ustar00rootroot00000000000000 Nodelet to combine two images Nodelet to smoothing by homogeneous filter, bilateral filter, gaussian filter, median filter Nodelet to find edges Nodelet to find lines Nodelet to find circles Nodelet to find contours Nodelet to find convex hulls Nodelet to creating bounding boxes and circles for contours Nodelet to find image moments Nodelet to find faces Nodelet for detecting corners using Shi-Tomasi method Nodelet to show mean-shift based tracking Nodelet to demonstrates dense optical flow algorithm by Gunnar Farneback Nodelet to calculate Lukas-Kanade optical flow Nodelet to demonstrate the use of the HoG descriptor Nodelet to demonstrate the use of phaseCorrelate Nodelet to demonstrate a simple method of connected components clean up of background subtraction Nodelet of SimpleFlow optical flow algorithm Nodelet of Simple Example from wiki Nodelet of Simple Example from wiki Nodelet of threshold Nodelet to demonstrate the famous watershed segmentation algorithm in OpenCV: watershed() ros-opencv-apps-1.11.14/package.xml000066400000000000000000000027301303612755600170420ustar00rootroot00000000000000 opencv_apps 1.11.14 The opencv_apps package, most of code is taken from https://github.com/Itseez/opencv/tree/master/samples/cpp Kei Okada Kei Okada BSD catkin cv_bridge dynamic_reconfigure image_transport message_generation nodelet roscpp std_msgs std_srvs cv_bridge dynamic_reconfigure image_transport message_runtime nodelet roscpp std_msgs std_srvs rostest rosbag rosservice rostopic image_proc image_view topic_tools image_transport_plugins ros-opencv-apps-1.11.14/src/000077500000000000000000000000001303612755600155125ustar00rootroot00000000000000ros-opencv-apps-1.11.14/src/node/000077500000000000000000000000001303612755600164375ustar00rootroot00000000000000ros-opencv-apps-1.11.14/src/node/standalone_nodelet_exec.cpp.in000066400000000000000000000046321303612755600244230ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2016, Kentaro Wada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kentaro Wada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #include #include int main(int argc, char **argv) { ros::init(argc, argv, "@NODE_NAME@", ros::init_options::AnonymousName); if (ros::names::remap("image") == "image") { ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n" "\t$ rosrun image_rotate image_rotate image:= [transport]"); } nodelet::Loader manager(false); nodelet::M_string remappings; nodelet::V_string my_argv(argv + 1, argv + argc); my_argv.push_back("--shutdown-on-close"); // Internal manager.load(ros::this_node::getName(), "@NODELET_NAME@", remappings, my_argv); ros::spin(); return 0; } ros-opencv-apps-1.11.14/src/nodelet/000077500000000000000000000000001303612755600171445ustar00rootroot00000000000000ros-opencv-apps-1.11.14/src/nodelet/adding_images_nodelet.cpp000066400000000000000000000217621303612755600241450ustar00rootroot00000000000000// -*- mode: c++ -*- /********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) JSK, 2016 Lab * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the JSK Lab nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/opencv/opencv/tree/2.4/samples/cpp/tutorial_code/ImgProc/AddingImages.cpp /** * This is a demo of adding image (linear blending). */ #include #include #include #include #include #include #include #include #include #include #include #include "opencv_apps/AddingImagesConfig.h" #include "opencv_apps/nodelet.h" namespace adding_images { class AddingImagesNodelet : public opencv_apps::Nodelet { private: boost::shared_ptr it_; typedef message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicyWithCameraInfo; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> ApproxSyncPolicyWithCameraInfo; typedef message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image> SyncPolicy; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image> ApproxSyncPolicy; //////////////////////////////////////////////////////// // Dynamic Reconfigure //////////////////////////////////////////////////////// typedef adding_images::AddingImagesConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_; image_transport::SubscriberFilter sub_image1_, sub_image2_; image_transport::Publisher img_pub_; message_filters::Subscriber sub_camera_info_; boost::shared_ptr > sync_with_info_; boost::shared_ptr > async_with_info_; boost::shared_ptr > sync_; boost::shared_ptr > async_; boost::mutex mutex_; bool approximate_sync_; double alpha_; double beta_; double gamma_; void imageCallbackWithInfo( const sensor_msgs::ImageConstPtr& msg1, const sensor_msgs::ImageConstPtr& msg2, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg1, msg2, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg1, const sensor_msgs::ImageConstPtr& msg2) { do_work(msg1, msg2, msg1->header.frame_id); } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); sub_image1_.subscribe(*it_, "image1", 3); sub_image2_.subscribe(*it_, "image2", 3); sub_camera_info_.subscribe(*pnh_, "info", 3); if (config_.use_camera_info) { if (approximate_sync_) { async_with_info_ = boost::make_shared< message_filters::Synchronizer >(100); async_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_); async_with_info_->registerCallback(boost::bind( &AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3)); } else { sync_with_info_ = boost::make_shared >(100); sync_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_); sync_with_info_->registerCallback(boost::bind( &AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3)); } } else { if (approximate_sync_) { async_ = boost::make_shared< message_filters::Synchronizer >(100); async_->connectInput(sub_image1_, sub_image2_); async_->registerCallback( boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2)); } else { sync_ = boost::make_shared >(100); sync_->connectInput(sub_image1_, sub_image2_); sync_->registerCallback( boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2)); } } } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); sub_image1_.unsubscribe(); sub_image2_.unsubscribe(); sub_camera_info_.unsubscribe(); } void reconfigureCallback(Config& config, uint32_t level) { boost::mutex::scoped_lock lock(mutex_); config_ = config; alpha_ = config.alpha; beta_ = 1.0 - alpha_; gamma_ = config.gamma; } void do_work(const sensor_msgs::Image::ConstPtr& image_msg1, const sensor_msgs::Image::ConstPtr& image_msg2, const std::string input_frame_from_msg) { boost::mutex::scoped_lock lock(mutex_); // Work on the image. try { cv::Mat image1 = cv_bridge::toCvShare(image_msg1, sensor_msgs::image_encodings::BGR8)->image; cv::Mat image2 = cv_bridge::toCvShare(image_msg2, sensor_msgs::image_encodings::BGR8)->image; cv::Mat result_image; cv::addWeighted(image1, alpha_, image2, beta_, gamma_, result_image); //-- Show what you got if (debug_view_) { cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); cv::imshow(window_name_, result_image); int c = cv::waitKey(1); } // publish bgr8 image img_pub_.publish(cv_bridge::CvImage(image_msg1->header, sensor_msgs::image_encodings::BGR8, result_image).toImageMsg()); } catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = image_msg1->header.stamp; } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr( new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "AddingImages Demo"; //////////////////////////////////////////////////////// // Dynamic Reconfigure //////////////////////////////////////////////////////// reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&AddingImagesNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); pnh_->param("approximate_sync", approximate_sync_, true); img_pub_ = advertiseImage(*pnh_, "image", 1); onInitPostProcess(); } }; } #include PLUGINLIB_EXPORT_CLASS(adding_images::AddingImagesNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/camshift_nodelet.cpp000066400000000000000000000341741303612755600231710ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/camshiftdemo.cpp /** * This is a demo that shows mean-shift based tracking * You select a color objects such as your face and it tracks it. * This reads from video camera (0 by default, or the camera number the user enters */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include #include #include #include #include "opencv_apps/CamShiftConfig.h" #include "opencv_apps/RotatedRectStamped.h" namespace camshift { class CamShiftNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_, bproj_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef camshift::CamShiftConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_, histogram_name_; static bool need_config_update_; static bool on_mouse_update_; static int on_mouse_event_; static int on_mouse_x_; static int on_mouse_y_; int vmin_, vmax_, smin_; bool backprojMode; bool selectObject; int trackObject; bool showHist; cv::Point origin; cv::Rect selection; bool paused; cv::Rect trackWindow; int hsize; float hranges[2]; const float* phranges; cv::Mat hist, histimg; //cv::Mat hsv; static void onMouse( int event, int x, int y, int, void* ) { on_mouse_update_ = true; on_mouse_event_ = event; on_mouse_x_ = x; on_mouse_y_ = y; } void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; vmin_ = config_.vmin; vmax_ = config_.vmax; smin_ = config_.smin; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; cv::Mat backproj; // Messages opencv_apps::RotatedRectStamped rect_msg; rect_msg.header = msg->header; // Do the work if( debug_view_) { /// Create Trackbars for Thresholds cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); cv::setMouseCallback( window_name_, onMouse, 0 ); cv::createTrackbar( "Vmin", window_name_, &vmin_, 256, trackbarCallback); cv::createTrackbar( "Vmax", window_name_, &vmax_, 256, trackbarCallback); cv::createTrackbar( "Smin", window_name_, &smin_, 256, trackbarCallback); if (need_config_update_) { config_.vmin = vmin_; config_.vmax = vmax_; config_.smin = smin_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } if ( on_mouse_update_ ) { int event = on_mouse_event_; int x = on_mouse_x_; int y = on_mouse_y_; if( selectObject ) { selection.x = MIN(x, origin.x); selection.y = MIN(y, origin.y); selection.width = std::abs(x - origin.x); selection.height = std::abs(y - origin.y); selection &= cv::Rect(0, 0, frame.cols, frame.rows); } switch( event ) { case cv::EVENT_LBUTTONDOWN: origin = cv::Point(x,y); selection = cv::Rect(x,y,0,0); selectObject = true; break; case cv::EVENT_LBUTTONUP: selectObject = false; if( selection.width > 0 && selection.height > 0 ) trackObject = -1; break; } on_mouse_update_ = false; } if( !paused ) { cv::Mat hsv, hue, mask; cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV); if( trackObject ) { int _vmin = vmin_, _vmax = vmax_; cv::inRange(hsv, cv::Scalar(0, smin_, MIN(_vmin,_vmax)), cv::Scalar(180, 256, MAX(_vmin, _vmax)), mask); int ch[] = {0, 0}; hue.create(hsv.size(), hsv.depth()); cv::mixChannels(&hsv, 1, &hue, 1, ch, 1); if( trackObject < 0 ) { cv::Mat roi(hue, selection), maskroi(mask, selection); cv::calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges); cv::normalize(hist, hist, 0, 255, cv::NORM_MINMAX); std::vector hist_value; hist_value.resize(hsize); for(int i = 0; i < hsize; i ++) { hist_value[i] = hist.at(i);} pnh_->setParam("histogram", hist_value); trackWindow = selection; trackObject = 1; histimg = cv::Scalar::all(0); int binW = histimg.cols / hsize; cv::Mat buf(1, hsize, CV_8UC3); for( int i = 0; i < hsize; i++ ) buf.at(i) = cv::Vec3b(cv::saturate_cast(i*180./hsize), 255, 255); cv::cvtColor(buf, buf, cv::COLOR_HSV2BGR); for( int i = 0; i < hsize; i++ ) { int val = cv::saturate_cast(hist.at(i)*histimg.rows/255); cv::rectangle( histimg, cv::Point(i*binW,histimg.rows), cv::Point((i+1)*binW,histimg.rows - val), cv::Scalar(buf.at(i)), -1, 8 ); } } cv::calcBackProject(&hue, 1, 0, hist, backproj, &phranges); backproj &= mask; cv::RotatedRect trackBox = cv::CamShift(backproj, trackWindow, cv::TermCriteria( cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 10, 1 )); if( trackWindow.area() <= 1 ) { int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6; //trackWindow = cv::Rect(trackWindow.x - r, trackWindow.y - r, // trackWindow.x + r, trackWindow.y + r) & trackWindow = cv::Rect(cols/2 - r, rows/2 - r, cols/2 + r, rows/2 + r) & cv::Rect(0, 0, cols, rows); } if( backprojMode ) cv::cvtColor( backproj, frame, cv::COLOR_GRAY2BGR ); #ifndef CV_VERSION_EPOCH cv::ellipse( frame, trackBox, cv::Scalar(0,0,255), 3, cv::LINE_AA ); #else cv::ellipse( frame, trackBox, cv::Scalar(0,0,255), 3, CV_AA ); #endif rect_msg.rect.angle = trackBox.angle; opencv_apps::Point2D point_msg; opencv_apps::Size size_msg; point_msg.x = trackBox.center.x; point_msg.y = trackBox.center.y; size_msg.width = trackBox.size.width; size_msg.height = trackBox.size.height; rect_msg.rect.center = point_msg; rect_msg.rect.size = size_msg; } } else if( trackObject < 0 ) paused = false; if( selectObject && selection.width > 0 && selection.height > 0 ) { cv::Mat roi(frame, selection); bitwise_not(roi, roi); } if( debug_view_ ) { cv::imshow( window_name_, frame ); cv::imshow( histogram_name_, histimg ); char c = (char)cv::waitKey(1); //if( c == 27 ) // break; switch(c) { case 'b': backprojMode = !backprojMode; break; case 'c': trackObject = 0; histimg = cv::Scalar::all(0); break; case 'h': showHist = !showHist; if( !showHist ) cv::destroyWindow( histogram_name_ ); else cv::namedWindow( histogram_name_, 1 ); break; case 'p': paused = !paused; break; default: ; } } // Publish the image. sensor_msgs::Image::Ptr out_img1 = cv_bridge::CvImage(msg->header, msg->encoding, frame).toImageMsg(); sensor_msgs::Image::Ptr out_img2 = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, backproj).toImageMsg(); img_pub_.publish(out_img1); bproj_pub_.publish(out_img2); if( trackObject ) msg_pub_.publish(rect_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &CamShiftNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &CamShiftNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "CamShift Demo"; histogram_name_ = "Histogram"; vmin_ = 10; vmax_ = 256; smin_ = 30; backprojMode = false; selectObject = false; trackObject = 0; showHist = true; paused = false; hsize = 16; hranges[0] = 0; hranges[1] = 180; phranges = hranges; histimg = cv::Mat::zeros(200, 320, CV_8UC3); reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&CamShiftNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); bproj_pub_ = advertiseImage(*pnh_, "back_project", 1); msg_pub_ = advertise(*pnh_, "track_box", 1); NODELET_INFO("Hot keys: "); NODELET_INFO("\tESC - quit the program"); NODELET_INFO("\tc - stop the tracking"); NODELET_INFO("\tb - switch to/from backprojection view"); NODELET_INFO("\th - show/hide object histogram"); NODELET_INFO("\tp - pause video"); NODELET_INFO("To initialize tracking, select the object with mouse"); std::vector hist_value; pnh_->getParam("histogram", hist_value); if ( hist_value.size() == hsize ) { hist.create(hsize, 1, CV_32F); for(int i = 0; i < hsize; i ++) { hist.at(i) = hist_value[i];} trackObject = 1; trackWindow = cv::Rect(0, 0, 640, 480); // histimg = cv::Scalar::all(0); int binW = histimg.cols / hsize; cv::Mat buf(1, hsize, CV_8UC3); for( int i = 0; i < hsize; i++ ) buf.at(i) = cv::Vec3b(cv::saturate_cast(i*180./hsize), 255, 255); cv::cvtColor(buf, buf, cv::COLOR_HSV2BGR); for( int i = 0; i < hsize; i++ ) { int val = cv::saturate_cast(hist.at(i)*histimg.rows/255); cv::rectangle( histimg, cv::Point(i*binW,histimg.rows), cv::Point((i+1)*binW,histimg.rows - val), cv::Scalar(buf.at(i)), -1, 8 ); } } onInitPostProcess(); } }; bool CamShiftNodelet::need_config_update_ = false; bool CamShiftNodelet::on_mouse_update_ = false; int CamShiftNodelet::on_mouse_event_ = 0; int CamShiftNodelet::on_mouse_x_ = 0; int CamShiftNodelet::on_mouse_y_ = 0; } #include PLUGINLIB_EXPORT_CLASS(camshift::CamShiftNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/contour_moments_nodelet.cpp000066400000000000000000000227331303612755600246240ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ShapeDescriptors/moments_demo.cpp /** * @function moments_demo.cpp * @brief Demo code to calculate moments * @author OpenCV team */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include "opencv_apps/ContourMomentsConfig.h" #include "opencv_apps/Moment.h" #include "opencv_apps/MomentArray.h" #include "opencv_apps/MomentArrayStamped.h" namespace contour_moments { class ContourMomentsNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef contour_moments::ContourMomentsConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; int low_threshold_; std::string window_name_; static bool need_config_update_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; low_threshold_ = config_.canny_low_threshold; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::MomentArrayStamped moments_msg; moments_msg.header = msg->header; // Do the work cv::Mat src_gray; /// Convert image to gray and blur it if ( frame.channels() > 1 ) { cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); } else { src_gray = frame; } cv::blur( src_gray, src_gray, cv::Size(3,3) ); /// Create window if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); } cv::Mat canny_output; std::vector > contours; std::vector hierarchy; cv::RNG rng(12345); /// Detect edges using canny cv::Canny( src_gray, canny_output, low_threshold_ , low_threshold_ *2, 3 ); /// Find contours cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); /// Get the moments std::vector mu(contours.size() ); for( size_t i = 0; i < contours.size(); i++ ) { mu[i] = moments( contours[i], false ); } /// Get the mass centers: std::vector mc( contours.size() ); for( size_t i = 0; i < contours.size(); i++ ) { mc[i] = cv::Point2f( static_cast(mu[i].m10/mu[i].m00) , static_cast(mu[i].m01/mu[i].m00) ); } /// Draw contours cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 ); for( size_t i = 0; i< contours.size(); i++ ) { cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() ); cv::circle( drawing, mc[i], 4, color, -1, 8, 0 ); } /// Calculate the area with the moments 00 and compare with the result of the OpenCV function printf("\t Info: Area and Contour Length \n"); for( size_t i = 0; i< contours.size(); i++ ) { NODELET_INFO(" * Contour[%d] - Area (M_00) = %.2f - Area OpenCV: %.2f - Length: %.2f \n", (int)i, mu[i].m00, cv::contourArea(contours[i]), cv::arcLength( contours[i], true ) ); cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() ); cv::circle( drawing, mc[i], 4, color, -1, 8, 0 ); opencv_apps::Moment moment_msg; moment_msg.m00 = mu[i].m00; moment_msg.m10 = mu[i].m10; moment_msg.m01 = mu[i].m01; moment_msg.m20 = mu[i].m20; moment_msg.m11 = mu[i].m11; moment_msg.m02 = mu[i].m02; moment_msg.m30 = mu[i].m30; moment_msg.m21 = mu[i].m21; moment_msg.m12 = mu[i].m12; moment_msg.m03 = mu[i].m03; moment_msg.mu20 = mu[i].mu20; moment_msg.mu11 = mu[i].mu11; moment_msg.mu02 = mu[i].mu02; moment_msg.mu30 = mu[i].mu30; moment_msg.mu21 = mu[i].mu21; moment_msg.mu12 = mu[i].mu12; moment_msg.mu03 = mu[i].mu03; moment_msg.nu20 = mu[i].nu20; moment_msg.nu11 = mu[i].nu11; moment_msg.nu02 = mu[i].nu02; moment_msg.nu30 = mu[i].nu30; moment_msg.nu21 = mu[i].nu21; moment_msg.nu12 = mu[i].nu12; moment_msg.nu03 = mu[i].nu03; opencv_apps::Point2D center_msg; center_msg.x = mc[i].x; center_msg.y = mc[i].y; moment_msg.center = center_msg; moment_msg.area = cv::contourArea(contours[i]); moment_msg.length = cv::arcLength(contours[i], true); moments_msg.moments.push_back(moment_msg); } if( debug_view_) { cv::imshow( window_name_, drawing ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", drawing).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(moments_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &ContourMomentsNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &ContourMomentsNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Contours"; low_threshold_ = 100; // only for canny reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&ContourMomentsNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "moments", 1); onInitPostProcess(); } }; bool ContourMomentsNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(contour_moments::ContourMomentsNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/convex_hull_nodelet.cpp000066400000000000000000000202761303612755600237170ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ShapeDescriptors/hull_demo.cpp /** * @function hull_demo.cpp * @brief Demo code to find contours in an image * @author OpenCV team */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include #include "opencv_apps/ConvexHullConfig.h" #include "opencv_apps/Contour.h" #include "opencv_apps/ContourArray.h" #include "opencv_apps/ContourArrayStamped.h" namespace convex_hull { class ConvexHullNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef convex_hull::ConvexHullConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; int threshold_; std::string window_name_; static bool need_config_update_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; threshold_ = config_.threshold; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::ContourArrayStamped contours_msg; contours_msg.header = msg->header; // Do the work cv::Mat src_gray; /// Convert image to gray and blur it if ( frame.channels() > 1 ) { cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY ); } else { src_gray = frame; } cv::blur( src_gray, src_gray, cv::Size(3,3) ); /// Create window if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); } cv::Mat threshold_output; int max_thresh = 255; std::vector > contours; std::vector hierarchy; cv::RNG rng(12345); /// Detect edges using Threshold cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY ); /// Find contours cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); /// Find the convex hull object for each contour std::vector >hull( contours.size() ); for( size_t i = 0; i < contours.size(); i++ ) { cv::convexHull( cv::Mat(contours[i]), hull[i], false ); } /// Draw contours + hull results cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 ); for( size_t i = 0; i< contours.size(); i++ ) { cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector(), 0, cv::Point() ); cv::drawContours( drawing, hull, (int)i, color, 4, 8, std::vector(), 0, cv::Point() ); opencv_apps::Contour contour_msg; for ( size_t j = 0; j < hull[i].size(); j++ ) { opencv_apps::Point2D point_msg; point_msg.x = hull[i][j].x; point_msg.y = hull[i][j].y; contour_msg.points.push_back(point_msg); } contours_msg.contours.push_back(contour_msg); } /// Create a Trackbar for user to enter threshold if( debug_view_) { if (need_config_update_) { config_.threshold = threshold_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback); cv::imshow( window_name_, drawing ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(contours_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &ConvexHullNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &ConvexHullNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Hull Demo"; threshold_ = 100; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&ConvexHullNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "hulls", 1); onInitPostProcess(); } }; bool ConvexHullNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(convex_hull::ConvexHullNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/edge_detection_nodelet.cpp000066400000000000000000000246101303612755600243270ustar00rootroot00000000000000// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*- /********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/ /** * @file Sobel_Demo.cpp * @brief Sample code using Sobel and/orScharr OpenCV functions to make a simple Edge Detector * @author OpenCV team */ /** * @file Laplace_Demo.cpp * @brief Sample code showing how to detect edges using the Laplace operator * @author OpenCV team */ /** * @file CannyDetector_Demo.cpp * @brief Sample code showing how to detect edges using the Canny Detector * @author OpenCV team */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include "opencv_apps/EdgeDetectionConfig.h" namespace edge_detection { class EdgeDetectionNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef edge_detection::EdgeDetectionConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; int canny_threshold1_; int canny_threshold2_; int apertureSize_; bool L2gradient_; bool apply_blur_pre_; bool apply_blur_post_; int postBlurSize_; double postBlurSigma_; std::string window_name_; static bool need_config_update_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; canny_threshold1_ = config_.canny_threshold1; canny_threshold2_ = config_.canny_threshold2; apertureSize_ = 2*((config_.apertureSize/2)) + 1; L2gradient_ = config_.L2gradient; apply_blur_pre_ = config_.apply_blur_pre; apply_blur_post_ = config_.apply_blur_post; postBlurSize_ = 2*((config_.postBlurSize)/2) + 1; postBlurSigma_ = config_.postBlurSigma; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Do the work cv::Mat src_gray; cv::GaussianBlur( frame, frame, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT ); /// Convert it to gray if ( frame.channels() > 1 ) { cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY ); } else { src_gray = frame; } /// Create window if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); } std::string new_window_name; cv::Mat grad; switch (config_.edge_type) { case edge_detection::EdgeDetection_Sobel: { /// Generate grad_x and grad_y cv::Mat grad_x, grad_y; cv::Mat abs_grad_x, abs_grad_y; int scale = 1; int delta = 0; int ddepth = CV_16S; /// Gradient X //Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT ); cv::Sobel( src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, cv::BORDER_DEFAULT ); cv::convertScaleAbs( grad_x, abs_grad_x ); /// Gradient Y //Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT ); cv::Sobel( src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, cv::BORDER_DEFAULT ); cv::convertScaleAbs( grad_y, abs_grad_y ); /// Total Gradient (approximate) cv::addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad ); new_window_name = "Sobel Edge Detection Demo"; break; } case edge_detection::EdgeDetection_Laplace: { cv::Mat dst; int kernel_size = 3; int scale = 1; int delta = 0; int ddepth = CV_16S; /// Apply Laplace function cv::Laplacian( src_gray, dst, ddepth, kernel_size, scale, delta, cv::BORDER_DEFAULT ); convertScaleAbs( dst, grad ); new_window_name = "Laplace Edge Detection Demo"; break; } case edge_detection::EdgeDetection_Canny: { int edgeThresh = 1; int kernel_size = 3; int const max_canny_threshold1 = 500; int const max_canny_threshold2 = 500; cv::Mat detected_edges; /// Reduce noise with a kernel 3x3 if(apply_blur_pre_) { cv::blur( src_gray, src_gray, cv::Size(apertureSize_, apertureSize_) ); } /// Canny detector cv::Canny( src_gray, grad, canny_threshold1_, canny_threshold2_, kernel_size, L2gradient_ ); if(apply_blur_post_) { cv::GaussianBlur(grad, grad, cv::Size(postBlurSize_, postBlurSize_), postBlurSigma_, postBlurSigma_); // 0.3*(ksize/2 - 1) + 0.8 } new_window_name = "Canny Edge Detection Demo"; /// Create a Trackbar for user to enter threshold if( debug_view_) { if (need_config_update_) { config_.canny_threshold1 = canny_threshold1_; config_.canny_threshold2 = canny_threshold2_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } if( window_name_ == new_window_name) { cv::createTrackbar( "Min CannyThreshold1:", window_name_, &canny_threshold1_, max_canny_threshold1, trackbarCallback); cv::createTrackbar( "Min CannyThreshold2:", window_name_, &canny_threshold2_, max_canny_threshold2, trackbarCallback); } } break; } } if( debug_view_) { if (window_name_ != new_window_name) { cv::destroyWindow(window_name_); window_name_ = new_window_name; } cv::imshow( window_name_, grad ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, grad).toImageMsg(); img_pub_.publish(out_img); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &EdgeDetectionNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &EdgeDetectionNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Edge Detection Demo"; canny_threshold1_ = 100; // only for canny canny_threshold2_ = 200; // only for canny reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&EdgeDetectionNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); //msg_pub_ = local_nh_.advertise("lines", 1, msg_connect_cb, msg_disconnect_cb); onInitPostProcess(); } }; bool EdgeDetectionNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(edge_detection::EdgeDetectionNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/face_detection_nodelet.cpp000066400000000000000000000206771303612755600243320ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/objectDetection/objectDetection.cpp /** * @file objectDetection.cpp * @author A. Huaman ( based in the classic facedetect.cpp in samples/c ) * @brief A simplified version of facedetect.cpp, show how to load a cascade classifier and how to find objects (Face + eyes) in a video stream */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include #include "opencv_apps/FaceDetectionConfig.h" #include "opencv_apps/Face.h" #include "opencv_apps/FaceArray.h" #include "opencv_apps/FaceArrayStamped.h" namespace face_detection { class FaceDetectionNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef face_detection::FaceDetectionConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; cv::CascadeClassifier face_cascade_; cv::CascadeClassifier eyes_cascade_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::FaceArrayStamped faces_msg; faces_msg.header = msg->header; // Do the work std::vector faces; cv::Mat frame_gray; if ( frame.channels() > 1 ) { cv::cvtColor( frame, frame_gray, cv::COLOR_BGR2GRAY ); } else { frame_gray = frame; } cv::equalizeHist( frame_gray, frame_gray ); //-- Detect faces #ifndef CV_VERSION_EPOCH face_cascade_.detectMultiScale( frame_gray, faces, 1.1, 2, 0, cv::Size(30, 30) ); #else face_cascade_.detectMultiScale( frame_gray, faces, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) ); #endif for( size_t i = 0; i < faces.size(); i++ ) { cv::Point center( faces[i].x + faces[i].width/2, faces[i].y + faces[i].height/2 ); cv::ellipse( frame, center, cv::Size( faces[i].width/2, faces[i].height/2), 0, 0, 360, cv::Scalar( 255, 0, 255 ), 2, 8, 0 ); opencv_apps::Face face_msg; face_msg.face.x = center.x; face_msg.face.y = center.y; face_msg.face.width = faces[i].width; face_msg.face.height = faces[i].height; cv::Mat faceROI = frame_gray( faces[i] ); std::vector eyes; //-- In each face, detect eyes #ifndef CV_VERSION_EPOCH eyes_cascade_.detectMultiScale( faceROI, eyes, 1.1, 2, 0, cv::Size(30, 30) ); #else eyes_cascade_.detectMultiScale( faceROI, eyes, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) ); #endif for( size_t j = 0; j < eyes.size(); j++ ) { cv::Point eye_center( faces[i].x + eyes[j].x + eyes[j].width/2, faces[i].y + eyes[j].y + eyes[j].height/2 ); int radius = cvRound( (eyes[j].width + eyes[j].height)*0.25 ); cv::circle( frame, eye_center, radius, cv::Scalar( 255, 0, 0 ), 3, 8, 0 ); opencv_apps::Rect eye_msg; eye_msg.x = eye_center.x; eye_msg.y = eye_center.y; eye_msg.width = eyes[j].width; eye_msg.height = eyes[j].height; face_msg.eyes.push_back(eye_msg); } faces_msg.faces.push_back(face_msg); } //-- Show what you got if( debug_view_) { cv::imshow( "Face detection", frame ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(faces_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &FaceDetectionNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &FaceDetectionNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_ ) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "faces", 1); std::string face_cascade_name, eyes_cascade_name; pnh_->param("face_cascade_name", face_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml")); pnh_->param("eyes_cascade_name", eyes_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml")); if( !face_cascade_.load( face_cascade_name ) ){ NODELET_ERROR("--Error loading %s", face_cascade_name.c_str()); }; if( !eyes_cascade_.load( eyes_cascade_name ) ){ NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str()); }; onInitPostProcess(); } }; } #include PLUGINLIB_EXPORT_CLASS(face_detection::FaceDetectionNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/fback_flow_nodelet.cpp000066400000000000000000000164331303612755600234660ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/fback.cpp /* * This program demonstrates dense optical flow algorithm by Gunnar Farneback * Mainly the function: calcOpticalFlowFarneback() */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include #include "opencv_apps/FBackFlowConfig.h" #include "opencv_apps/FlowArrayStamped.h" namespace fback_flow { class FBackFlowNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef fback_flow::FBackFlowConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_; static bool need_config_update_; cv::Mat prevgray, gray, flow, cflow; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::FlowArrayStamped flows_msg; flows_msg.header = msg->header; if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); if (need_config_update_) { reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } // Do the work if ( frame.channels() > 1 ) { cv::cvtColor( frame, gray, cv::COLOR_BGR2GRAY ); } else { frame.copyTo(gray); } if( prevgray.data ) { cv::calcOpticalFlowFarneback(prevgray, gray, flow, 0.5, 3, 15, 3, 5, 1.2, 0); cv::cvtColor(prevgray, cflow, cv::COLOR_GRAY2BGR); //drawOptFlowMap(flow, cflow, 16, 1.5, Scalar(0, 255, 0)); int step = 16; cv::Scalar color = cv::Scalar(0, 255, 0); for(int y = 0; y < cflow.rows; y += step) for(int x = 0; x < cflow.cols; x += step) { const cv::Point2f& fxy = flow.at(y, x); cv::line(cflow, cv::Point(x,y), cv::Point(cvRound(x+fxy.x), cvRound(y+fxy.y)), color); cv::circle(cflow, cv::Point(x,y), 2, color, -1); opencv_apps::Flow flow_msg; opencv_apps::Point2D point_msg; opencv_apps::Point2D velocity_msg; point_msg.x = x; point_msg.y = y; velocity_msg.x = fxy.x; velocity_msg.y = fxy.y; flow_msg.point = point_msg; flow_msg.velocity = velocity_msg; flows_msg.flow.push_back(flow_msg); } } std::swap(prevgray, gray); //-- Show what you got if( debug_view_) { cv::imshow( window_name_, cflow ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", cflow).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(flows_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &FBackFlowNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &FBackFlowNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "flow"; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&FBackFlowNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "flows", 1); onInitPostProcess(); } }; bool FBackFlowNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(fback_flow::FBackFlowNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/find_contours_nodelet.cpp000066400000000000000000000200761303612755600242430ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ShapeDescriptors/findContours_Demo.cpp /** * @function findContours_Demo.cpp * @brief Demo code to find contours in an image * @author OpenCV team */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include #include "opencv_apps/FindContoursConfig.h" #include "opencv_apps/Contour.h" #include "opencv_apps/ContourArray.h" #include "opencv_apps/ContourArrayStamped.h" namespace find_contours { class FindContoursNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef find_contours::FindContoursConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; int low_threshold_; std::string window_name_; static bool need_config_update_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; low_threshold_ = config_.canny_low_threshold; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::ContourArrayStamped contours_msg; contours_msg.header = msg->header; // Do the work cv::Mat src_gray; /// Convert it to gray if ( frame.channels() > 1 ) { cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY ); } else { src_gray = frame; } cv::GaussianBlur( src_gray, src_gray, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT ); /// Create window if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); } cv::Mat canny_output; int max_thresh = 255; std::vector > contours; std::vector hierarchy; cv::RNG rng(12345); /// Reduce noise with a kernel 3x3 cv::blur( src_gray, canny_output, cv::Size(3,3) ); /// Canny detector cv::Canny( canny_output, canny_output, low_threshold_, low_threshold_*2, 3 ); /// Find contours cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); /// Draw contours cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 ); for( size_t i = 0; i< contours.size(); i++ ) { cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() ); opencv_apps::Contour contour_msg; for ( size_t j = 0; j < contours[i].size(); j++ ) { opencv_apps::Point2D point_msg; point_msg.x = contours[i][j].x; point_msg.y = contours[i][j].y; contour_msg.points.push_back(point_msg); } contours_msg.contours.push_back(contour_msg); } /// Create a Trackbar for user to enter threshold if( debug_view_) { if (need_config_update_) { config_.canny_low_threshold = low_threshold_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } cv::createTrackbar( "Canny thresh:", window_name_, &low_threshold_, max_thresh, trackbarCallback); cv::imshow( window_name_, drawing ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(contours_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &FindContoursNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &FindContoursNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Demo code to find contours in an image"; low_threshold_ = 100; // only for canny reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&FindContoursNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "contours", 1); onInitPostProcess(); } }; bool FindContoursNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(find_contours::FindContoursNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/general_contours_nodelet.cpp000066400000000000000000000231221303612755600247330ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ShapeDescriptors/generalContours_demo2.cpp /** * @function generalContours_demo2.cpp * @brief Demo code to obtain ellipses and rotated rectangles that contain detected contours * @author OpenCV team */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include #include "opencv_apps/GeneralContoursConfig.h" #include "opencv_apps/RotatedRect.h" #include "opencv_apps/RotatedRectArray.h" #include "opencv_apps/RotatedRectArrayStamped.h" namespace general_contours { class GeneralContoursNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher rects_pub_, ellipses_pub_; boost::shared_ptr it_; typedef general_contours::GeneralContoursConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; int threshold_; std::string window_name_; static bool need_config_update_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; threshold_ = config_.threshold; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::RotatedRectArrayStamped rects_msg, ellipses_msg; rects_msg.header = msg->header; ellipses_msg.header = msg->header; // Do the work cv::Mat src_gray; /// Convert image to gray and blur it if ( frame.channels() > 1 ) { cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY ); } else { src_gray = frame; } cv::blur( src_gray, src_gray, cv::Size(3,3) ); /// Create window if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); } cv::Mat threshold_output; int max_thresh = 255; std::vector > contours; std::vector hierarchy; cv::RNG rng(12345); /// Detect edges using Threshold cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY ); /// Find contours cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); /// Find the rotated rectangles and ellipses for each contour std::vector minRect( contours.size() ); std::vector minEllipse( contours.size() ); for( size_t i = 0; i < contours.size(); i++ ) { minRect[i] = cv::minAreaRect( cv::Mat(contours[i]) ); if( contours[i].size() > 5 ) { minEllipse[i] = cv::fitEllipse( cv::Mat(contours[i]) ); } } /// Draw contours + rotated rects + ellipses cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 ); for( size_t i = 0; i< contours.size(); i++ ) { cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); // contour cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector(), 0, cv::Point() ); // ellipse cv::ellipse( drawing, minEllipse[i], color, 2, 8 ); // rotated rectangle cv::Point2f rect_points[4]; minRect[i].points( rect_points ); for( int j = 0; j < 4; j++ ) cv::line( drawing, rect_points[j], rect_points[(j+1)%4], color, 1, 8 ); opencv_apps::RotatedRect rect_msg; opencv_apps::Point2D rect_center; opencv_apps::Size rect_size; rect_center.x = minRect[i].center.x; rect_center.y = minRect[i].center.y; rect_size.width = minRect[i].size.width; rect_size.height = minRect[i].size.height; rect_msg.center = rect_center; rect_msg.size = rect_size; rect_msg.angle = minRect[i].angle; opencv_apps::RotatedRect ellipse_msg; opencv_apps::Point2D ellipse_center; opencv_apps::Size ellipse_size; ellipse_center.x = minEllipse[i].center.x; ellipse_center.y = minEllipse[i].center.y; ellipse_size.width = minEllipse[i].size.width; ellipse_size.height = minEllipse[i].size.height; ellipse_msg.center = ellipse_center; ellipse_msg.size = ellipse_size; ellipse_msg.angle = minEllipse[i].angle; rects_msg.rects.push_back(rect_msg); ellipses_msg.rects.push_back(ellipse_msg); } /// Create a Trackbar for user to enter threshold if( debug_view_) { if (need_config_update_) { config_.threshold = threshold_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback); cv::imshow( window_name_, drawing ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg(); img_pub_.publish(out_img); rects_pub_.publish(rects_msg); ellipses_pub_.publish(ellipses_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &GeneralContoursNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &GeneralContoursNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Contours"; threshold_ = 100; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&GeneralContoursNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); rects_pub_ = advertise(*pnh_, "rectangles", 1); ellipses_pub_ = advertise(*pnh_, "ellipses", 1); onInitPostProcess(); } }; bool GeneralContoursNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(general_contours::GeneralContoursNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/goodfeature_track_nodelet.cpp000066400000000000000000000177631303612755600250700ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // http://github.com/Itseez/opencv/blob/master/samples/cpp/tutorial_code/TrackingMotion/goodFeaturesToTrack_Demo.cpp /** * @function goodFeaturesToTrack_Demo.cpp * @brief Demo code for detecting corners using Shi-Tomasi method * @author OpenCV team */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include "opencv_apps/GoodfeatureTrackConfig.h" #include "opencv_apps/Point2D.h" #include "opencv_apps/Point2DArrayStamped.h" namespace goodfeature_track { class GoodfeatureTrackNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef goodfeature_track::GoodfeatureTrackConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_; static bool need_config_update_; int max_corners_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; max_corners_ = config_.max_corners; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::Point2DArrayStamped corners_msg; corners_msg.header = msg->header; // Do the work cv::Mat src_gray; int maxTrackbar = 100; if ( frame.channels() > 1 ) { cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); } else { src_gray = frame; cv::cvtColor( src_gray, frame, cv::COLOR_GRAY2BGR ); } if( debug_view_) { /// Create Trackbars for Thresholds cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); cv::createTrackbar( "Max corners", window_name_, &max_corners_, maxTrackbar, trackbarCallback); if (need_config_update_) { config_.max_corners = max_corners_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } /// void goodFeaturesToTrack_Demo( int, void* ) if( max_corners_ < 1 ) { max_corners_ = 1; } /// Parameters for Shi-Tomasi algorithm std::vector corners; double qualityLevel = 0.01; double minDistance = 10; int blockSize = 3; bool useHarrisDetector = false; double k = 0.04; cv::RNG rng(12345); /// Apply corner detection cv::goodFeaturesToTrack( src_gray, corners, max_corners_, qualityLevel, minDistance, cv::Mat(), blockSize, useHarrisDetector, k ); /// Draw corners detected NODELET_INFO_STREAM("** Number of corners detected: "<header, "bgr8", frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(corners_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &GoodfeatureTrackNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &GoodfeatureTrackNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Image"; max_corners_ = 23; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&GoodfeatureTrackNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "corners", 1); onInitPostProcess(); } }; bool GoodfeatureTrackNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(goodfeature_track::GoodfeatureTrackNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/hough_circles_nodelet.cpp000066400000000000000000000324771303612755600242150ustar00rootroot00000000000000// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*- /********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/HoughCircle_Demo.cpp /** * @file HoughCircle_Demo.cpp * @brief Demo code for Hough Transform * @author OpenCV team */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include "opencv_apps/HoughCirclesConfig.h" #include "opencv_apps/Circle.h" #include "opencv_apps/CircleArray.h" #include "opencv_apps/CircleArrayStamped.h" namespace hough_circles { class HoughCirclesNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef hough_circles::HoughCirclesConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_; static bool need_config_update_; // initial and max values of the parameters of interests. int canny_threshold_initial_value_; int accumulator_threshold_initial_value_; int max_accumulator_threshold_; int max_canny_threshold_; double canny_threshold_; int canny_threshold_int; // for trackbar double accumulator_threshold_; int accumulator_threshold_int; int gaussian_blur_size_; double gaussian_sigma_x_; int gaussian_sigma_x_int; double gaussian_sigma_y_; int gaussian_sigma_y_int; int voting_threshold_; double min_distance_between_circles_; int min_distance_between_circles_int; double dp_; int dp_int; int min_circle_radius_; int max_circle_radius_; image_transport::Publisher debug_image_pub_; int debug_image_type_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; canny_threshold_ = config_.canny_threshold; accumulator_threshold_ = config_.accumulator_threshold; gaussian_blur_size_ = config_.gaussian_blur_size; gaussian_sigma_x_ = config_.gaussian_sigma_x; gaussian_sigma_y_ = config_.gaussian_sigma_y; dp_ = config_.dp; min_circle_radius_ = config_.min_circle_radius; max_circle_radius_ = config_.max_circle_radius; debug_image_type_ = config_.debug_image_type; min_distance_between_circles_ = config_.min_distance_between_circles; canny_threshold_int = int(canny_threshold_); accumulator_threshold_int = int(accumulator_threshold_); gaussian_sigma_x_int = int(gaussian_sigma_x_); gaussian_sigma_y_int = int(gaussian_sigma_y_); min_distance_between_circles_int = int(min_distance_between_circles_); dp_int = int(dp_); } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int value, void* userdata) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::CircleArrayStamped circles_msg; circles_msg.header = msg->header; // Do the work std::vector faces; cv::Mat src_gray, edges; if ( frame.channels() > 1 ) { cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); } else { src_gray = frame; } // create the main window, and attach the trackbars if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); cv::createTrackbar("Canny Threshold", window_name_, &canny_threshold_int, max_canny_threshold_, trackbarCallback); cv::createTrackbar("Accumulator Threshold", window_name_, &accumulator_threshold_int, max_accumulator_threshold_, trackbarCallback); cv::createTrackbar("Gaussian Blur Size", window_name_, &gaussian_blur_size_, 30, trackbarCallback); cv::createTrackbar("Gaussian Sigam X", window_name_, &gaussian_sigma_x_int, 10, trackbarCallback); cv::createTrackbar("Gaussian Sigma Y", window_name_, &gaussian_sigma_y_int, 10, trackbarCallback); cv::createTrackbar("Min Distance between Circles", window_name_, &min_distance_between_circles_int, 100, trackbarCallback); cv::createTrackbar("Dp", window_name_, &dp_int, 100, trackbarCallback); cv::createTrackbar("Min Circle Radius", window_name_, &min_circle_radius_, 500, trackbarCallback); cv::createTrackbar("Max Circle Radius", window_name_, &max_circle_radius_, 2000, trackbarCallback); if (need_config_update_) { config_.canny_threshold = canny_threshold_ = (double)canny_threshold_int; config_.accumulator_threshold = accumulator_threshold_ = (double)accumulator_threshold_int; config_.gaussian_blur_size = gaussian_blur_size_; config_.gaussian_sigma_x = gaussian_sigma_x_ = (double)gaussian_sigma_x_int; config_.gaussian_sigma_y = gaussian_sigma_y_ = (double)gaussian_sigma_y_int; config_.min_distance_between_circles = min_distance_between_circles_ = (double)min_distance_between_circles_int; config_.dp = dp_ = (double)dp_int; config_.min_circle_radius = min_circle_radius_; config_.max_circle_radius = max_circle_radius_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } // Reduce the noise so we avoid false circle detection // gaussian_blur_size_ must be odd number if (gaussian_blur_size_%2 != 1) { gaussian_blur_size_ = gaussian_blur_size_ + 1; } cv::GaussianBlur( src_gray, src_gray, cv::Size(gaussian_blur_size_, gaussian_blur_size_), gaussian_sigma_x_, gaussian_sigma_y_ ); // those paramaters cannot be =0 // so we must check here canny_threshold_ = std::max(canny_threshold_, 1.0); accumulator_threshold_ = std::max(accumulator_threshold_, 1.0); if( debug_view_) { // https://github.com/Itseez/opencv/blob/2.4.8/modules/imgproc/src/hough.cpp#L817 cv::Canny(frame, edges, MAX(canny_threshold_/2,1), canny_threshold_, 3 ); } if ( min_distance_between_circles_ == 0 ) { // set inital value min_distance_between_circles_ = src_gray.rows/8; config_.min_distance_between_circles = min_distance_between_circles_; reconfigure_server_->updateConfig(config_); } //runs the detection, and update the display // will hold the results of the detection std::vector circles; // runs the actual detection cv::HoughCircles( src_gray, circles, CV_HOUGH_GRADIENT, dp_, min_distance_between_circles_, canny_threshold_, accumulator_threshold_, min_circle_radius_, max_circle_radius_ ); cv::Mat out_image; if ( frame.channels() == 1 ) { cv::cvtColor( frame, out_image, cv::COLOR_GRAY2BGR); } else { out_image = frame; } // clone the colour, input image for displaying purposes for( size_t i = 0; i < circles.size(); i++ ) { cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1])); int radius = cvRound(circles[i][2]); // circle center circle( out_image, center, 3, cv::Scalar(0,255,0), -1, 8, 0 ); // circle outline circle( out_image, center, radius, cv::Scalar(0,0,255), 3, 8, 0 ); opencv_apps::Circle circle_msg; circle_msg.center.x = center.x; circle_msg.center.y = center.y; circle_msg.radius = radius; circles_msg.circles.push_back(circle_msg); } // shows the results if( debug_view_ || debug_image_pub_.getNumSubscribers() > 0 ) { cv::Mat debug_image; switch (debug_image_type_) { case 1: debug_image = src_gray; break; case 2: debug_image = edges; break; default: debug_image = frame; break; } if ( debug_view_ ) { cv::imshow( window_name_, debug_image ); int c = cv::waitKey(1); if ( c == 's' ) { debug_image_type_ = (++debug_image_type_)%3; config_.debug_image_type = debug_image_type_; reconfigure_server_->updateConfig(config_); } } if ( debug_image_pub_.getNumSubscribers() > 0 ) { sensor_msgs::Image::Ptr out_debug_img = cv_bridge::CvImage(msg->header, msg->encoding, debug_image).toImageMsg(); debug_image_pub_.publish(out_debug_img); } } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(circles_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &HoughCirclesNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &HoughCirclesNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); debug_image_type_ = 0; pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = debug_view_; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Hough Circle Detection Demo"; canny_threshold_initial_value_ = 200; accumulator_threshold_initial_value_ = 50; max_accumulator_threshold_ = 200; max_canny_threshold_ = 255; min_distance_between_circles_ = 0; //declare and initialize both parameters that are subjects to change canny_threshold_ = canny_threshold_initial_value_; accumulator_threshold_ = accumulator_threshold_initial_value_; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&HoughCirclesNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "circles", 1); debug_image_type_ = 0; debug_image_pub_ = advertiseImage(*pnh_, "debug_image", 1); onInitPostProcess(); } }; bool HoughCirclesNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(hough_circles::HoughCirclesNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/hough_lines_nodelet.cpp000066400000000000000000000240441303612755600236720ustar00rootroot00000000000000// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*- /********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/HoughLines_Demo.cpp /** * @file HoughLines_Demo.cpp * @brief Demo code for Hough Transform * @author OpenCV team */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include "opencv_apps/HoughLinesConfig.h" #include "opencv_apps/Line.h" #include "opencv_apps/LineArray.h" #include "opencv_apps/LineArrayStamped.h" namespace hough_lines { class HoughLinesNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef hough_lines::HoughLinesConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_; static bool need_config_update_; int min_threshold_; int max_threshold_; int threshold_; double rho_; double theta_; double minLineLength_; double maxLineGap_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; rho_ = config_.rho; theta_ = config_.theta; threshold_ = config_.threshold; minLineLength_ = config_.minLineLength; maxLineGap_ = config_.maxLineGap; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat in_image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; cv::Mat src_gray; if (in_image.channels() > 1) { cv::cvtColor( in_image, src_gray, cv::COLOR_BGR2GRAY ); /// Apply Canny edge detector Canny( src_gray, in_image, 50, 200, 3 ); } else { /// Check whether input gray image is filtered such that canny, sobel ...etc bool is_filtered = true; for(int y=0; y < in_image.rows; ++y) { for(int x=0; x < in_image.cols; ++x) { if(!(in_image.at(y, x) == 0 || in_image.at(y, x) == 255)) { is_filtered = false; break; } if(!is_filtered) { break; } } } if(!is_filtered) { Canny( in_image, in_image, 50, 200, 3 ); } } cv::Mat out_image; cv::cvtColor(in_image, out_image, CV_GRAY2BGR); // Messages opencv_apps::LineArrayStamped lines_msg; lines_msg.header = msg->header; // Do the work std::vector faces; if( debug_view_) { /// Create Trackbars for Thresholds char thresh_label[50]; sprintf( thresh_label, "Thres: %d + input", min_threshold_ ); cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); cv::createTrackbar( thresh_label, window_name_, &threshold_, max_threshold_, trackbarCallback); if (need_config_update_) { config_.threshold = threshold_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } switch (config_.hough_type) { case hough_lines::HoughLines_Standard_Hough_Transform: { std::vector s_lines; /// 1. Use Standard Hough Transform cv::HoughLines( in_image, s_lines, rho_, theta_ * CV_PI/180, threshold_, minLineLength_, maxLineGap_ ); /// Show the result for( size_t i = 0; i < s_lines.size(); i++ ) { float r = s_lines[i][0], t = s_lines[i][1]; double cos_t = cos(t), sin_t = sin(t); double x0 = r*cos_t, y0 = r*sin_t; double alpha = 1000; cv::Point pt1( cvRound(x0 + alpha*(-sin_t)), cvRound(y0 + alpha*cos_t) ); cv::Point pt2( cvRound(x0 - alpha*(-sin_t)), cvRound(y0 - alpha*cos_t) ); #ifndef CV_VERSION_EPOCH cv::line( out_image, pt1, pt2, cv::Scalar(255,0,0), 3, cv::LINE_AA); #else cv::line( out_image, pt1, pt2, cv::Scalar(255,0,0), 3, CV_AA); #endif opencv_apps::Line line_msg; line_msg.pt1.x = pt1.x; line_msg.pt1.y = pt1.y; line_msg.pt2.x = pt2.x; line_msg.pt2.y = pt2.y; lines_msg.lines.push_back(line_msg); } break; } case hough_lines::HoughLines_Probabilistic_Hough_Transform: default: { std::vector p_lines; /// 2. Use Probabilistic Hough Transform cv::HoughLinesP( in_image, p_lines, rho_, theta_ * CV_PI/180, threshold_, minLineLength_, maxLineGap_ ); /// Show the result for( size_t i = 0; i < p_lines.size(); i++ ) { cv::Vec4i l = p_lines[i]; #ifndef CV_VERSION_EPOCH cv::line( out_image, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255,0,0), 3, cv::LINE_AA); #else cv::line( out_image, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255,0,0), 3, CV_AA); #endif opencv_apps::Line line_msg; line_msg.pt1.x = l[0]; line_msg.pt1.y = l[1]; line_msg.pt2.x = l[2]; line_msg.pt2.y = l[3]; lines_msg.lines.push_back(line_msg); } break; } } //-- Show what you got if( debug_view_) { cv::imshow( window_name_, out_image ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(lines_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &HoughLinesNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &HoughLinesNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Hough Lines Demo"; min_threshold_ = 50; max_threshold_ = 150; threshold_ = max_threshold_; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&HoughLinesNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "lines", 1); onInitPostProcess(); } }; bool HoughLinesNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(hough_lines::HoughLinesNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/lk_flow_nodelet.cpp000066400000000000000000000246731303612755600230330ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/lk_demo.cpp /** * This is a demo of Lukas-Kanade optical flow lkdemo(), */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include #include #include "std_srvs/Empty.h" #include "opencv_apps/LKFlowConfig.h" #include "opencv_apps/FlowArrayStamped.h" namespace lk_flow { class LKFlowNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; ros::ServiceServer initialize_points_service_; ros::ServiceServer delete_points_service_; ros::ServiceServer toggle_night_mode_service_; boost::shared_ptr it_; typedef lk_flow::LKFlowConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_; static bool need_config_update_; int MAX_COUNT; bool needToInit; bool nightMode; cv::Point2f point; bool addRemovePt; cv::Mat gray, prevGray; std::vector points[2]; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } #if 0 static void onMouse( int event, int x, int y, int /*flags*/, void* /*param*/ ) { if( event == CV_EVENT_LBUTTONDOWN ) { point = Point2f((float)x, (float)y); addRemovePt = true; } } #endif static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::FlowArrayStamped flows_msg; flows_msg.header = msg->header; if( debug_view_) { /// Create Trackbars for Thresholds cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); //cv::setMouseCallback( window_name_, onMouse, 0 ); if (need_config_update_) { reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } // Do the work cv::TermCriteria termcrit(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 20, 0.03); cv::Size subPixWinSize(10,10), winSize(31,31); if ( image.channels() > 1 ) { cv::cvtColor( image, gray, cv::COLOR_BGR2GRAY ); } else { image.copyTo(gray); } if( nightMode ) image = cv::Scalar::all(0); if( needToInit ) { // automatic initialization cv::goodFeaturesToTrack(gray, points[1], MAX_COUNT, 0.01, 10, cv::Mat(), 3, 0, 0.04); cv::cornerSubPix(gray, points[1], subPixWinSize, cv::Size(-1,-1), termcrit); addRemovePt = false; } else if( !points[0].empty() ) { std::vector status; std::vector err; if(prevGray.empty()) gray.copyTo(prevGray); cv::calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize, 3, termcrit, 0, 0.001); size_t i, k; for( i = k = 0; i < points[1].size(); i++ ) { if( addRemovePt ) { if( cv::norm(point - points[1][i]) <= 5 ) { addRemovePt = false; continue; } } if( !status[i] ) continue; points[1][k++] = points[1][i]; cv::circle( image, points[1][i], 3, cv::Scalar(0,255,0), -1, 8); cv::line( image, points[1][i], points[0][i], cv::Scalar(0,255,0), 1, 8, 0); opencv_apps::Flow flow_msg; opencv_apps::Point2D point_msg; opencv_apps::Point2D velocity_msg; point_msg.x = points[1][i].x; point_msg.y = points[1][i].y; velocity_msg.x = points[1][i].x-points[0][i].x; velocity_msg.y = points[1][i].y-points[0][i].y; flow_msg.point = point_msg; flow_msg.velocity = velocity_msg; flows_msg.flow.push_back(flow_msg); } points[1].resize(k); } if( addRemovePt && points[1].size() < (size_t)MAX_COUNT ) { std::vector tmp; tmp.push_back(point); cv::cornerSubPix( gray, tmp, winSize, cv::Size(-1,-1), termcrit); points[1].push_back(tmp[0]); addRemovePt = false; } needToInit = false; if( debug_view_) { cv::imshow(window_name_, image); char c = (char)cv::waitKey(1); //if( c == 27 ) // break; switch( c ) { case 'r': needToInit = true; break; case 'c': points[0].clear(); points[1].clear(); break; case 'n': nightMode = !nightMode; break; } } std::swap(points[1], points[0]); cv::swap(prevGray, gray); // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, image).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(flows_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } bool initialize_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { needToInit = true; return true; } bool delete_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { points[0].clear(); points[1].clear(); return true; } bool toggle_night_mode_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { nightMode = !nightMode; return true; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &LKFlowNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &LKFlowNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "LK Demo"; MAX_COUNT = 500; needToInit = true; nightMode = false; addRemovePt = false; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&LKFlowNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "flows", 1); initialize_points_service_ = pnh_->advertiseService("initialize_points", &LKFlowNodelet::initialize_points_cb, this); delete_points_service_ = pnh_->advertiseService("delete_points", &LKFlowNodelet::delete_points_cb, this); toggle_night_mode_service_ = pnh_->advertiseService("toggle_night_mode", &LKFlowNodelet::toggle_night_mode_cb, this); NODELET_INFO("Hot keys: "); NODELET_INFO("\tESC - quit the program"); NODELET_INFO("\tr - auto-initialize tracking"); NODELET_INFO("\tc - delete all the points"); NODELET_INFO("\tn - switch the \"night\" mode on/off"); //NODELET_INFO("To add/remove a feature point click it"); onInitPostProcess(); } }; bool LKFlowNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(lk_flow::LKFlowNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/nodelet.cpp000066400000000000000000000141321303612755600213030ustar00rootroot00000000000000// -*- mode: c++ -*- /********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2016, Ryohei Ueda. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/o2r other materials provided * with the distribution. * * Neither the name of the JSK Lab nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #include "opencv_apps/nodelet.h" namespace opencv_apps { void Nodelet::onInit() { connection_status_ = NOT_SUBSCRIBED; nh_.reset (new ros::NodeHandle (getMTNodeHandle ())); pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ())); pnh_->param("always_subscribe", always_subscribe_, false); pnh_->param("verbose_connection", verbose_connection_, false); if (!verbose_connection_) { nh_->param("verbose_connection", verbose_connection_, false); } // timer to warn when no connection in a few seconds ever_subscribed_ = false; timer_ = nh_->createWallTimer( ros::WallDuration(5), &Nodelet::warnNeverSubscribedCallback, this, /*oneshot=*/true); } void Nodelet::onInitPostProcess() { if (always_subscribe_) { subscribe(); } } void Nodelet::warnNeverSubscribedCallback(const ros::WallTimerEvent& event) { if (!ever_subscribed_) { NODELET_WARN("'%s' subscribes topics only with child subscribers.", nodelet::Nodelet::getName().c_str()); } } void Nodelet::connectionCallback(const ros::SingleSubscriberPublisher& pub) { if (verbose_connection_) { NODELET_INFO("New connection or disconnection is detected"); } if (!always_subscribe_) { boost::mutex::scoped_lock lock(connection_mutex_); for (size_t i = 0; i < publishers_.size(); i++) { ros::Publisher pub = publishers_[i]; if (pub.getNumSubscribers() > 0) { if (!ever_subscribed_) { ever_subscribed_ = true; } if (connection_status_ != SUBSCRIBED) { if (verbose_connection_) { NODELET_INFO("Subscribe input topics"); } subscribe(); connection_status_ = SUBSCRIBED; } return; } } if (connection_status_ == SUBSCRIBED) { if (verbose_connection_) { NODELET_INFO("Unsubscribe input topics"); } unsubscribe(); connection_status_ = NOT_SUBSCRIBED; } } } void Nodelet::imageConnectionCallback( const image_transport::SingleSubscriberPublisher& pub) { if (verbose_connection_) { NODELET_INFO("New image connection or disconnection is detected"); } if (!always_subscribe_) { boost::mutex::scoped_lock lock(connection_mutex_); for (size_t i = 0; i < image_publishers_.size(); i++) { image_transport::Publisher pub = image_publishers_[i]; if (pub.getNumSubscribers() > 0) { if (!ever_subscribed_) { ever_subscribed_ = true; } if (connection_status_ != SUBSCRIBED) { if (verbose_connection_) { NODELET_INFO("Subscribe input topics"); } subscribe(); connection_status_ = SUBSCRIBED; } return; } } if (connection_status_ == SUBSCRIBED) { if (verbose_connection_) { NODELET_INFO("Unsubscribe input topics"); } unsubscribe(); connection_status_ = NOT_SUBSCRIBED; } } } void Nodelet::cameraConnectionCallback( const image_transport::SingleSubscriberPublisher& pub) { cameraConnectionBaseCallback(); } void Nodelet::cameraInfoConnectionCallback( const ros::SingleSubscriberPublisher& pub) { cameraConnectionBaseCallback(); } void Nodelet::cameraConnectionBaseCallback() { if (verbose_connection_) { NODELET_INFO("New image connection or disconnection is detected"); } if (!always_subscribe_) { boost::mutex::scoped_lock lock(connection_mutex_); for (size_t i = 0; i < camera_publishers_.size(); i++) { image_transport::CameraPublisher pub = camera_publishers_[i]; if (pub.getNumSubscribers() > 0) { if (!ever_subscribed_) { ever_subscribed_ = true; } if (connection_status_ != SUBSCRIBED) { if (verbose_connection_) { NODELET_INFO("Subscribe input topics"); } subscribe(); connection_status_ = SUBSCRIBED; } return; } } if (connection_status_ == SUBSCRIBED) { if (verbose_connection_) { NODELET_INFO("Unsubscribe input topics"); } unsubscribe(); connection_status_ = NOT_SUBSCRIBED; } } } } ros-opencv-apps-1.11.14/src/nodelet/people_detect_nodelet.cpp000066400000000000000000000176671303612755600242170ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/peopledetect.cpp /** * Demonstrate the use of the HoG descriptor using * HOGDescriptor::hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector()); */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include #include "opencv_apps/PeopleDetectConfig.h" #include "opencv_apps/Rect.h" #include "opencv_apps/RectArrayStamped.h" namespace people_detect { class PeopleDetectNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef people_detect::PeopleDetectConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_; static bool need_config_update_; cv::HOGDescriptor hog_; double hit_threshold_; int win_stride_; int padding_; double scale0_; int group_threshold_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; hit_threshold_ = config_.hit_threshold; win_stride_ = config_.win_stride; padding_ = config_.padding; scale0_ = config_.scale0; group_threshold_ = config_.group_threshold; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::RectArrayStamped found_msg; found_msg.header = msg->header; // Do the work if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); } std::vector found, found_filtered; double t = (double)cv::getTickCount(); // run the detector with default parameters. to get a higher hit-rate // (and more false alarms, respectively), decrease the hitThreshold and // groupThreshold (set groupThreshold to 0 to turn off the grouping completely). hog_.detectMultiScale(frame, found, hit_threshold_, cv::Size(win_stride_, win_stride_), cv::Size(padding_, padding_), scale0_, group_threshold_); t = (double)cv::getTickCount() - t; NODELET_INFO("tdetection time = %gms", t*1000./cv::getTickFrequency()); size_t i, j; for( i = 0; i < found.size(); i++ ) { cv::Rect r = found[i]; for( j = 0; j < found.size(); j++ ) if( j != i && (r & found[j]) == r) break; if( j == found.size() ) found_filtered.push_back(r); } for( i = 0; i < found_filtered.size(); i++ ) { cv::Rect r = found_filtered[i]; // the HOG detector returns slightly larger rectangles than the real objects. // so we slightly shrink the rectangles to get a nicer output. r.x += cvRound(r.width*0.1); r.width = cvRound(r.width*0.8); r.y += cvRound(r.height*0.07); r.height = cvRound(r.height*0.8); cv::rectangle(frame, r.tl(), r.br(), cv::Scalar(0,255,0), 3); opencv_apps::Rect rect_msg; rect_msg.x = r.x; rect_msg.y = r.y; rect_msg.width = r.width; rect_msg.height = r.height; found_msg.rects.push_back(rect_msg); } //-- Show what you got if( debug_view_) { cv::imshow( window_name_, frame ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(found_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &PeopleDetectNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &PeopleDetectNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "people detector"; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&PeopleDetectNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector()); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "found", 1); onInitPostProcess(); } }; bool PeopleDetectNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(people_detect::PeopleDetectNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/phase_corr_nodelet.cpp000066400000000000000000000162161303612755600235150ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/master/samples/cpp/phase_corr.cpp #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include "opencv_apps/PhaseCorrConfig.h" #include "opencv_apps/Point2DStamped.h" namespace phase_corr { class PhaseCorrNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef phase_corr::PhaseCorrConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; cv::Mat curr, prev, curr64f, prev64f, hann; std::string window_name_; static bool need_config_update_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::Point2DStamped shift_msg; shift_msg.header = msg->header; // Do the work if ( frame.channels() > 1 ) { cv::cvtColor( frame, curr, cv::COLOR_BGR2GRAY ); } else { curr = frame; } if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); if (need_config_update_) { reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } if(prev.empty()) { prev = curr.clone(); cv::createHanningWindow(hann, curr.size(), CV_64F); } prev.convertTo(prev64f, CV_64F); curr.convertTo(curr64f, CV_64F); cv::Point2d shift = cv::phaseCorrelate(prev64f, curr64f, hann); double radius = cv::sqrt(shift.x*shift.x + shift.y*shift.y); if(radius > 0) { // draw a circle and line indicating the shift direction... cv::Point center(curr.cols >> 1, curr.rows >> 1); #ifndef CV_VERSION_EPOCH cv::circle(frame, center, (int)(radius*5), cv::Scalar(0, 255, 0), 3, cv::LINE_AA); cv::line(frame, center, cv::Point(center.x + (int)(shift.x*5), center.y + (int)(shift.y*5)), cv::Scalar(0, 255, 0), 3, cv::LINE_AA); #else cv::circle(frame, center, (int)(radius*5), cv::Scalar(0, 255, 0), 3, CV_AA); cv::line(frame, center, cv::Point(center.x + (int)(shift.x*5), center.y + (int)(shift.y*5)), cv::Scalar(0, 255, 0), 3, CV_AA); #endif // shift_msg.point.x = shift.x; shift_msg.point.y = shift.y; } //-- Show what you got if( debug_view_) { cv::imshow( window_name_, frame ); int c = cv::waitKey(1); } prev = curr.clone(); // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(shift_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &PhaseCorrNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &PhaseCorrNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "phase shift"; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&PhaseCorrNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "shift", 1); onInitPostProcess(); } }; bool PhaseCorrNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(phase_corr::PhaseCorrNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/segment_objects_nodelet.cpp000066400000000000000000000222611303612755600245400ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/segment_objects.cpp /** * This program demonstrated a simple method of connected components clean up of background subtraction */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include #include "opencv_apps/SegmentObjectsConfig.h" #include "std_srvs/Empty.h" #include "std_msgs/Float64.h" #include "opencv_apps/Contour.h" #include "opencv_apps/ContourArray.h" #include "opencv_apps/ContourArrayStamped.h" namespace segment_objects { class SegmentObjectsNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_, area_pub_; ros::ServiceServer update_bg_model_service_; boost::shared_ptr it_; typedef segment_objects::SegmentObjectsConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_; static bool need_config_update_; #ifndef CV_VERSION_EPOCH cv::Ptr bgsubtractor; #else cv::BackgroundSubtractorMOG bgsubtractor; #endif bool update_bg_model; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::ContourArrayStamped contours_msg; contours_msg.header = msg->header; // Do the work cv::Mat bgmask, out_frame; if( debug_view_) { /// Create Trackbars for Thresholds cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); if (need_config_update_) { reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } #ifndef CV_VERSION_EPOCH bgsubtractor->apply(frame, bgmask, update_bg_model ? -1 : 0); #else bgsubtractor(frame, bgmask, update_bg_model ? -1 : 0); #endif //refineSegments(tmp_frame, bgmask, out_frame); int niters = 3; std::vector > contours; std::vector hierarchy; cv::Mat temp; cv::dilate(bgmask, temp, cv::Mat(), cv::Point(-1,-1), niters); cv::erode(temp, temp, cv::Mat(), cv::Point(-1,-1), niters*2); cv::dilate(temp, temp, cv::Mat(), cv::Point(-1,-1), niters); cv::findContours( temp, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ); out_frame = cv::Mat::zeros(frame.size(), CV_8UC3); if( contours.size() == 0 ) return; // iterate through all the top-level contours, // draw each connected component with its own random color int idx = 0, largestComp = 0; double maxArea = 0; for( ; idx >= 0; idx = hierarchy[idx][0] ) { const std::vector& c = contours[idx]; double area = fabs(cv::contourArea(cv::Mat(c))); if( area > maxArea ) { maxArea = area; largestComp = idx; } } cv::Scalar color( 0, 0, 255 ); cv::drawContours( out_frame, contours, largestComp, color, CV_FILLED, 8, hierarchy ); std_msgs::Float64 area_msg; area_msg.data = maxArea; for( size_t i = 0; i< contours.size(); i++ ) { opencv_apps::Contour contour_msg; for ( size_t j = 0; j < contours[i].size(); j++ ) { opencv_apps::Point2D point_msg; point_msg.x = contours[i][j].x; point_msg.y = contours[i][j].y; contour_msg.points.push_back(point_msg); } contours_msg.contours.push_back(contour_msg); } //-- Show what you got if( debug_view_) { cv::imshow( window_name_, out_frame ); int keycode = cv::waitKey(1); //if( keycode == 27 ) // break; if( keycode == ' ' ) { update_bg_model = !update_bg_model; NODELET_INFO("Learn background is in state = %d",update_bg_model); } } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, out_frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(contours_msg); area_pub_.publish(area_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } bool update_bg_model_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { update_bg_model = !update_bg_model; NODELET_INFO("Learn background is in state = %d",update_bg_model); return true; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &SegmentObjectsNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &SegmentObjectsNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "segmented"; update_bg_model = true; #ifndef CV_VERSION_EPOCH bgsubtractor = cv::createBackgroundSubtractorMOG2(); #else bgsubtractor.set("noiseSigma", 10); #endif reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&SegmentObjectsNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "contours", 1); area_pub_ = advertise(*pnh_, "area", 1); update_bg_model_service_ = pnh_->advertiseService("update_bg_model", &SegmentObjectsNodelet::update_bg_model_cb, this); onInitPostProcess(); } }; bool SegmentObjectsNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(segment_objects::SegmentObjectsNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/simple_compressed_example_nodelet.cpp000066400000000000000000000163751303612755600266260ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2015, Tal Regev. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages /** * This is a demo of Simple Example from wiki tutorial */ #include #include #include #include #include #include #include #include #include #include namespace simple_compressed_example { static const std::string OPENCV_WINDOW = "Image window"; class ImageConverter { ros::NodeHandle nh_; ros::Subscriber image_sub_; ros::Publisher image_pub_; bool debug_view_; public: ImageConverter() { // Subscrive to input video feed and publish output video feed image_sub_ = nh_.subscribe("image/compressed", 1, &ImageConverter::imageCb,this); image_pub_ = nh_.advertise("/image_converter/output_video/compressed", 1); ros::NodeHandle pnh_("~"); pnh_.param("debug_view", debug_view_, false); if( debug_view_) { cv::namedWindow(OPENCV_WINDOW); } } ~ImageConverter() { if( debug_view_) { cv::destroyWindow(OPENCV_WINDOW); } } void imageCb(const sensor_msgs::CompressedImageConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try { /* Supporting CompressedImage in cv_bridge has been started from 1.11.9 (2015-11-29) note : hydro 1.10.18, indigo : 1.11.13 ... https://github.com/ros-perception/vision_opencv/pull/70 */ #ifndef CV_BRIDGE_COMPRESSED_IMAGE_IS_NOT_SUPPORTED cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); #else //cv_ptr = cv_bridge::toCvCopyImpl(matFromImage(msg), msg.header, sensor_msgs::image_encodings::BGR8, sensor_msgs::image_encodings::BGR8); //cv::Mat matFromImage(const sensor_msgs::CompressedImage& source) cv::Mat jpegData(1,msg->data.size(),CV_8UC1); jpegData.data = const_cast(&msg->data[0]); cv::InputArray data(jpegData); cv::Mat image = cv::imdecode(data,cv::IMREAD_COLOR); //cv_ptr = cv_bridge::toCvCopyImpl(bgrMat, msg->header, sensor_msgs::image_encodings::BGR8, sensor_msgs::image_encodings::BGR8); sensor_msgs::Image ros_image; ros_image.header = msg->header; ros_image.height = image.rows; ros_image.width = image.cols; ros_image.encoding = sensor_msgs::image_encodings::BGR8; ros_image.is_bigendian = false; ros_image.step = image.cols * image.elemSize(); size_t size = ros_image.step * image.rows; ros_image.data.resize(size); if (image.isContinuous()) { memcpy((char*)(&ros_image.data[0]), image.data, size); } else { // Copy by row by row uchar* ros_data_ptr = (uchar*)(&ros_image.data[0]); uchar* cv_data_ptr = image.data; for (int i = 0; i < image.rows; ++i) { memcpy(ros_data_ptr, cv_data_ptr, ros_image.step); ros_data_ptr += ros_image.step; cv_data_ptr += image.step; } } cv_ptr = cv_bridge::toCvCopy(ros_image, sensor_msgs::image_encodings::BGR8); #endif } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } // Draw an example circle on the video stream if (cv_ptr->image.rows > 110 && cv_ptr->image.cols > 110) cv::circle(cv_ptr->image, cv::Point(cv_ptr->image.cols/2, cv_ptr->image.rows/2), 100, CV_RGB(255,0,0)); if( debug_view_) { // Update GUI Window cv::imshow(OPENCV_WINDOW, cv_ptr->image); cv::waitKey(3); } // Output modified video stream #ifndef CV_BRIDGE_COMPRESSED_IMAGE_IS_NOT_SUPPORTED image_pub_.publish(cv_ptr->toCompressedImageMsg()); #else image_pub_.publish(toCompressedImageMsg(cv_ptr)); #endif } #ifdef CV_BRIDGE_COMPRESSED_IMAGE_IS_NOT_SUPPORTED sensor_msgs::CompressedImage toCompressedImageMsg(cv_bridge::CvImagePtr cv_ptr) const { sensor_msgs::CompressedImage ros_image; const std::string dst_format = std::string(); ros_image.header = cv_ptr->header; cv::Mat image; if(cv_ptr->encoding != sensor_msgs::image_encodings::BGR8) { cv_bridge::CvImagePtr tempThis = boost::make_shared(*cv_ptr); cv_bridge::CvImagePtr temp = cv_bridge::cvtColor(tempThis,sensor_msgs::image_encodings::BGR8); cv_ptr->image = temp->image; } else { image = cv_ptr->image; } std::vector buf; if (dst_format.empty() || dst_format == "jpg") { ros_image.format = "jpg"; cv::imencode(".jpg", image, buf); } if (dst_format == "png") { ros_image.format = "png"; cv::imencode(".png", image, buf); } //TODO: check this formats (on rviz) and add more formats //from http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) if (dst_format == "jp2") { ros_image.format = "jp2"; cv::imencode(".jp2", image, buf); } if (dst_format == "bmp") { ros_image.format = "bmp"; cv::imencode(".bmp", image, buf); } if (dst_format == "tif") { ros_image.format = "tif"; cv::imencode(".tif", image, buf); } ros_image.data = buf; return ros_image; } #endif }; class SimpleCompressedExampleNodelet : public nodelet::Nodelet { public: virtual void onInit() { simple_compressed_example::ImageConverter ic; ros::spin(); } }; } #include PLUGINLIB_EXPORT_CLASS(simple_compressed_example::SimpleCompressedExampleNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/simple_example_nodelet.cpp000066400000000000000000000077061303612755600244000ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2015, Tal Regev. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages /** * This is a demo of Simple Example from wiki tutorial */ #include #include #include #include #include #include #include #include namespace simple_example { static const std::string OPENCV_WINDOW = "Image window"; class ImageConverter { ros::NodeHandle nh_; image_transport::ImageTransport it_; image_transport::Subscriber image_sub_; image_transport::Publisher image_pub_; bool debug_view_; public: ImageConverter() : it_(nh_) { // Subscrive to input video feed and publish output video feed image_sub_ = it_.subscribe("image", 1, &ImageConverter::imageCb, this); image_pub_ = it_.advertise("/image_converter/output_video/raw", 1); ros::NodeHandle pnh_("~"); pnh_.param("debug_view", debug_view_, false); if( debug_view_) { cv::namedWindow(OPENCV_WINDOW); } } ~ImageConverter() { if( debug_view_) { cv::destroyWindow(OPENCV_WINDOW); } } void imageCb(const sensor_msgs::ImageConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } // Draw an example circle on the video stream if (cv_ptr->image.rows > 110 && cv_ptr->image.cols > 110) cv::circle(cv_ptr->image, cv::Point(cv_ptr->image.cols/2, cv_ptr->image.rows/2), 100, CV_RGB(255,0,0)); if( debug_view_) { // Update GUI Window cv::imshow(OPENCV_WINDOW, cv_ptr->image); cv::waitKey(3); } // Output modified video stream image_pub_.publish(cv_ptr->toImageMsg()); } }; class SimpleExampleNodelet : public nodelet::Nodelet { public: virtual void onInit() { simple_example::ImageConverter ic; ros::spin(); } }; } #include PLUGINLIB_EXPORT_CLASS(simple_example::SimpleExampleNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/simple_flow_nodelet.cpp000066400000000000000000000207601303612755600237070ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/simpleflow_demo.cpp /** * This is a demo of SimpleFlow optical flow algorithm */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #if CV_MAJOR_VERSION == 3 #include #endif #include #include "opencv_apps/SimpleFlowConfig.h" #include "opencv_apps/FlowArrayStamped.h" namespace simple_flow { class SimpleFlowNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef simple_flow::SimpleFlowConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; int subscriber_count_; ros::Time prev_stamp_; std::string window_name_; static bool need_config_update_; int scale_; cv::Mat gray, prevGray; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; scale_ = config_.scale; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame_src = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; /// Convert it to gray cv::Mat frame; if ( frame_src.channels() > 1 ) { frame = frame_src; } else { cv::cvtColor( frame_src, frame, cv::COLOR_GRAY2BGR ); } cv::resize(frame, gray, cv::Size(frame.cols/(double)MAX(1,scale_), frame.rows/(double)MAX(1,scale_)), 0, 0, CV_INTER_AREA); if(prevGray.empty()) gray.copyTo(prevGray); if (gray.rows != prevGray.rows && gray.cols != prevGray.cols) { NODELET_WARN("Images should be of equal sizes"); gray.copyTo(prevGray); } if (frame.type() != 16) { NODELET_ERROR("Images should be of equal type CV_8UC3"); } // Messages opencv_apps::FlowArrayStamped flows_msg; flows_msg.header = msg->header; // Do the work cv::Mat flow; if( debug_view_) { cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); cv::createTrackbar( "Scale", window_name_, &scale_, 24, trackbarCallback); if (need_config_update_) { config_.scale = scale_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } float start = (float)cv::getTickCount(); #if CV_MAJOR_VERSION == 3 cv::optflow::calcOpticalFlowSF(gray, prevGray, #else cv::calcOpticalFlowSF(gray, prevGray, #endif flow, 3, 2, 4, 4.1, 25.5, 18, 55.0, 25.5, 0.35, 18, 55.0, 25.5, 10); NODELET_INFO("calcOpticalFlowSF : %lf sec", (cv::getTickCount() - start) / cv::getTickFrequency()); //writeOpticalFlowToFile(flow, file); int cols = flow.cols; int rows = flow.rows; double scale_col = frame.cols/(double)flow.cols; double scale_row = frame.rows/(double)flow.rows; for (int i= 0; i < rows; ++i) { for (int j = 0; j < cols; ++j) { cv::Vec2f flow_at_point = flow.at(i, j); cv::line(frame, cv::Point(scale_col*j, scale_row*i), cv::Point(scale_col*(j+flow_at_point[0]), scale_row*(i+flow_at_point[1])), cv::Scalar(0,255,0), 1, 8, 0 ); opencv_apps::Flow flow_msg; opencv_apps::Point2D point_msg; opencv_apps::Point2D velocity_msg; point_msg.x = scale_col*j; point_msg.y = scale_row*i; velocity_msg.x = scale_col*flow_at_point[0]; velocity_msg.y = scale_row*flow_at_point[1]; flow_msg.point = point_msg; flow_msg.velocity = velocity_msg; flows_msg.flow.push_back(flow_msg); } } //cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); /// Apply Canny edge detector //Canny( src_gray, edges, 50, 200, 3 ); //-- Show what you got if ( debug_view_) { cv::imshow( window_name_, frame ); int c = cv::waitKey(1); } cv::swap(prevGray, gray); // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(flows_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &SimpleFlowNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &SimpleFlowNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "simpleflow_demo"; scale_ = 4.0; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&SimpleFlowNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "flows", 1); onInitPostProcess(); } }; bool SimpleFlowNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(simple_flow::SimpleFlowNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/smoothing_nodelet.cpp000066400000000000000000000171121303612755600233730ustar00rootroot00000000000000// -*- mode: c++ -*- /********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2016, JSK Lab * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the JSK Lab nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/opencv/opencv/blob/2.4/samples/cpp/tutorial_code/ImgProc/Smoothing.cpp /** * file Smoothing.cpp * brief Sample code for simple filters * author OpenCV team */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include "opencv2/imgproc/imgproc.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/features2d/features2d.hpp" #include #include "opencv_apps/SmoothingConfig.h" /// Global Variables int MAX_KERNEL_LENGTH = 31; namespace smoothing { class SmoothingNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; boost::shared_ptr it_; typedef smoothing::SmoothingConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_; static bool need_config_update_; int kernel_size_; void reconfigureCallback(Config &new_config, uint32_t level) { config_ = new_config; kernel_size_ = (config_.kernel_size/2)*2+1; // kernel_size must be odd number } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat in_image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; if( debug_view_) { /// Create Trackbars for Thresholds char kernel_label[] = "Kernel Size : "; cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); cv::createTrackbar( kernel_label, window_name_, &kernel_size_, MAX_KERNEL_LENGTH, trackbarCallback); if (need_config_update_) { kernel_size_ = (kernel_size_/2)*2+1; // kernel_size must be odd number config_.kernel_size = kernel_size_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } cv::Mat out_image = in_image.clone(); int i = kernel_size_; switch (config_.filter_type) { case smoothing::Smoothing_Homogeneous_Blur: { /// Applying Homogeneous blur ROS_DEBUG_STREAM("Applying Homogeneous blur with kernel size " << i ); cv::blur( in_image, out_image, cv::Size( i, i ), cv::Point(-1,-1) ); break; } case smoothing::Smoothing_Gaussian_Blur: { /// Applying Gaussian blur ROS_DEBUG_STREAM("Applying Gaussian blur with kernel size " << i ); cv::GaussianBlur( in_image, out_image, cv::Size( i, i ), 0, 0 ); break; } case smoothing::Smoothing_Median_Blur: { /// Applying Median blur ROS_DEBUG_STREAM("Applying Median blur with kernel size " << i ); cv::medianBlur ( in_image, out_image, i ); break; } case smoothing::Smoothing_Bilateral_Filter: { /// Applying Bilateral Filter ROS_DEBUG_STREAM("Applying Bilateral blur with kernel size " << i ); cv::bilateralFilter ( in_image, out_image, i, i*2, i/2 ); break; } } //-- Show what you got if( debug_view_) { cv::imshow( window_name_, out_image ); int c = cv::waitKey(1); } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg(); img_pub_.publish(out_img); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &SmoothingNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &SmoothingNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Smoothing Demo"; kernel_size_ = 7; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&SmoothingNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); onInitPostProcess(); } }; bool SmoothingNodelet::need_config_update_ = false; } #include PLUGINLIB_EXPORT_CLASS(smoothing::SmoothingNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/threshold_nodelet.cpp000066400000000000000000000143171303612755600233640ustar00rootroot00000000000000// -*- mode: c++ -*- /********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2016, JSK Lab * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the JSK Lab nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/opencv/opencv/blob/2.4/samples/cpp/tutorial_code/ImgProc/Threshold.cpp /** * This is a demo of threshold image processing, */ #include #include #include #include #include #include "opencv2/imgproc/imgproc.hpp" #include "opencv_apps/nodelet.h" #include "opencv_apps/ThresholdConfig.h" #include namespace threshold { class ThresholdNodelet : public opencv_apps::Nodelet { //////////////////////////////////////////////////////// // Dynamic Reconfigure //////////////////////////////////////////////////////// typedef threshold::ThresholdConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; std::string window_name_; image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; boost::shared_ptr it_; boost::mutex mutex_; int threshold_type_; int max_binary_value_; int threshold_value_; bool apply_otsu_; void imageCallbackWithInfo( const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera( "image", 1, &ThresholdNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 1, &ThresholdNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } void reconfigureCallback(Config& config, uint32_t level) { boost::mutex::scoped_lock lock(mutex_); config_ = config; threshold_value_ = config.threshold; threshold_type_ = config.threshold_type; max_binary_value_ = config.max_binary; apply_otsu_ = config.apply_otsu; } void do_work(const sensor_msgs::Image::ConstPtr& image_msg, const std::string input_frame_from_msg) { try { cv::Mat src_image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8)->image; cv::Mat gray_image; cv::cvtColor(src_image, gray_image, cv::COLOR_BGR2GRAY); cv::Mat result_image; if (apply_otsu_) { threshold_type_ |= CV_THRESH_OTSU; } cv::threshold(gray_image, result_image, threshold_value_, max_binary_value_, threshold_type_); //-- Show what you got if (debug_view_) { cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); cv::imshow(window_name_, result_image); int c = cv::waitKey(1); } img_pub_.publish(cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::MONO8, result_image).toImageMsg()); } catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr( new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } //////////////////////////////////////////////////////// // Dynamic Reconfigure //////////////////////////////////////////////////////// reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&ThresholdNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); onInitPostProcess(); } }; } #include PLUGINLIB_EXPORT_CLASS(threshold::ThresholdNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/src/nodelet/watershed_segmentation_nodelet.cpp000066400000000000000000000271521303612755600261340ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2014, Kei Okada. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/watershed.cpp /** * This program demonstrates the famous watershed segmentation algorithm in OpenCV: watershed() */ #include #include "opencv_apps/nodelet.h" #include #include #include #include #include #include #include "opencv_apps/WatershedSegmentationConfig.h" #include "opencv_apps/Contour.h" #include "opencv_apps/ContourArray.h" #include "opencv_apps/ContourArrayStamped.h" #include "opencv_apps/Point2DArray.h" namespace watershed_segmentation { class WatershedSegmentationNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; ros::Subscriber add_seed_points_sub_; boost::shared_ptr it_; typedef watershed_segmentation::WatershedSegmentationConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; Config config_; boost::shared_ptr reconfigure_server_; bool debug_view_; ros::Time prev_stamp_; std::string window_name_, segment_name_; static bool need_config_update_; static bool on_mouse_update_; static int on_mouse_event_; static int on_mouse_x_; static int on_mouse_y_; static int on_mouse_flags_; cv::Mat markerMask; cv::Point prevPt; static void onMouse( int event, int x, int y, int flags, void* ) { on_mouse_update_ = true; on_mouse_event_ = event; on_mouse_x_ = x; on_mouse_y_ = y; on_mouse_flags_ = flags; } void reconfigureCallback(watershed_segmentation::WatershedSegmentationConfig &new_config, uint32_t level) { config_ = new_config; } const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) { if (frame.empty()) return image_frame; return frame; } void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { do_work(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } static void trackbarCallback( int, void* ) { need_config_update_ = true; } void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) { // Work on the image. try { // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; // Messages opencv_apps::ContourArrayStamped contours_msg; contours_msg.header = msg->header; // Do the work //std::vector faces; cv::Mat imgGray; /// Initialize if ( markerMask.empty() ) { cv::cvtColor(frame, markerMask, cv::COLOR_BGR2GRAY); cv::cvtColor(markerMask, imgGray, cv::COLOR_GRAY2BGR); markerMask = cv::Scalar::all(0); } if( debug_view_) { cv::imshow( window_name_, frame); cv::setMouseCallback( window_name_, onMouse, 0 ); if (need_config_update_) { reconfigure_server_->updateConfig(config_); need_config_update_ = false; } if ( on_mouse_update_ ) { int event = on_mouse_event_; int x = on_mouse_x_; int y = on_mouse_y_; int flags = on_mouse_flags_; if( x < 0 || x >= frame.cols || y < 0 || y >= frame.rows ) return; if( event == cv::EVENT_LBUTTONUP || !(flags & cv::EVENT_FLAG_LBUTTON) ) prevPt = cv::Point(-1,-1); else if( event == cv::EVENT_LBUTTONDOWN ) prevPt = cv::Point(x,y); else if( event == cv::EVENT_MOUSEMOVE && (flags & cv::EVENT_FLAG_LBUTTON) ) { cv::Point pt(x, y); if( prevPt.x < 0 ) prevPt = pt; cv::line( markerMask, prevPt, pt, cv::Scalar::all(255), 5, 8, 0 ); cv::line( frame, prevPt, pt, cv::Scalar::all(255), 5, 8, 0 ); prevPt = pt; cv::imshow(window_name_, markerMask); } } cv::waitKey(1); } int i, j, compCount = 0; std::vector > contours; std::vector hierarchy; cv::findContours(markerMask, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); if( contours.empty() ) { NODELET_WARN("contnorus is empty"); return; //continue; } cv::Mat markers(markerMask.size(), CV_32S); markers = cv::Scalar::all(0); int idx = 0; for( ; idx >= 0; idx = hierarchy[idx][0], compCount++ ) cv::drawContours(markers, contours, idx, cv::Scalar::all(compCount+1), -1, 8, hierarchy, INT_MAX); if( compCount == 0 ) { NODELET_WARN("compCount is 0"); return; //continue; } for( size_t i = 0; i< contours.size(); i++ ) { opencv_apps::Contour contour_msg; for ( size_t j = 0; j < contours[i].size(); j++ ) { opencv_apps::Point2D point_msg; point_msg.x = contours[i][j].x; point_msg.y = contours[i][j].y; contour_msg.points.push_back(point_msg); } contours_msg.contours.push_back(contour_msg); } std::vector colorTab; for( i = 0; i < compCount; i++ ) { int b = cv::theRNG().uniform(0, 255); int g = cv::theRNG().uniform(0, 255); int r = cv::theRNG().uniform(0, 255); colorTab.push_back(cv::Vec3b((uchar)b, (uchar)g, (uchar)r)); } double t = (double)cv::getTickCount(); cv::watershed( frame, markers ); t = (double)cv::getTickCount() - t; NODELET_INFO( "execution time = %gms", t*1000./cv::getTickFrequency() ); cv::Mat wshed(markers.size(), CV_8UC3); // paint the watershed image for( i = 0; i < markers.rows; i++ ) for( j = 0; j < markers.cols; j++ ) { int index = markers.at(i,j); if( index == -1 ) wshed.at(i,j) = cv::Vec3b(255,255,255); else if( index <= 0 || index > compCount ) wshed.at(i,j) = cv::Vec3b(0,0,0); else wshed.at(i,j) = colorTab[index - 1]; } wshed = wshed*0.5 + imgGray*0.5; //-- Show what you got if( debug_view_) { cv::imshow( segment_name_, wshed ); int c = cv::waitKey(1); //if( (char)c == 27 ) // break; if( (char)c == 'r' ) { markerMask = cv::Scalar::all(0); } } // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, wshed).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(contours_msg); } catch (cv::Exception &e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } void add_seed_point_cb(const opencv_apps::Point2DArray& msg) { if ( msg.points.size() == 0 ) { markerMask = cv::Scalar::all(0); } else { for(size_t i = 0; i < msg.points.size(); i++ ) { cv::Point pt0(msg.points[i].x, msg.points[i].y); cv::Point pt1(pt0.x + 1, pt0.y + 1); cv::line( markerMask, pt0, pt1, cv::Scalar::all(255), 5, 8, 0 ); } } } void subscribe() { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) cam_sub_ = it_->subscribeCamera("image", 3, &WatershedSegmentationNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe("image", 3, &WatershedSegmentationNodelet::imageCallback, this); } void unsubscribe() { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } public: virtual void onInit() { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("debug_view", debug_view_, false); if (debug_view_) { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "roughly mark the areas to segment on the image"; segment_name_ = "watershed transform"; prevPt.x = -1; prevPt.y = -1; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind(&WatershedSegmentationNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); add_seed_points_sub_ = pnh_->subscribe("add_seed_points", 1, &WatershedSegmentationNodelet::add_seed_point_cb, this); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "contours", 1); NODELET_INFO("This program demonstrates the famous watershed segmentation algorithm in OpenCV: watershed()"); NODELET_INFO("Hot keys: "); NODELET_INFO("\tESC - quit the program"); NODELET_INFO("\tr - restore the original image"); NODELET_INFO("\tw or SPACE - run watershed segmentation algorithm"); NODELET_INFO("\t\t(before running it, *roughly* mark the areas to segment on the image)"); NODELET_INFO("\t (before that, roughly outline several markers on the image)"); onInitPostProcess(); } }; bool WatershedSegmentationNodelet::need_config_update_ = false; bool WatershedSegmentationNodelet::on_mouse_update_ = false; int WatershedSegmentationNodelet::on_mouse_event_ = 0; int WatershedSegmentationNodelet::on_mouse_x_ = 0; int WatershedSegmentationNodelet::on_mouse_y_ = 0; int WatershedSegmentationNodelet::on_mouse_flags_ = 0; } #include PLUGINLIB_EXPORT_CLASS(watershed_segmentation::WatershedSegmentationNodelet, nodelet::Nodelet); ros-opencv-apps-1.11.14/test/000077500000000000000000000000001303612755600157025ustar00rootroot00000000000000ros-opencv-apps-1.11.14/test/CMakeLists.txt000066400000000000000000000047721303612755600204540ustar00rootroot00000000000000# Tests simple calibration dataset catkin_download_test_data(face_detector_withface_test_diamondback.bag http://download.ros.org/data/face_detector/face_detector_withface_test_diamondback.bag DESTINATION ${CMAKE_CURRENT_SOURCE_DIR} MD5 59126117e049e69d577b7ee27251a6f8 ) catkin_download_test_data(vslam_tutorial.bag http://download.ros.org/data/vslam_system/vslam_tutorial.bag DESTINATION ${CMAKE_CURRENT_BINARY_DIR} MD5 f5aece448b7af00a38a993eb71400806 ) add_custom_command(OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/vslam_tutorial.bag DEPENDS vslam_tutorial.bag COMMAND rosbag reindex vslam_tutorial.bag --output-dir ${CMAKE_CURRENT_SOURCE_DIR} ) add_custom_target(vslam_tutorial_bag DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/vslam_tutorial.bag) add_dependencies(tests vslam_tutorial_bag) #add_rostest(test-apps.test) add_rostest(test-adding_images.test ARGS gui:=false) add_rostest(test-smoothing.test ARGS gui:=false) add_rostest(test-threshold.test ARGS gui:=false) add_rostest(test-edge_detection.test ARGS gui:=false) add_rostest(test-hough_lines.test ARGS gui:=false) add_rostest(test-hough_circles.test ARGS gui:=false) add_rostest(test-find_contours.test ARGS gui:=false) add_rostest(test-convex_hull.test ARGS gui:=false) add_rostest(test-general_contours.test ARGS gui:=false) add_rostest(test-contour_moments.test ARGS gui:=false) if(OpenCV_VERSION VERSION_LESS "3.0") add_rostest(test-face_detection.test ARGS gui:=false) else() add_rostest(test-face_detection.test ARGS gui:=false use_opencv3:=true) endif() add_rostest(test-goodfeature_track.test ARGS gui:=false) add_rostest(test-camshift.test ARGS gui:=false) add_rostest(test-fback_flow.test ARGS gui:=false) add_rostest(test-lk_flow.test ARGS gui:=false) add_rostest(test-people_detect.test ARGS gui:=false) add_rostest(test-phase_corr.test ARGS gui:=false) add_rostest(test-segment_objects.test ARGS gui:=false) # simple flow requires opencv-contrib https://github.com/ros-perception/vision_opencv/issues/108 if(OPENCV_HAVE_OPTFLOW) add_rostest(test-simple_flow.test ARGS gui:=false) endif() add_rostest(test-watershed_segmentation.test ARGS gui:=false) add_rostest(test-simple_example.test ARGS gui:=false) add_rostest(test-simple_compressed_example.test ARGS gui:=false) if(roslaunch_VERSION VERSION_LESS "1.11.1") message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}") else() file(GLOB LAUNCH_FILES *.test) foreach(LAUNCH_FILE ${LAUNCH_FILES}) roslaunch_add_file_check(${LAUNCH_FILE}) endforeach() endif() ros-opencv-apps-1.11.14/test/test-adding_images.test000066400000000000000000000032361303612755600223370ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-camshift.test000066400000000000000000000031761303612755600213650ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-contour_moments.test000066400000000000000000000023271303612755600230170ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-convex_hull.test000066400000000000000000000023331303612755600221070ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-edge_detection.test000066400000000000000000000063401303612755600225250ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-face_detection.test000066400000000000000000000024631303612755600225210ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-fback_flow.test000066400000000000000000000022501303612755600216540ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-find_contours.test000066400000000000000000000023731303612755600224410ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-general_contours.test000066400000000000000000000033051303612755600231320ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-goodfeature_track.test000066400000000000000000000025171303612755600232550ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-hough_circles.test000066400000000000000000000024541303612755600224030ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-hough_lines.test000066400000000000000000000046671303612755600221010ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-lk_flow.test000066400000000000000000000026421303612755600212210ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-people_detect.test000066400000000000000000000023031303612755600223720ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-phase_corr.test000066400000000000000000000022461303612755600217110ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-segment_objects.test000066400000000000000000000024141303612755600227340ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-simple_compressed_example.test000066400000000000000000000024511303612755600250120ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-simple_example.test000066400000000000000000000024531303612755600225700ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-simple_flow.test000066400000000000000000000023261303612755600221030ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-smoothing.test000066400000000000000000000102321303612755600215650ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-threshold.test000066400000000000000000000024721303612755600215610ustar00rootroot00000000000000 ros-opencv-apps-1.11.14/test/test-watershed_segmentation.test000066400000000000000000000033611303612755600243260ustar00rootroot00000000000000