diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/CHANGELOG.rst b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/CHANGELOG.rst new file mode 100644 index 00000000000..be3c16ca82b --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/CHANGELOG.rst @@ -0,0 +1,396 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_recognition_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.6 (2018-11-02) +------------------ +* Install sample and test into SHARE_DESTINATION (`#2345 `_) +* jsk_pcl_ros: support lazy mode for pointcloud_screenpoint nodelet (`#2277 `_)p + * add 'bool no_update' to TransformScreenpoint.srv + +* Contributors: Yuki Furuta, Yuto Uchimi + +1.2.5 (2018-04-09) +------------------ + +1.2.4 (2018-01-12) +------------------ + +1.2.3 (2017-11-23) +------------------ + +1.2.2 (2017-07-23) +------------------ + +1.2.1 (2017-07-15) +------------------ + +1.2.0 (2017-07-15) +------------------ +* [ADD NEW MESSAGE] Add Object.msg, ObjectArray.msg to represent object property (`#2148 `_) + * Message to represent object property (Object.msg/ObjectArray.msg) + +* Enhance PeoplePoseEstimation2D (`#2162 `_ + * scripts/people_pose_array_to_pose_array.py: Visualize people 3D pose on rviz in sample + +* Contributors: Kentaro Wada, Yuki Furuta + +1.1.3 (2017-07-07) +------------------ +* [jsk_perception] apply candidates node supports topic update (`#2143 `_) + * update Label msg API + * add Label and LabelArray msg +* Rewrite KinfuNodelet with some enhancements and new features (`#2129 `_) + * Save kinfu mesh model with bbox and ground frame id + * Create polygon mesh with bbox request in kinfu + * Create jsk_recognition_msgs/TrackingStatus.msg and use it in Kinfue +* [jsk_perception] PeoplePoseEstimation2D (`#2115 `_) + * [jsk_recogntion_msgs/PoseArray] Add score + * [jsk_perception/people_pose_estimation_2d] Modified type of PeoplePose.msg + * [jsk_recognition_msgs] Add people_pose msgs +* Contributors: Kei Okada, Kentaro Wada, Shingo Kitagawa, Iori Yanokura + +1.1.2 (2017-06-16) +------------------ +* [jsk_recognition_msgs] add segment messages. (`#2047 `_ ) +* Generate README by script (`#2064 `_ ) +* Contributors: Kentaro Wada, Masaki Murooka + +1.1.1 (2017-03-04) +------------------ + +1.1.0 (2017-02-09) +------------------ + +1.0.4 (2017-02-09) +------------------ + +1.0.3 (2017-02-08) +------------------ +* Compute box overlap and publish it (`#1993 `_ ) + * add Accuracy.msg +* Contributors: Kentaro Wada + +1.0.2 (2017-01-12) +------------------ + +1.0.1 (2016-12-13) +------------------ + +1.0.0 (2016-12-12) +------------------ + +0.3.29 (2016-10-30) +------------------- + +0.3.28 (2016-10-29) +------------------- +* [Major Release] Copy jsk_pcl_ros/srv and jsk_perception/srv files to jsk_recognition_msgs (`#1914 `_ ) +* Copy deprecated srv files to jsk_recognition_msgs + - jsk_pcl_ros/srv -> jsk_recognition_msgs/srv + - jsk_perception/srv -> jsk_recognition_msgs/srv + TODO + - 1. Migrate current code for srv files in jsk_recognition_msgs + - 2. Remove srv files in jsk_pcl_ros and jsk_perception +* Contributors: Kei Okada, Kentaro Wada + +0.3.27 (2016-10-29) +------------------- + +0.3.26 (2016-10-27) +------------------- +* Apply context to label probability (`#1901 `_) +* Contributors: Kentaro Wada + +0.3.25 (2016-09-16) +------------------- + +0.3.24 (2016-09-15) +------------------- + +0.3.23 (2016-09-14) +------------------- + +0.3.22 (2016-09-13) +------------------- + +0.3.21 (2016-04-15) +------------------- + +0.3.20 (2016-04-14) +------------------- + +0.3.19 (2016-03-22) +------------------- + +0.3.18 (2016-03-21) +------------------- + +0.3.17 (2016-03-20) +------------------- + +0.3.16 (2016-02-11) +------------------- + +0.3.15 (2016-02-09) +------------------- + +0.3.14 (2016-02-04) +------------------- +* [jsk_perception] BoundingBoxToRectArray and rect_array_to_image_marker.py +* [jsk_pcl_ros] Publish current tracking status (running or idle) + from particle_fitler_tracking. + And add some scripts to visualize them. +* [jsk_recognition_msgs] Add min/max fields to PlotDataArray +* [jsk_recognition_msgs] Update PlotData message to support more 2d plotting +* Contributors: Ryohei Ueda + +0.3.13 (2015-12-19) +------------------- + +0.3.12 (2015-12-19) +------------------- +* [jsk_pcl_ros_utils] Introduce new package called jsk_pcl_ros_utils + in order to speed-up compilation of jsk_pcl_ros +* Contributors: Ryohei Ueda + +0.3.11 (2015-12-18) +------------------- + +0.3.10 (2015-12-17) +------------------- + +0.3.9 (2015-12-14) +------------------ +* [jsk_perception] Add PolygonArrayColorHistogram +* Contributors: Ryohei Ueda + +0.3.8 (2015-12-08) +------------------ +* [jsk_pcl_ros] Add Failure flag to Torus message +* Remove types on docs for jsk_pcl_ros + See http://docs.ros.org/indigo/api/jsk_recognition_msgs/html/index-msg.html for message types +* Contributors: Kentaro Wada, Ryohei Ueda + +0.3.7 (2015-11-19) +------------------ +* Merge pull request `#1276 `_ from mmurooka/add-octomap-contact + [jsk_pcl_ros] Add octomap contact +* [jsk_recognition_msgs] Add VectorArray.msg +* add message for octomap_server_contact +* [jsk_recognition_msgs] Add new msg ClassificationResult +* [jsk_recognition_msgs] Sort msg files in CMakeLists.txt +* [jsk_recognition_msgs] Add WeightedPoseArray +* add new output msg for handle estimate +* Contributors: Kentaro Wada, Masaki Murooka, Ryohei Ueda, Yu Ohara + +0.3.6 (2015-09-11) +------------------ + +0.3.5 (2015-09-09) +------------------ + +0.3.4 (2015-09-07) +------------------ + +0.3.3 (2015-09-06) +------------------ + +0.3.2 (2015-09-05) +------------------ + +0.3.1 (2015-09-04) +------------------ + +0.3.0 (2015-09-04) +------------------ + +0.2.18 (2015-09-04) +------------------- +* [jsk_recognition_msgs] Add script to convert + jsk_recognition_msgs/PlotData into csv +* [jsk_pcl_ros] Add tool to visualize variance of raser scan +* Contributors: Ryohei Ueda + +0.2.17 (2015-08-21) +------------------- +* [jsk_recognition_msgs/PolygonArray] Add lebels and likelihood for + colorizing on rviz +* Contributors: Ryohei Ueda + +0.2.16 (2015-08-19) +------------------- + +0.2.15 (2015-08-18) +------------------- + +0.2.14 (2015-08-13) +------------------- +* [jsk_recognition_msgs] Add value field to BoundingBox to represent likelihood +* [jsk_recognition_msgs] HistogramWithRange message to represent rich histogram + data +* [jsk_pcl_ros] Add config topic to chain heightmap configuration +* [jsk_perception] Scripts for bof and its hist extractor +* Contributors: Kentaro Wada, Ryohei Ueda + +0.2.13 (2015-06-11) +------------------- +* [jsk_perception] Use ImageDifferenceValue.msg instead of Float32Stamped.msg +* [jsk_recognition_msgs] Add Float32Stamped.msg +* Contributors: Kentaro Wada + +0.2.12 (2015-05-04) +------------------- +* JSK Recognition Msg for handling Array of 2D Rects +* Contributors: iKrishneel + +0.2.11 (2015-04-13) +------------------- + +0.2.10 (2015-04-09) +------------------- + +0.2.9 (2015-03-29) +------------------ +* 0.2.8 +* Update Changelog +* Contributors: Ryohei Ueda + +0.2.8 (2015-03-29) +------------------ + +0.2.7 (2015-03-26) +------------------ + +0.2.6 (2015-03-25) +------------------ + +0.2.5 (2015-03-17) +------------------ + +0.2.4 (2015-03-08) +------------------ +* [jsk_recognition_msgs] Add resolution to SimpleOccupancyGrid +* Contributors: Ryohei Ueda + +0.2.3 (2015-02-02) +------------------ +* add CATKIN_DEPENDS +* [jsk_recognition_msgs] Add new message for occupancy grid for more + simple usage +* Contributors: Ryohei Ueda, Kei Okada + +0.2.2 (2015-01-30) +------------------ + +0.2.1 (2015-01-30) +------------------ + +0.2.0 (2015-01-29) +------------------ + +0.1.34 (2015-01-29) +------------------- +* [jsk_pcl_ros, jsk_perception] Use jsk_recognition_msgs +* Contributors: Ryohei Ueda + +0.1.33 (2015-01-24) +------------------- +* [jsk_recognition_msgs] Add jsk_recognition_msgs +* Contributors: Ryohei Ueda + +0.1.32 (2015-01-12) +------------------- + +0.1.31 (2015-01-08) +------------------- + +0.1.30 (2014-12-24 16:45) +------------------------- + +0.1.29 (2014-12-24 12:43) +------------------------- + +0.1.28 (2014-12-17) +------------------- + +0.1.27 (2014-12-09) +------------------- + +0.1.26 (2014-11-23) +------------------- + +0.1.25 (2014-11-21) +------------------- + +0.1.24 (2014-11-15) +------------------- + +0.1.23 (2014-10-09) +------------------- + +0.1.22 (2014-09-24) +------------------- + +0.1.21 (2014-09-20) +------------------- + +0.1.20 (2014-09-17) +------------------- + +0.1.19 (2014-09-15) +------------------- + +0.1.18 (2014-09-13) +------------------- + +0.1.17 (2014-09-07) +------------------- + +0.1.16 (2014-09-04) +------------------- + +0.1.15 (2014-08-26) +------------------- + +0.1.14 (2014-08-01) +------------------- + +0.1.13 (2014-07-29) +------------------- + +0.1.12 (2014-07-24) +------------------- + +0.1.11 (2014-07-08) +------------------- + +0.1.10 (2014-07-07) +------------------- + +0.1.9 (2014-07-01) +------------------ + +0.1.8 (2014-06-29) +------------------ + +0.1.7 (2014-05-31) +------------------ + +0.1.6 (2014-05-30) +------------------ + +0.1.5 (2014-05-29) +------------------ + +0.1.4 (2014-04-25) +------------------ + +0.1.3 (2014-04-12) +------------------ + +0.1.2 (2014-04-11) +------------------ + +0.1.1 (2014-04-10) +------------------ diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/CMakeLists.txt b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/CMakeLists.txt new file mode 100644 index 00000000000..c4b906da317 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/CMakeLists.txt @@ -0,0 +1,123 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_recognition_msgs) +find_package(catkin REQUIRED + std_msgs sensor_msgs geometry_msgs message_generation pcl_msgs jsk_footstep_msgs) +add_message_files( + FILES + Accuracy.msg + BoundingBoxArray.msg + BoundingBoxMovement.msg + BoundingBox.msg + BoundingBoxArrayWithCameraInfo.msg + Circle2DArray.msg + Circle2D.msg + ClassificationResult.msg + ClusterPointIndices.msg + ColorHistogramArray.msg + ColorHistogram.msg + DepthCalibrationParameter.msg + DepthErrorResult.msg + HeightmapConfig.msg + Histogram.msg + HistogramWithRangeBin.msg + HistogramWithRange.msg + HistogramWithRangeArray.msg + ICPResult.msg + ImageDifferenceValue.msg + Int32Stamped.msg + Label.msg + LabelArray.msg + LineArray.msg + Line.msg + ModelCoefficientsArray.msg + Object.msg + ObjectArray.msg + ParallelEdgeArray.msg + ParallelEdge.msg + PlotData.msg + PointsArray.msg + PolygonArray.msg + PosedCameraInfo.msg + RectArray.msg + Rect.msg + RotatedRect.msg + RotatedRectStamped.msg + SimpleHandle.msg + SimpleOccupancyGridArray.msg + SimpleOccupancyGrid.msg + SlicedPointCloud.msg + SnapItRequest.msg + SparseImage.msg + SparseOccupancyGridArray.msg + SparseOccupancyGridCell.msg + SparseOccupancyGridColumn.msg + SparseOccupancyGrid.msg + TimeRange.msg + TorusArray.msg + Torus.msg + TrackerStatus.msg + TrackingStatus.msg + BoolStamped.msg + VectorArray.msg + WeightedPoseArray.msg + ContactSensor.msg + ContactSensorArray.msg + PlotDataArray.msg + Segment.msg + SegmentStamped.msg + SegmentArray.msg + PeoplePose.msg + PeoplePoseArray.msg + ) + +# TODO(wkentaro): Most of srv files are duplicated in jsk_pcl_ros, +# and jsk_perception, so those in the packages should be removed. +add_service_files(FILES + CallPolygon.srv + CallSnapIt.srv + CheckCircle.srv + CheckCollision.srv + EnvironmentLock.srv + EuclideanSegment.srv + ICPAlign.srv + ICPAlignWithBox.srv + NonMaximumSuppression.srv + PolygonOnEnvironment.srv + RobotPickupReleasePoint.srv + SaveMesh.srv + SetDepthCalibrationParameter.srv + SetLabels.srv + SetPointCloud2.srv + SetTemplate.srv + SnapFootstep.srv + SwitchTopic.srv + TowerPickUp.srv + TowerRobotMoveCommand.srv + TransformScreenpoint.srv + UpdateOffset.srv + WhiteBalancePoints.srv + WhiteBalance.srv + ) + +generate_messages( + DEPENDENCIES + std_msgs sensor_msgs geometry_msgs pcl_msgs jsk_footstep_msgs +) +catkin_package( + CATKIN_DEPENDS std_msgs sensor_msgs geometry_msgs pcl_msgs jsk_footstep_msgs +) + +# From https://github.com/jsk-ros-pkg/jsk_recognition/pull/2345 +# Install header files directly into ${CATKIN_PACKAGE_INCLUDE_DESTINATION}. +# If the package has setup.py and modules under src/${PROJECT_NAME}/, +# install python executables directly into ${CATKIN_PACKAGE_BIN_DESTINATION}. +# However, if it doesn't have setup.py, directories including python executables +# should be installed recursively into ${CATKIN_PACKAGE_SHARE_DESTINATION}. +# Also, other directories like 'launch' or 'sample' must be installed +# recursively into ${CATKIN_PACKAGE_SHARE_DESTINATION}. +# Be careful that 'launch' and 'launch/' are different: the former is directory +# and the latter is each content. +install(DIRECTORY sample scripts + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Accuracy.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Accuracy.msg new file mode 100644 index 00000000000..be3861c2b23 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Accuracy.msg @@ -0,0 +1,2 @@ +Header header +float32 accuracy diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoolStamped.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoolStamped.msg new file mode 100644 index 00000000000..b82ca6736d7 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoolStamped.msg @@ -0,0 +1,2 @@ +Header header +bool data diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBox.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBox.msg new file mode 100644 index 00000000000..2b6a24c29cb --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBox.msg @@ -0,0 +1,7 @@ +# BoundingBox represents a oriented bounding box. +Header header +geometry_msgs/Pose pose +geometry_msgs/Vector3 dimensions # size of bounding box (x, y, z) +# You can use this field to hold value such as likelihood +float32 value +uint32 label diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBoxArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBoxArray.msg new file mode 100644 index 00000000000..2341978a2d5 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBoxArray.msg @@ -0,0 +1,4 @@ +# BoundingBoxArray is a list of BoundingBox. +# You can use jsk_rviz_plugins to visualize BoungingBoxArray on rviz. +Header header +BoundingBox[] boxes diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBoxArrayWithCameraInfo.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBoxArrayWithCameraInfo.msg new file mode 100644 index 00000000000..3f0d2bf5515 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBoxArrayWithCameraInfo.msg @@ -0,0 +1,3 @@ +Header header +BoundingBoxArray boxes +sensor_msgs/CameraInfo camera_info diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBoxMovement.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBoxMovement.msg new file mode 100644 index 00000000000..808ab90ec7d --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/BoundingBoxMovement.msg @@ -0,0 +1,4 @@ +Header header +BoundingBox box +geometry_msgs/Pose handle_pose +geometry_msgs/PoseStamped destination diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Circle2D.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Circle2D.msg new file mode 100644 index 00000000000..e77c629d8c4 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Circle2D.msg @@ -0,0 +1,4 @@ +Header header +float64 radius +float64 x +float64 y diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Circle2DArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Circle2DArray.msg new file mode 100644 index 00000000000..578b9e34e01 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Circle2DArray.msg @@ -0,0 +1,2 @@ +Header header +Circle2D[] circles diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ClassificationResult.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ClassificationResult.msg new file mode 100644 index 00000000000..9b18cbb86c4 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ClassificationResult.msg @@ -0,0 +1,12 @@ +# information about frame and timestamp +Header header + +# prediction results +uint32[] labels # numerical labels +string[] label_names # non-numerical labels +float64[] label_proba # probabilities of labels +float64[] probabilities # probabilities about each classification for all target_names + +# information about classifier +string classifier # name of classifier +string[] target_names # set in which label_names should be diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ClusterPointIndices.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ClusterPointIndices.msg new file mode 100644 index 00000000000..302d9cbc20f --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ClusterPointIndices.msg @@ -0,0 +1,4 @@ +# ClusterPointIndices is used to represent segmentation result. +# Simply put, ClusterPointIndices is a list of PointIndices. +Header header +pcl_msgs/PointIndices[] cluster_indices diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ColorHistogram.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ColorHistogram.msg new file mode 100644 index 00000000000..357e365c98a --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ColorHistogram.msg @@ -0,0 +1,2 @@ +Header header +float32[] histogram diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ColorHistogramArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ColorHistogramArray.msg new file mode 100644 index 00000000000..5b38fb8ff52 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ColorHistogramArray.msg @@ -0,0 +1,2 @@ +Header header +ColorHistogram[] histograms diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ContactSensor.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ContactSensor.msg new file mode 100644 index 00000000000..685c8f2ee95 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ContactSensor.msg @@ -0,0 +1,7 @@ +# Header +Header header + +# Whether sensor detects contact or not +bool contact + +string link_name diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ContactSensorArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ContactSensorArray.msg new file mode 100644 index 00000000000..b4e25158132 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ContactSensorArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +jsk_recognition_msgs/ContactSensor[] datas \ No newline at end of file diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/DepthCalibrationParameter.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/DepthCalibrationParameter.msg new file mode 100644 index 00000000000..555cb5ad1cd --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/DepthCalibrationParameter.msg @@ -0,0 +1,6 @@ +# each vector stands for C(u, v) +# C(u, v) = a_0 * u^2 + a_1 * u + a_2 * v^2 + a_3 * v + a_4 +float64[] coefficients2 +float64[] coefficients1 +float64[] coefficients0 +bool use_abs diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/DepthErrorResult.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/DepthErrorResult.msg new file mode 100644 index 00000000000..bbd1f99d022 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/DepthErrorResult.msg @@ -0,0 +1,7 @@ +Header header +uint32 u +uint32 v +float32 center_u +float32 center_v +float32 true_depth +float32 observed_depth diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HeightmapConfig.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HeightmapConfig.msg new file mode 100644 index 00000000000..40531e3aa14 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HeightmapConfig.msg @@ -0,0 +1,4 @@ +float32 min_x +float32 max_x +float32 min_y +float32 max_y diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Histogram.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Histogram.msg new file mode 100644 index 00000000000..f9550c747b4 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Histogram.msg @@ -0,0 +1,2 @@ +Header header +float64[] histogram diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HistogramWithRange.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HistogramWithRange.msg new file mode 100644 index 00000000000..da0bbae373f --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HistogramWithRange.msg @@ -0,0 +1,3 @@ +Header header +HistogramWithRangeBin[] bins + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HistogramWithRangeArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HistogramWithRangeArray.msg new file mode 100644 index 00000000000..a5cb0608762 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HistogramWithRangeArray.msg @@ -0,0 +1,4 @@ +Header header +HistogramWithRange[] histograms + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HistogramWithRangeBin.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HistogramWithRangeBin.msg new file mode 100644 index 00000000000..e57272f847c --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/HistogramWithRangeBin.msg @@ -0,0 +1,3 @@ +float64 min_value +float64 max_value +uint32 count diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ICPResult.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ICPResult.msg new file mode 100644 index 00000000000..a13fcc73b99 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ICPResult.msg @@ -0,0 +1,4 @@ +Header header +geometry_msgs/Pose pose +string name +float64 score \ No newline at end of file diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ImageDifferenceValue.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ImageDifferenceValue.msg new file mode 100644 index 00000000000..ac4d1c29145 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ImageDifferenceValue.msg @@ -0,0 +1,3 @@ +Header header + +float32 difference diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Int32Stamped.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Int32Stamped.msg new file mode 100644 index 00000000000..5b2498fc46f --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Int32Stamped.msg @@ -0,0 +1,2 @@ +Header header +int32 data diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Label.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Label.msg new file mode 100644 index 00000000000..e391b32108a --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Label.msg @@ -0,0 +1,2 @@ +int32 id +string name diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/LabelArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/LabelArray.msg new file mode 100644 index 00000000000..167010cff44 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/LabelArray.msg @@ -0,0 +1,2 @@ +Header header +Label[] labels diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Line.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Line.msg new file mode 100644 index 00000000000..c0e9925c177 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Line.msg @@ -0,0 +1,6 @@ +float64 x1 +float64 y1 +float64 x2 +float64 y2 + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/LineArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/LineArray.msg new file mode 100644 index 00000000000..09dd17ef1dc --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/LineArray.msg @@ -0,0 +1,3 @@ +Header header +Line[] lines + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ModelCoefficientsArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ModelCoefficientsArray.msg new file mode 100644 index 00000000000..49d26a8a2d8 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ModelCoefficientsArray.msg @@ -0,0 +1,5 @@ +# ModelCoefficientsArray is used to represent coefficients of model +# for each segmented clusters. +# Simply put, ModelCoefficientsArray is a list of ModelCoefficients. +Header header +pcl_msgs/ModelCoefficients[] coefficients diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Object.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Object.msg new file mode 100644 index 00000000000..613eb5a1f87 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Object.msg @@ -0,0 +1,13 @@ +# object instant info (ex. oreo_cookie) +int32 id # object id +string name # object name + +# object class info (ex. snack) +int32 class_id +string class_name + +string[] image_resources # image urls +string mesh_resource # mesh file url + +float32 weight # weight [kg] +geometry_msgs/Vector3 dimensions # bounding box [m] diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ObjectArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ObjectArray.msg new file mode 100644 index 00000000000..29c30e5b642 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ObjectArray.msg @@ -0,0 +1,2 @@ +Header header +jsk_recognition_msgs/Object[] objects diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ParallelEdge.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ParallelEdge.msg new file mode 100644 index 00000000000..f4081ed5a61 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ParallelEdge.msg @@ -0,0 +1,3 @@ +Header header +pcl_msgs/PointIndices[] cluster_indices +pcl_msgs/ModelCoefficients[] coefficients diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ParallelEdgeArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ParallelEdgeArray.msg new file mode 100644 index 00000000000..b6a16318e64 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/ParallelEdgeArray.msg @@ -0,0 +1,2 @@ +Header header +ParallelEdge[] edge_groups diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PeoplePose.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PeoplePose.msg new file mode 100644 index 00000000000..95d30f55973 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PeoplePose.msg @@ -0,0 +1,3 @@ +string[] limb_names +geometry_msgs/Pose[] poses +float32[] scores \ No newline at end of file diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PeoplePoseArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PeoplePoseArray.msg new file mode 100644 index 00000000000..2347f01de16 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PeoplePoseArray.msg @@ -0,0 +1,2 @@ +Header header +jsk_recognition_msgs/PeoplePose[] poses diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PlotData.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PlotData.msg new file mode 100644 index 00000000000..3817322bd46 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PlotData.msg @@ -0,0 +1,10 @@ +uint32 SCATTER=1 +uint32 LINE=2 + +Header header +float32[] xs +float32[] ys +uint32 type #SCATTER or LINE +string label +bool fit_line +bool fit_line_ransac diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PlotDataArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PlotDataArray.msg new file mode 100644 index 00000000000..6da4a8f0943 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PlotDataArray.msg @@ -0,0 +1,8 @@ +Header header +jsk_recognition_msgs/PlotData[] data +bool no_legend +float32 legend_font_size +float32 max_x +float32 min_x +float32 min_y +float32 max_y diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PointsArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PointsArray.msg new file mode 100644 index 00000000000..324028e45c3 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PointsArray.msg @@ -0,0 +1,2 @@ +Header header +sensor_msgs/PointCloud2[] cloud_list diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PolygonArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PolygonArray.msg new file mode 100644 index 00000000000..17b27e3681a --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PolygonArray.msg @@ -0,0 +1,6 @@ +# PolygonArray is a list of PolygonStamped. +# You can use jsk_rviz_plugins to visualize PolygonArray on rviz. +Header header +geometry_msgs/PolygonStamped[] polygons +uint32[] labels +float32[] likelihood diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PosedCameraInfo.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PosedCameraInfo.msg new file mode 100644 index 00000000000..f352b652192 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/PosedCameraInfo.msg @@ -0,0 +1,3 @@ +Header header +sensor_msgs/CameraInfo camera_info +geometry_msgs/Pose offset diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Rect.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Rect.msg new file mode 100644 index 00000000000..eccaa623116 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Rect.msg @@ -0,0 +1,4 @@ +int32 x +int32 y +int32 width +int32 height diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/RectArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/RectArray.msg new file mode 100644 index 00000000000..0843a95d27b --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/RectArray.msg @@ -0,0 +1,2 @@ +Header header +Rect[] rects diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/RotatedRect.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/RotatedRect.msg new file mode 100644 index 00000000000..bcca6bebd0b --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/RotatedRect.msg @@ -0,0 +1,5 @@ +float64 x +float64 y +float64 width +float64 height +float64 angle # degree diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/RotatedRectStamped.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/RotatedRectStamped.msg new file mode 100644 index 00000000000..fa000def46e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/RotatedRectStamped.msg @@ -0,0 +1,2 @@ +Header header +RotatedRect rect diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Segment.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Segment.msg new file mode 100644 index 00000000000..6578bf09fbf --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Segment.msg @@ -0,0 +1,2 @@ +geometry_msgs/Point start_point +geometry_msgs/Point end_point diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SegmentArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SegmentArray.msg new file mode 100644 index 00000000000..3afd7d45b37 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SegmentArray.msg @@ -0,0 +1,2 @@ +Header header +Segment[] segments diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SegmentStamped.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SegmentStamped.msg new file mode 100644 index 00000000000..4f8f4371e2e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SegmentStamped.msg @@ -0,0 +1,2 @@ +Header header +Segment segment diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SimpleHandle.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SimpleHandle.msg new file mode 100644 index 00000000000..67d3e75ae97 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SimpleHandle.msg @@ -0,0 +1,3 @@ +Header header +geometry_msgs/Pose pose +float64 handle_width diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SimpleOccupancyGrid.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SimpleOccupancyGrid.msg new file mode 100644 index 00000000000..8245e77facc --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SimpleOccupancyGrid.msg @@ -0,0 +1,6 @@ +Header header +# plane coefficients +float32[4] coefficients +# cells +float32 resolution +geometry_msgs/Point[] cells diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SimpleOccupancyGridArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SimpleOccupancyGridArray.msg new file mode 100644 index 00000000000..568c2b59ead --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SimpleOccupancyGridArray.msg @@ -0,0 +1,2 @@ +Header header +SimpleOccupancyGrid[] grids diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SlicedPointCloud.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SlicedPointCloud.msg new file mode 100644 index 00000000000..4e5c1076c70 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SlicedPointCloud.msg @@ -0,0 +1,3 @@ +sensor_msgs/PointCloud2 point_cloud +uint8 slice_index +uint8 sequence_id diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SnapItRequest.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SnapItRequest.msg new file mode 100644 index 00000000000..50b53b4f2d9 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SnapItRequest.msg @@ -0,0 +1,14 @@ +Header header +uint8 MODEL_PLANE=0 +uint8 MODEL_CYLINDER=1 +uint8 model_type + +geometry_msgs/PolygonStamped target_plane + +geometry_msgs/PointStamped center +geometry_msgs/Vector3Stamped direction +float64 radius +float64 height +# parameters, 0 means +float64 max_distance +float64 eps_angle diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseImage.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseImage.msg new file mode 100644 index 00000000000..06b90a51ff1 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseImage.msg @@ -0,0 +1,9 @@ +Header header + +uint32 width +uint32 height + +# each uint8 or uint16 contains position of white pixel expressed like: "(x << 8 or 16) ^ y" +# if max(width, height) > 256(=2^8) use data32 array, else use data16 array. +uint16[] data16 +uint32[] data32 diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGrid.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGrid.msg new file mode 100644 index 00000000000..7dc72fa9851 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGrid.msg @@ -0,0 +1,5 @@ +Header header +geometry_msgs/Pose origin_pose +float32 resolution + +SparseOccupancyGridColumn[] columns diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGridArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGridArray.msg new file mode 100644 index 00000000000..feccb27bf8e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGridArray.msg @@ -0,0 +1,2 @@ +Header header +SparseOccupancyGrid[] grids diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGridCell.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGridCell.msg new file mode 100644 index 00000000000..952e6804280 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGridCell.msg @@ -0,0 +1,2 @@ +int32 row_index +float32 value diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGridColumn.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGridColumn.msg new file mode 100644 index 00000000000..64a6bb732a6 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/SparseOccupancyGridColumn.msg @@ -0,0 +1,2 @@ +int32 column_index +SparseOccupancyGridCell[] cells diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TimeRange.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TimeRange.msg new file mode 100644 index 00000000000..e655ff88402 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TimeRange.msg @@ -0,0 +1,4 @@ +# Represents range of time. +std_msgs/Header header +time start +time end diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Torus.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Torus.msg new file mode 100644 index 00000000000..ae46859ccd8 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/Torus.msg @@ -0,0 +1,5 @@ +Header header +bool failure +geometry_msgs/Pose pose +float64 large_radius +float64 small_radius diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TorusArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TorusArray.msg new file mode 100644 index 00000000000..34f2c3d0c25 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TorusArray.msg @@ -0,0 +1,2 @@ +Header header +Torus[] toruses diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TrackerStatus.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TrackerStatus.msg new file mode 100644 index 00000000000..6246557bfda --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TrackerStatus.msg @@ -0,0 +1,5 @@ +Header header +# if true, it means the tracker is trying to track object, +# else, the tracker think scene is stable and no need to track +# to save computational resource. +bool is_tracking diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TrackingStatus.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TrackingStatus.msg new file mode 100644 index 00000000000..9063156c477 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/TrackingStatus.msg @@ -0,0 +1,4 @@ +Header header + +# if true, it means the tracker abandon to track the object. +bool is_lost diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/VectorArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/VectorArray.msg new file mode 100644 index 00000000000..cb95241bd3a --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/VectorArray.msg @@ -0,0 +1,3 @@ +Header header +int32 vector_dim +float64[] data diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/WeightedPoseArray.msg b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/WeightedPoseArray.msg new file mode 100644 index 00000000000..f20e719a6e1 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/msg/WeightedPoseArray.msg @@ -0,0 +1,3 @@ +Header header +float32[] weights +geometry_msgs/PoseArray poses diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/package.xml b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/package.xml new file mode 100644 index 00000000000..c51e62c33b4 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/package.xml @@ -0,0 +1,28 @@ + + + jsk_recognition_msgs + 1.2.6 + ROS messages for jsk_pcl_ros and jsk_perception. + + http://github.com/jsk-ros-pkg/jsk_recognition + http://github.com/jsk-ros-pkg/jsk_recognition/issues + http://wiki.ros.org/jsk_recognition_msgs + + Ryohei Ueda + BSD + catkin + pcl_msgs + pcl_msgs + std_msgs + std_msgs + geometry_msgs + geometry_msgs + sensor_msgs + sensor_msgs + message_generation + message_generation + jsk_footstep_msgs + jsk_footstep_msgs + + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/sample/sample_object_array_publisher.launch b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/sample/sample_object_array_publisher.launch new file mode 100644 index 00000000000..0c20ad0a04f --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/sample/sample_object_array_publisher.launch @@ -0,0 +1,47 @@ + + + + + latch: true + objects: + - id: 1 + name: avery_binder + class_id: 1 + class_name: book + image_resources: + - package://jsk_arc2017_common/data/objects/avery_binder/top.jpg + mesh_resource: package://jsk_arc2017_common/data/objects/avery_binder/mesh/mesh.obj + weight: 0.382 + dimensions: + x: 0.266 + y: 0.294 + z: 0.041 + - id: 18 + name: hinged_ruled_index_cards + class_id: 1 + class_name: book + image_resources: + - package://jsk_arc2017_common/data/objects/hinged_ruled_index_cards/top.jpg + mesh_resource: package://jsk_arc2017_common/data/objects/hinged_ruled_index_cards/mesh/mesh.obj + weight: 0.098 + dimensions: + x: 0.104 + y: 0.127 + z: 0.022 + - id: 35 + name: tennis_ball_container + class_id: 2 + class_name: cylinder + image_resources: + - package://jsk_arc2017_common/data/objects/tennis_ball_container/top.jpg + mesh_resource: package://jsk_arc2017_common/data/objects/tennis_ball_container/mesh/mesh.obj + weight: 0.217 + dimensions: + x: 0.077 + y: 0.218 + z: 0.077 + + + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/object_array_publisher.py b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/object_array_publisher.py new file mode 100755 index 00000000000..97284fa2b91 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/object_array_publisher.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +import rospy +import genpy +from jsk_recognition_msgs.msg import Object +from jsk_recognition_msgs.msg import ObjectArray + + +class ObjectArrayPublisher(object): + + def __init__(self): + objects = rospy.get_param('~objects') + self.msg = ObjectArray() + for obj in objects: + obj_msg = Object() + genpy.message.fill_message_args(obj_msg, obj) + self.msg.objects.append(obj_msg) + + latch = rospy.get_param('~latch', False) + self._pub = rospy.Publisher('~output', ObjectArray, queue_size=1, + latch=True) + self._timer = rospy.Timer(rospy.Duration(1), self._timer_cb, + oneshot=latch) + + def _timer_cb(self, event): + self.msg.header.stamp = event.current_real + self._pub.publish(self.msg) + + +if __name__ == '__main__': + rospy.init_node('object_array_publisher') + app = ObjectArrayPublisher() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/people_pose_array_to_pose_array.py b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/people_pose_array_to_pose_array.py new file mode 100755 index 00000000000..6914146c2dc --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/people_pose_array_to_pose_array.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +from geometry_msgs.msg import PoseArray +from jsk_topic_tools import ConnectionBasedTransport +import rospy + +from jsk_recognition_msgs.msg import PeoplePoseArray + + +class PeoplePoseArrayToPoseArray(ConnectionBasedTransport): + + def __init__(self): + super(PeoplePoseArrayToPoseArray, self).__init__() + self.pub = self.advertise('~output', PoseArray, queue_size=1) + + def subscribe(self): + self.sub = rospy.Subscriber('~input', PeoplePoseArray, self._cb) + + def unsubscribe(self): + self.sub.unregister() + + def _cb(self, msg): + out_msg = PoseArray() + for person_pose in msg.poses: + out_msg.poses.extend(person_pose.poses) + out_msg.header = msg.header + self.pub.publish(out_msg) + + +if __name__ == '__main__': + rospy.init_node('people_pose_array_to_pose_array') + app = PeoplePoseArrayToPoseArray() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/plot_data_to_csv.py b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/plot_data_to_csv.py new file mode 100755 index 00000000000..728e7b6b0c1 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/plot_data_to_csv.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python + +import rospy +from jsk_recognition_msgs.msg import PlotData +import csv + +counter = 0 +def callback(msg): + global counter + rospy.loginfo("writing to %s" % (filename % counter)) + with open(filename % counter, "w") as f: + writer = csv.writer(f, delimiter=',') + writer.writerow(["x", "y"]) + for x, y in zip(msg.xs, msg.ys): + writer.writerow([x, y]) + rospy.loginfo("done") + counter = counter + 1 + +if __name__ == "__main__": + rospy.init_node("plot_data_to_csv") + filename = rospy.get_param("~filename", "output_%04d.csv") + sub = rospy.Subscriber("~input", PlotData, callback, queue_size=1) + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/save_mesh_server.py b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/save_mesh_server.py new file mode 100755 index 00000000000..a23bd0bb235 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/scripts/save_mesh_server.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python + +import rospy +from jsk_recognition_msgs.msg import BoundingBox +from jsk_recognition_msgs.srv import SaveMesh +from jsk_recognition_msgs.srv import SaveMeshRequest +from std_srvs.srv import Empty +from std_srvs.srv import EmptyResponse + + +class SaveMeshServer(object): + + def __init__(self): + self.ground_frame_id = rospy.get_param('~ground_frame_id', '') + self.sub_bbox = rospy.Subscriber('~input/bbox', BoundingBox, self._cb) + self.srv_client = rospy.ServiceProxy('~save_mesh', SaveMesh) + self.srv_server = rospy.Service('~request', Empty, self._request_cb) + self.bbox_msg = None + + def _cb(self, bbox_msg): + self.bbox_msg = bbox_msg + + def _request_cb(self, req): + if self.bbox_msg is None: + rospy.logerr('No bounding box is set, so ignoring the request.') + return EmptyResponse() + + req = SaveMeshRequest() + req.box = self.bbox_msg + req.ground_frame_id = self.ground_frame_id + self.srv_client.call(req) + return EmptyResponse() + + +if __name__ == '__main__': + rospy.init_node('save_mesh_server') + server = SaveMeshServer() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CallPolygon.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CallPolygon.srv new file mode 100644 index 00000000000..151d8913567 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CallPolygon.srv @@ -0,0 +1,3 @@ +string filename +--- +geometry_msgs/PolygonStamped points diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CallSnapIt.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CallSnapIt.srv new file mode 100644 index 00000000000..af2285b3f62 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CallSnapIt.srv @@ -0,0 +1,3 @@ +jsk_recognition_msgs/SnapItRequest request +--- +geometry_msgs/Pose transformation diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CheckCircle.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CheckCircle.srv new file mode 100644 index 00000000000..126cb618636 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CheckCircle.srv @@ -0,0 +1,5 @@ +geometry_msgs/Point point +--- +bool clicked +int32 index +string msg diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CheckCollision.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CheckCollision.srv new file mode 100644 index 00000000000..6bacae53b53 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/CheckCollision.srv @@ -0,0 +1,4 @@ +sensor_msgs/JointState joint +geometry_msgs/PoseStamped pose +--- +bool result diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/EnvironmentLock.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/EnvironmentLock.srv new file mode 100644 index 00000000000..fa7f94abcf5 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/EnvironmentLock.srv @@ -0,0 +1,2 @@ +--- +uint32 environment_id diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/EuclideanSegment.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/EuclideanSegment.srv new file mode 100644 index 00000000000..d31009c55ec --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/EuclideanSegment.srv @@ -0,0 +1,4 @@ +sensor_msgs/PointCloud2 input +float32 tolerance +--- +sensor_msgs/PointCloud2[] output diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/ICPAlign.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/ICPAlign.srv new file mode 100644 index 00000000000..a696adb9265 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/ICPAlign.srv @@ -0,0 +1,4 @@ +sensor_msgs/PointCloud2 reference_cloud +sensor_msgs/PointCloud2 target_cloud +--- +jsk_recognition_msgs/ICPResult result diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/ICPAlignWithBox.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/ICPAlignWithBox.srv new file mode 100644 index 00000000000..38857dc08cb --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/ICPAlignWithBox.srv @@ -0,0 +1,4 @@ +sensor_msgs/PointCloud2 target_cloud +jsk_recognition_msgs/BoundingBox target_box +--- +jsk_recognition_msgs/ICPResult result diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/NonMaximumSuppression.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/NonMaximumSuppression.srv new file mode 100644 index 00000000000..4c576e47599 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/NonMaximumSuppression.srv @@ -0,0 +1,5 @@ +jsk_recognition_msgs/Rect[] rect +float32 threshold +--- +jsk_recognition_msgs/Rect[] bbox +int64 bbox_count diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/PolygonOnEnvironment.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/PolygonOnEnvironment.srv new file mode 100644 index 00000000000..66802b92e98 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/PolygonOnEnvironment.srv @@ -0,0 +1,6 @@ +uint32 environment_id +uint32 plane_index +geometry_msgs/PolygonStamped polygon +--- +bool result +string reason diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/RobotPickupReleasePoint.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/RobotPickupReleasePoint.srv new file mode 100644 index 00000000000..2bd31c7ec57 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/RobotPickupReleasePoint.srv @@ -0,0 +1,5 @@ +Header header +geometry_msgs/Point target_point +byte pick_or_release # 0 -> pick, 1 -> release +--- +bool success diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SaveMesh.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SaveMesh.srv new file mode 100644 index 00000000000..441b309dacb --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SaveMesh.srv @@ -0,0 +1,4 @@ +string ground_frame_id # to create solid model +jsk_recognition_msgs/BoundingBox box # to crop mesh +--- +bool success diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetDepthCalibrationParameter.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetDepthCalibrationParameter.srv new file mode 100644 index 00000000000..f0368d38a25 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetDepthCalibrationParameter.srv @@ -0,0 +1,2 @@ +jsk_recognition_msgs/DepthCalibrationParameter parameter +--- diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetLabels.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetLabels.srv new file mode 100644 index 00000000000..c07ad7bd616 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetLabels.srv @@ -0,0 +1,3 @@ +int64[] labels +--- +bool success diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetPointCloud2.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetPointCloud2.srv new file mode 100644 index 00000000000..2ba5dba2705 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetPointCloud2.srv @@ -0,0 +1,4 @@ +sensor_msgs/PointCloud2 cloud +string name +--- +string output diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetTemplate.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetTemplate.srv new file mode 100644 index 00000000000..4611f47de4c --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SetTemplate.srv @@ -0,0 +1,8 @@ +string type +sensor_msgs/Image image +# dimensions of the texture in the real world (in meters) +float32 dimx +float32 dimy +geometry_msgs/Pose relativepose # used to set the coordinate system of the object relative to the texture +string savefilename # if not empty, will save the resulting pp file to here +--- diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SnapFootstep.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SnapFootstep.srv new file mode 100644 index 00000000000..31119d2a05a --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SnapFootstep.srv @@ -0,0 +1,3 @@ +jsk_footstep_msgs/FootstepArray input +--- +jsk_footstep_msgs/FootstepArray output diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SwitchTopic.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SwitchTopic.srv new file mode 100644 index 00000000000..1438a856f8d --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/SwitchTopic.srv @@ -0,0 +1,4 @@ +# new topics +string camera_info +string points +--- diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/TowerPickUp.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/TowerPickUp.srv new file mode 100644 index 00000000000..bb68d9a1db8 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/TowerPickUp.srv @@ -0,0 +1,2 @@ +int32 tower_index +--- diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/TowerRobotMoveCommand.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/TowerRobotMoveCommand.srv new file mode 100644 index 00000000000..58b5fcfd4f7 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/TowerRobotMoveCommand.srv @@ -0,0 +1,23 @@ +# a service to move robot with tower index +byte ROBOT1=1 +byte ROBOT2=2 +byte ROBOT3=3 + +byte PLATE_SMALL=1 +byte PLATE_MIDDLE=2 +byte PLATE_LARGE=3 + +byte TOWER_LOWEST=1 +byte TOWER_MIDDLE=2 +byte TOWER_HIGHEST=3 +byte TOWER_LOWEST2=1 + +byte OPTION_NONE=0 +byte OPTION_MOVE_INITIAL=1 + +int32 robot_index +int32 plate_index +int32 from_tower +int32 to_tower +int32 option_command +--- diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/TransformScreenpoint.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/TransformScreenpoint.srv new file mode 100644 index 00000000000..8c6dca5060b --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/TransformScreenpoint.srv @@ -0,0 +1,9 @@ +# screen point +float32 x +float32 y +bool no_update +--- +# position in actual world +std_msgs/Header header +geometry_msgs/Point point +geometry_msgs/Vector3 vector diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/UpdateOffset.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/UpdateOffset.srv new file mode 100644 index 00000000000..fbe5188f488 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/UpdateOffset.srv @@ -0,0 +1,5 @@ +# UpdatePose.srv +# This service is designed to specify localization +# transformation manually +geometry_msgs/TransformStamped transformation +--- diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/WhiteBalance.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/WhiteBalance.srv new file mode 100644 index 00000000000..885d26a8fd6 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/WhiteBalance.srv @@ -0,0 +1,4 @@ +float32[3] reference_color +sensor_msgs/Image input +--- +sensor_msgs/Image output diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/WhiteBalancePoints.srv b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/WhiteBalancePoints.srv new file mode 100644 index 00000000000..4de0ca00ce9 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_msgs/srv/WhiteBalancePoints.srv @@ -0,0 +1,4 @@ +float32[3] reference_color +sensor_msgs/PointCloud2 input +--- +sensor_msgs/PointCloud2 output diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/CHANGELOG.rst b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/CHANGELOG.rst new file mode 100644 index 00000000000..07c8f2c5309 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/CHANGELOG.rst @@ -0,0 +1,539 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_recognition_utils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.6 (2018-11-02) +------------------ +* Install 'sample' and 'test'into SHARE_DESTINATION (`#2345 `_) +* Use diagnostic nodelet for EuclideanClustering and other nodelets (`#2301 `_) + * jsk_pcl_ros: euclidean_clustering: use dianogistc nodelet + Use DiagnosticNodelet::updateDiagnostic preferrably +* Describe the hierachy of rosparams of ClusterPointIndicesDecomposer (`#2285 `_) + * Fix test of add_bounding_box_array The input topics are slow (~1Hz), so slop should be larger (it was 0.1 before). + +* Fix for AssertionError in fast_rcnn.py (`#2281 `_) + * Use roi_pooling_2d defined in chainer for fast_rcnn on CPU mode + +* Re-enable tests which use chainer inside them (`#2280 `_) + * Looser timeout for test of add_bounding_box_array +* Node to concatenate BoundingBoxArray (`#2264 `_) + * Doc for add_bounding_box_array.py + * Node to concatenate BoundingBoxArray + * Move section to the sample launch files +* Contributors: Yuki Furuta, Kei Okada, Kentaro Wada + +1.2.5 (2018-04-09) +------------------ +* Fix build of jsk_recognition_utils (on Kinetic) (`#2262 `_ ) + * Close https://github.com/jsk-ros-pkg/jsk_recognition/issues/2259 +* [jsk_perception/fast_rcnn.py] fast_rcnn node to follow chainer-v2 version (`#2249 `_) + * modify fast_rcnn model to follow chainer-v2 version +* Contributors: Yuki Furuta, Kentaro Wada, Shingo Kitagawa + +1.2.4 (2018-01-12) +------------------ +* Add image gallery to README (`#2225 `_) +* Contributors: Kentaro Wada + +1.2.3 (2017-11-23) +------------------ +* Regional feature based object recognition using ResNet (`#2172 `_) +* Convert bounding box to mask (`#2176 `_) + * Add RectArrayToPolygonArray +* Contributors: Kentaro Wada + +1.2.2 (2017-07-23) +------------------ + +1.2.1 (2017-07-15) +------------------ + +1.2.0 (2017-07-15) +------------------ +* jsk_pcl_ros: Add Primitive shape classifier nodelet (`#2141 `_) + * jsk_recognition_utils/include/jsk_recognition_utils/geo/polygon.h + * [jsk_recognition_utils][polygon][fromROSMsg] use identity matrix for default transformation + +* jsk_recognition_utils/setup.py: Find packages automatically in jsk_recognition_utils (`#2156 `_) +* [jsk_recognition_utils] install chainermodels dir in setup.py (`#2154 `_) + * [jsk_perception][jsk_recognition_utils] support chainer-v2 in alexnet and vgg16 (`#2153 `_) + * alexnet and vgg16 support chainer-v2 + +* Contributors: Kentaro Wada, Shingo Kitagawa, Yuki Furuta + +1.1.3 (2017-07-07) +------------------ +* [jsk_pcl_ros_utils] Add nodelet for computing & comparing color histogram (`#2101 `_ ) + * jsk_recognition_utils/include/jsk_recognition_utils/pcl/color_histogram.h: add color_histogram_classifier and visualizer +* Generate Kinfu texture model with attention (BoundingBox) and Groundframe to fix occluded surface (`#2135 `_ ) + * Create function to crop point cloud by bounding box `#2118 `_ ) +* install node_scripts in jsk_recognition_utils +* Contributors: Kentaro Wada, Shingo Kitagawa, Yuki Furuta + +1.1.2 (2017-06-16) +------------------ +* [jsk_percption][jsk_recogniton_utils] add imagenet_object_recognition +launch and its sample (`#2085 `_ ) + * add n_class in VGG16 + * format Alex -> AlexNet +* [jsk_perception] add AlexNet object recognition node #2083 (`#2083 `_ ) + * add alex_object_recognition node +* Remove README.md to generate doc of jsk_recognition_utils (`#2078 `_ ) + Modified: + - doc/jsk_recognition_utils/index.rst + Added: + - jsk_recognition_utils/sample/sample_static_virtual_camera.launch +* [jsk_pcl_ros_utils][polygon_magnifier] allow negative distance to magnify (`#2053 `_ ) + [jsk_pcl_ros_utils][polygon_magnifier] update docs + [jsk_recognition_utils] add polygon_array_publisher.py / sample_polygon_array_publisher.launch + [jsk_pcl_ros_utils] add sample / test for polygon_magnifier +* Generate README by script (`#2064 `_ ) +* [jsk_recognition_utils/geo/Polygon] add distance method. (`#2031 `_ ) +* [jsk_recognition_utils] add PolyLine to polyline.{h,cpp} and add code to segment.{h, cpp} (`#2026 `_ ) +* Contributors: Kentaro Wada, Masaki Murooka, Shingo Kitagawa, Yohei Kakiuchi, Yuki Furuta + +1.1.1 (2017-03-04) +------------------ + +1.1.0 (2017-02-09) +------------------ + +1.0.4 (2017-02-09) +------------------ +* [jsk_recognition_utils] src/geo/segment.cpp: fix argument name of Segment::midpoint. (`#2013 `_ ) +* Contributors: Masaki Murooka + +1.0.3 (2017-02-08) +------------------ +* [jsk_recognition_utils] add mipoint method to segment class. (`#2009 `_ ) + * src/edge_depth_refinement_nodelet.cpp + * src/geo/segment.cpp + include/jsk_recognition_utils/geo/segment.h +* Evaluate voxel segmentation by IU (`#1993 `_ ) + * Stop depending on jsk_interactive_marker + * node_scripts/evaluate_voxel_segmentation_by_gt_box.py + * Compute box overlap and publish it : intersect-over-union (overlap) = volume_tp / (volume_fn + volume_fp + volume_tp) + * test/evaluate_box_segmentation_by_gt_box.test + * test/evaluate_voxel_segmentation_by_gt_box.test + * sample/sample_evaluate_box_segmentation_by_gt_box.launch + * sample/sample_evaluate_voxel_segmentation_by_gt_box.launch + * scripts/evaluate_box_segmentation_by_gt_box.py + * scripts/evaluate_voxel_segmentation_by_gt_box.py + * Move evaluation scripts of box segmentation from jsk_recognition_utils to to jsk_pcl_ros_utils + +* Contributors: Kentaro Wada, Masaki Murooka + +1.0.2 (2017-01-12) +------------------ + +1.0.1 (2016-12-13) +------------------ + +1.0.0 (2016-12-12) +------------------ +* Fix fo kinetic (`#1943 `_) + * use std::isnan instead of isnan, knetic compiler requires this + +* Contributors: Kei Okada + +0.3.29 (2016-10-30) +------------------- + +0.3.28 (2016-10-29) +------------------- + +0.3.27 (2016-10-29) +------------------- + +0.3.26 (2016-10-27) +------------------- +* Stop using deprecated jsk_topic_tools/log_utils.h (`#1933 `_) +* [heightmap] change type of heightmap to image/32FC2 (`#1886 `_) +* Prettify the style of rosparam for bbox publisher (`#1885 `_) + This shows deprecation warning and does not break the current api. + (BTW, this code is quite new and I think no one use this other than me.) +* Contributors: Kentaro Wada, Yohei Kakiuchi + +0.3.25 (2016-09-16) +------------------- + +0.3.24 (2016-09-15) +------------------- + +0.3.23 (2016-09-14) +------------------- + +0.3.22 (2016-09-13) +------------------- +* Merge pull request #1826 from mmurooka/polyarr-to-poly2 + [jsk_recognition_utils/node_scripts] add polygon_array_to_polygon.py +* [jsk_recognition_utils/node_scripts] add polygon_array_to_polygon.py +* Skip rostest on hydro because of unreleased test tools +* Add test for bounding_box_array_publisher.py +* Add sample for bounding_box_array_publisher.py +* Node to publish bounding box array +* Skip rostest on hydro because of unreleased test tools +* Add test for bounding_box_array_publisher.py +* Add sample for bounding_box_array_publisher.py +* Node to publish bounding box array +* Merge pull request #1809 from wkentaro/feature/pose-array-to-pose + Convert PoseArray to PoseStamped with a specified index +* Convert PoseArray to PoseStamped with a specified index +* Rename test files in favor to {NODE_NAME}.test +* Add util to convert image 16uc1 to 32fc1 +* Merge pull request #1694 from wkentaro/get-numpy-include-dirs + [jsk_recognition_utils] Set Numpy include directories in cmake to fix error on OS X +* Set Numpy include directories in cmake to fix error on OS X +* Remove color_gategoryXX (use labelcolormap) +* Add label color utility function +* Remove nms.py that is moved to nms.pyx +* Recognize object with VGG16 net +* Rename vgg16 -> vgg16_fast_rcnn +* Cythonize Non-maximum Supression baseline +* Remove dependency on rbgirshick/fast-rcnn +* Support old scipy which does not have face() +* Add static virtual camera +* Copy jsk_perception/image_utils.h to jsk_recognition_utils/cv_utils.h +* Stop passing -z flag to ld with Clang (#1601) +* Contributors: Kei Okada, Kentaro Wada, Masaki Murooka + +0.3.21 (2016-04-15) +------------------- + +0.3.20 (2016-04-14) +------------------- +* [jsk_recognition_utils] Support Affine3d project function in Plane (`#1576 `_) +* [jsk_recognition_utils] Add multiple ClusterPointIndices to one (`#1581 `_) + * Add multiple ClusterPointIndices to one + Added: + - jsk_recognition_utils/node_scripts/add_cluster_indices.py + * Document for add_cluster_indices.py +* Visualize ClusterPointIndices for image (`#1579 `_) +* Contributors: Iori Kumagai, Kentaro Wada + +0.3.19 (2016-03-22) +------------------- + +0.3.18 (2016-03-21) +------------------- + +0.3.17 (2016-03-20) +------------------- +* [jsk_perception] Use timer callback to speed up tile_image with no_sync:=true +* [jsk_perception] Cache concatenated image to speed up +* Contributors: Ryohei Ueda + +0.3.16 (2016-02-11) +------------------- + +0.3.15 (2016-02-09) +------------------- + +0.3.14 (2016-02-04) +------------------- +* [jsk_recognition_utils] Tile different size images with centerization + Modified: + - jsk_recognition_utils/python/jsk_recognition_utils/visualize.py +* [jsk_perception] BoundingBoxToRectArray and rect_array_to_image_marker.py +* jsk_recognition_utils/CMakeLists.txt: include_directories should have include/ before catkin_INCLUDE_DIRS +* Merge remote-tracking branch 'origin/master' into auto-change-point-type +* [jsk_pcl_ros] Publish current tracking status (running or idle) + from particle_fitler_tracking. + And add some scripts to visualize them. +* [jsk_pcl_ros] Automatically detect point type in OctreeVoxelGrid + Modified: + - doc/jsk_pcl_ros/nodes/octree_voxel_grid.md + - jsk_pcl_ros/cfg/OctreeVoxelGrid.cfg + - jsk_pcl_ros/include/jsk_pcl_ros/octree_voxel_grid.h + - jsk_pcl_ros/src/octree_voxel_grid_nodelet.cpp + - jsk_recognition_utils/include/jsk_recognition_utils/pcl_ros_util.h + - jsk_recognition_utils/src/pcl_ros_util.cpp +* [jsk_recognition_utils] Add SeriesedBoolean::isAllTrueFilled method + to check all the buffer is filled by true +* [jsk_pcl_ros] Fix WallDurationTimer to publish correct average value +* [jsk_pcl_ros] Publish computation time in icp_registration and torus_finder + Modified: + - doc/jsk_pcl_ros/nodes/icp_registration.md + - doc/jsk_pcl_ros/nodes/torus_f_inder.md + - jsk_pcl_ros/include/jsk_pcl_ros/icp_registration.h + - jsk_pcl_ros/include/jsk_pcl_ros/torus_finder.h + - jsk_pcl_ros/src/icp_registration_nodelet.cpp + - jsk_pcl_ros/src/torus_finder_nodelet.cpp + - jsk_recognition_utils/include/jsk_recognition_utils/time_util.h +* [jsk_perception] Keep original resolution if all the input images has + same shape and add ~draw_input_topic parameter to draw topic name on + the tiled images + Modified: + - jsk_perception/node_scripts/tile_image.py + - jsk_recognition_utils/python/jsk_recognition_utils/visualize.py +* [jsk_perception] Fix tile_image.py for hydro. + 1. Disable approximate sync for hydro. it's not supported on hydro + 2. Use PIL.Image.frombytes instead of PIL.Image.fromstring +* Contributors: Kei Okada, Kentaro Wada, Ryohei Ueda + +0.3.13 (2015-12-19) +------------------- + +0.3.12 (2015-12-19) +------------------- + +0.3.11 (2015-12-18) +------------------- + +0.3.10 (2015-12-17) +------------------- +* [jsk_recognition_utils] Fix import error on server caused by matplotlib +* [jsk_pcl_ros] Check header.frame_id before resolving 3-D spacially + Modified: + jsk_pcl_ros/src/multi_plane_extraction_nodelet.cpp + jsk_perception/src/polygon_array_color_histogram.cpp + jsk_recognition_utils/include/jsk_recognition_utils/pcl_ros_util.h + jsk_recognition_utils/src/pcl_ros_util.cpp +* Contributors: Kentaro Wada, Ryohei Ueda + +0.3.9 (2015-12-14) +------------------ +* [jsk_perception] Compute polygon likelihood based on color histogram. +* [jsk_perception] Add PolygonArrayColorHistogram +* [jsk_recognition_utils] Better API to measure and publish computation time +* Contributors: Ryohei Ueda + +0.3.8 (2015-12-08) +------------------ +* Use ccache if installed to make it fast to generate object file +* [jsk_recognition_utils, jsk_pcl_ros] Measure time to compute + NormalEstimationOMP and RegionGriwongMultiplePlaneSegmentation. + Add utility class to measure time: jsk_recognition_utils::WallDurationTimer +* [jsk_recognition_utils] Split fore/background with depth +* Contributors: Kei Okada, Kentaro Wada, Ryohei Ueda + +0.3.7 (2015-11-19) +------------------ +* Use gcc -z defs to check undefined symbols in shared + objects (jsk_recognitoin_utils, jsk_pcl_ros, jsk_perception). + build_check.cpp cannot run on the environment using multiple processes + because of invoking libjsk_pcl_ros.so link. +* Merge pull request `#1319 `_ from wkentaro/146-various-rgb-colors + [jsk_recognition_utils] Add labelToRGB with 146 rgb colors +* Merge pull request `#1324 `_ from wkentaro/test-tf-listener-singleton + [jsk_recognition_utils] Test tf_listener_singleton.cpp +* [jsk_recognition_utils] Test rgb_colors.cpp +* [jsk_recognition_utils] Test labelToRGB +* [jsk_recognition_utils] 146 rgb colors +* [jsk_recognition_utils] Test tf_listener_singleton.cpp +* [jsk_recognition_utils] Add labelToRGB +* [jsk_recognition_utils] 146 rgb colors +* [jsk_recognition_utils] Util to decompose descriptors with label +* [jsk_recognition_utils] Test tf::Transformer::lookupTransformation +* [jsk_recognition_utils] Bag of Features as python module +* [jsk_recognition_utils] Handle canvas to get safely plot image +* [jsk_recognition_utils] Add bounding_rect_of_mask +* [jsk_recognition_utils] Add jsk_recognition_utils.get_tile_image() +* [jsk_recognition_utils] Fix laser model +* Contributors: Kei Okada, Kentaro Wada, Ryohei Ueda + +0.3.6 (2015-09-11) +------------------ + +0.3.5 (2015-09-09) +------------------ + +0.3.4 (2015-09-07) +------------------ +* Merge pull request `#1168 `_ from k-okada/add_yaml + jsk_recognition_utils: forget to add include to install +* jsk_recognition_utils: forget to add include to install +* [jsk_recognition_utils/README] Add link to doxygen documentation +* [jsk_recognition_utils/Line] Add documentation +* Contributors: Kei Okada, Ryohei Ueda + +0.3.3 (2015-09-06) +------------------ +* [jsk_recognition_utils] Depends on visualization_msgs +* [jsk_recognition_utils] Separate grid_plane.h from geo_util.h +* [jsk_recognition_utils] Separate cylinder.h from geo_util.h +* [jsk_recognition_utils] Separate cube.h from geo_util.h +* [jsk_recognition_utils] Separate convex_polygon.h from geo_util.h +* [jsk_recognition_utils] Separate polygon.h from geo_util.h +* [jsk_recognition_utils] Separate plane.h from geo_util.h +* [jsk_recognition_utils] Separate segment.h from geo_util.h +* [jsk_recognition_utils] Separate line.h from geo_util.h +* Contributors: Ryohei Ueda + +0.3.2 (2015-09-05) +------------------ +* add yaml-cpp to depends +* Merge pull request `#1151 `_ from garaemon/use-histograms + [jsk_perception] Use histograms to compute distance in TabletopColorDifferenceLikelihood +* [jsk_perception] Use histograms to compute distance in TabletopColorDifferenceLikelihood +* Contributors: Kei Okada, Ryohei Ueda + +0.3.1 (2015-09-04) +------------------ +* Add README.md to jsk_recognition_utils +* Contributors: Ryohei Ueda + +0.3.0 (2015-09-04) +------------------ +* [jsk_recognition_utils] Introduce new package jsk_recognition_utils + in order to use utility libraries defined in jsk_pcl_ros in jsk_perception +* Contributors: Ryohei Ueda + +0.2.18 (2015-09-04) +------------------- +* [jsk_recognition_utils] Introduce new package jsk_recognition_utils + in order to use utility libraries defined in jsk_pcl_ros in jsk_perception +* Contributors: Ryohei Ueda + +0.2.17 (2015-08-21) +------------------- + +0.2.16 (2015-08-19) +------------------- + +0.2.15 (2015-08-18) +------------------- + +0.2.14 (2015-08-13) +------------------- + +0.2.13 (2015-06-11) +------------------- + +0.2.12 (2015-05-04) +------------------- + +0.2.11 (2015-04-13) +------------------- + +0.2.10 (2015-04-09) +------------------- + +0.2.9 (2015-03-29) +------------------ + +0.2.7 (2015-03-26) +------------------ + +0.2.6 (2015-03-25) +------------------ + +0.2.5 (2015-03-17) +------------------ + +0.2.4 (2015-03-08) +------------------ + +0.2.3 (2015-02-02) +------------------ + +0.2.2 (2015-01-30 19:29) +------------------------ + +0.2.1 (2015-01-30 00:35) +------------------------ + +0.2.0 (2015-01-29 12:20) +------------------------ + +0.1.34 (2015-01-29 11:53) +------------------------- + +0.1.33 (2015-01-24) +------------------- + +0.1.32 (2015-01-12) +------------------- + +0.1.31 (2015-01-08) +------------------- + +0.1.30 (2014-12-24 16:45) +------------------------- + +0.1.29 (2014-12-24 12:43) +------------------------- + +0.1.28 (2014-12-17) +------------------- + +0.1.27 (2014-12-09) +------------------- + +0.1.26 (2014-11-23) +------------------- + +0.1.25 (2014-11-21) +------------------- + +0.1.24 (2014-11-15) +------------------- + +0.1.23 (2014-10-09) +------------------- + +0.1.22 (2014-09-24) +------------------- + +0.1.21 (2014-09-20) +------------------- + +0.1.20 (2014-09-17) +------------------- + +0.1.19 (2014-09-15) +------------------- + +0.1.18 (2014-09-13) +------------------- + +0.1.17 (2014-09-07) +------------------- + +0.1.16 (2014-09-04) +------------------- + +0.1.15 (2014-08-26) +------------------- + +0.1.14 (2014-08-01) +------------------- + +0.1.13 (2014-07-29) +------------------- + +0.1.12 (2014-07-24) +------------------- + +0.1.11 (2014-07-08) +------------------- + +0.1.10 (2014-07-07) +------------------- + +0.1.9 (2014-07-01) +------------------ + +0.1.8 (2014-06-29) +------------------ + +0.1.7 (2014-05-31) +------------------ + +0.1.6 (2014-05-30) +------------------ + +0.1.5 (2014-05-29) +------------------ + +0.1.4 (2014-04-25) +------------------ + +0.1.3 (2014-04-12) +------------------ + +0.1.2 (2014-04-11) +------------------ + +0.1.1 (2014-04-10) +------------------ diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/CMakeLists.txt b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/CMakeLists.txt new file mode 100644 index 00000000000..61d6dc6dc84 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/CMakeLists.txt @@ -0,0 +1,132 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_recognition_utils) + +# Use ccache if installed to make it fast to generate object files +find_program(CCACHE_FOUND ccache) +if(CCACHE_FOUND) + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) +endif(CCACHE_FOUND) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + dynamic_reconfigure + jsk_recognition_msgs + pcl_ros + pcl_msgs + eigen_conversions + message_generation + rostest + tf_conversions + tf + tf2_ros + image_geometry + sensor_msgs + geometry_msgs + jsk_topic_tools + visualization_msgs +) + +catkin_python_setup() + +generate_dynamic_reconfigure_options( + cfg/PoseArrayToPose.cfg + cfg/PolygonArrayToPolygon.cfg +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES jsk_recognition_utils + CATKIN_DEPENDS jsk_recognition_msgs pcl_ros visualization_msgs message_runtime +) + +# Cythonize pyx files +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +include(UseCython) +add_subdirectory(python/${PROJECT_NAME}) + +find_package(OpenCV REQUIRED core imgproc) +find_package(PCL REQUIRED) +find_package(PkgConfig) +pkg_check_modules(yaml_cpp yaml-cpp REQUIRED) +IF(${yaml_cpp_VERSION} VERSION_LESS "0.5.0") +## indigo yaml-cpp : 0.5.0 / hydro yaml-cpp : 0.3.0 + add_definitions("-DUSE_OLD_YAML") +ENDIF() +if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU") + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -z defs") +endif() +include_directories( + include ${catkin_INCLUDE_DIRS} +) +link_libraries(${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} yaml-cpp) + +add_library(jsk_recognition_utils SHARED + src/grid_index.cpp + src/grid_map.cpp + src/grid_line.cpp + src/geo_util.cpp + src/random_util.cpp + src/pcl_ros_util.cpp + src/pcl_conversion_util.cpp + src/pcl_util.cpp + src/tf_listener_singleton.cpp + src/pcl/ear_clipping_patched.cpp + src/sensor_model_utils.cpp + src/cv_utils.cpp + src/rgb_colors.cpp + src/geo/line.cpp + src/geo/segment.cpp + src/geo/polyline.cpp + src/geo/plane.cpp + src/geo/polygon.cpp + src/geo/convex_polygon.cpp + src/geo/cube.cpp + src/geo/cylinder.cpp + src/geo/grid_plane.cpp + src/time_util.cpp + ) + +add_dependencies(jsk_recognition_utils jsk_recognition_msgs_generate_messages_cpp) + +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_tf_listener_singleton test/tf_listener_singleton.test src/tests/test_tf_listener_singleton.cpp) + target_link_libraries(test_tf_listener_singleton ${PROJECT_NAME} ${catkin_LIBRARIES}) + catkin_add_gtest(test_cv_utils src/tests/test_cv_utils.cpp) + target_link_libraries(test_cv_utils ${PROJECT_NAME} ${OpenCV_LIBRARIES}) + catkin_add_gtest(test_rgb_colors src/tests/test_rgb_colors.cpp) + target_link_libraries(test_rgb_colors ${PROJECT_NAME} ${OpenCV_LIBRARIES}) + if("$ENV{ROS_DISTRO}" STRGREATER "hydro") + # FIXME: jsk_tools/test_topic_published.py does not work on hydro travis/jenkins + # https://github.com/jsk-ros-pkg/jsk_common/pull/1293#issuecomment-164158260 + add_rostest(sample/sample_add_bounding_box_array.launch ARGS gui:=false) + add_rostest(sample/sample_bounding_box_array_publisher.launch ARGS gui:=false) + endif() +endif() + +# From https://github.com/jsk-ros-pkg/jsk_recognition/pull/2345 +# Install header files directly into ${CATKIN_PACKAGE_INCLUDE_DESTINATION}. +# If the package has setup.py and modules under src/${PROJECT_NAME}/, +# install python executables directly into ${CATKIN_PACKAGE_BIN_DESTINATION}. +# However, if it doesn't have setup.py, directories including python executables +# should be installed recursively into ${CATKIN_PACKAGE_SHARE_DESTINATION}. +# Also, other directories like 'launch' or 'sample' must be installed +# recursively into ${CATKIN_PACKAGE_SHARE_DESTINATION}. +# Be careful that 'launch' and 'launch/' are different: the former is directory +# and the latter is each content. +install(TARGETS jsk_recognition_utils + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY node_scripts sample test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cfg/PolygonArrayToPolygon.cfg b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cfg/PolygonArrayToPolygon.cfg new file mode 100755 index 00000000000..4dac1a98f28 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cfg/PolygonArrayToPolygon.cfg @@ -0,0 +1,13 @@ +#!/usr/bin/env python + +PACKAGE = 'jsk_recognition_utils' +ID = 'PolygonArrayToPolygon' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add('index', int_t, 0, + 'Index where the polygon is extracted from polygon array.', default=-1) + +exit(gen.generate(PACKAGE, PACKAGE, ID)) diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cfg/PoseArrayToPose.cfg b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cfg/PoseArrayToPose.cfg new file mode 100755 index 00000000000..5abe0ec9810 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cfg/PoseArrayToPose.cfg @@ -0,0 +1,13 @@ +#!/usr/bin/env python + +PACKAGE = 'jsk_recognition_utils' +ID = 'PoseArrayToPose' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add('index', int_t, 0, + 'Index where the pose is extracted from pose array.', default=-1) + +exit(gen.generate(PACKAGE, PACKAGE, ID)) diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cmake/FindCython.cmake b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cmake/FindCython.cmake new file mode 100644 index 00000000000..7d28eb1408b --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cmake/FindCython.cmake @@ -0,0 +1,45 @@ +# Find the Cython compiler. +# +# This code sets the following variables: +# +# CYTHON_EXECUTABLE +# +# See also UseCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Use the Cython executable that lives next to the Python executable +# if it is a local installation. +find_package( PythonInterp ) +if( PYTHONINTERP_FOUND ) + get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH ) + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat + HINTS ${_python_path} + ) +else() + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat cython3 + ) +endif() + + +include( FindPackageHandleStandardArgs ) +FIND_PACKAGE_HANDLE_STANDARD_ARGS( Cython REQUIRED_VARS CYTHON_EXECUTABLE ) + +mark_as_advanced( CYTHON_EXECUTABLE ) + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cmake/UseCython.cmake b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cmake/UseCython.cmake new file mode 100644 index 00000000000..f432c890c12 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/cmake/UseCython.cmake @@ -0,0 +1,287 @@ +# Define a function to create Cython modules. +# +# For more information on the Cython project, see http://cython.org/. +# "Cython is a language that makes writing C extensions for the Python language +# as easy as Python itself." +# +# This file defines a CMake function to build a Cython Python module. +# To use it, first include this file. +# +# include( UseCython ) +# +# Then call cython_add_module to create a module. +# +# cython_add_module( ... ) +# +# To create a standalone executable, the function +# +# cython_add_standalone_executable( [MAIN_MODULE src1] ... ) +# +# To avoid dependence on Python, set the PYTHON_LIBRARY cache variable to point +# to a static library. If a MAIN_MODULE source is specified, +# the "if __name__ == '__main__':" from that module is used as the C main() method +# for the executable. If MAIN_MODULE, the source with the same basename as +# is assumed to be the MAIN_MODULE. +# +# Where is the name of the resulting Python module and +# ... are source files to be compiled into the module, e.g. *.pyx, +# *.py, *.c, *.cxx, etc. A CMake target is created with name . This can +# be used for target_link_libraries(), etc. +# +# The sample paths set with the CMake include_directories() command will be used +# for include directories to search for *.pxd when running the Cython complire. +# +# Cache variables that effect the behavior include: +# +# CYTHON_ANNOTATE +# CYTHON_NO_DOCSTRINGS +# CYTHON_FLAGS +# +# Source file properties that effect the build process are +# +# CYTHON_IS_CXX +# +# If this is set of a *.pyx file with CMake set_source_files_properties() +# command, the file will be compiled as a C++ file. +# +# See also FindCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Configuration options. +set( CYTHON_ANNOTATE OFF + CACHE BOOL "Create an annotated .html file when compiling *.pyx." ) +set( CYTHON_NO_DOCSTRINGS OFF + CACHE BOOL "Strip docstrings from the compiled module." ) +set( CYTHON_FLAGS "" CACHE STRING + "Extra flags to the cython compiler." ) +mark_as_advanced( CYTHON_ANNOTATE CYTHON_NO_DOCSTRINGS CYTHON_FLAGS ) + +find_package( Cython REQUIRED ) +find_package( PythonLibs REQUIRED ) + +set( CYTHON_CXX_EXTENSION "cxx" ) +set( CYTHON_C_EXTENSION "c" ) + +# Create a *.c or *.cxx file from a *.pyx file. +# Input the generated file basename. The generate file will put into the variable +# placed in the "generated_file" argument. Finally all the *.py and *.pyx files. +function( compile_pyx _name generated_file ) + # Default to assuming all files are C. + set( cxx_arg "" ) + set( extension ${CYTHON_C_EXTENSION} ) + set( pyx_lang "C" ) + set( comment "Compiling Cython C source for ${_name}..." ) + + set( cython_include_directories "" ) + set( pxd_dependencies "" ) + set( c_header_dependencies "" ) + set( pyx_locations "" ) + + foreach( pyx_file ${ARGN} ) + get_filename_component( pyx_file_basename "${pyx_file}" NAME_WE ) + + # Determine if it is a C or C++ file. + get_source_file_property( property_is_cxx ${pyx_file} CYTHON_IS_CXX ) + if( ${property_is_cxx} ) + set( cxx_arg "--cplus" ) + set( extension ${CYTHON_CXX_EXTENSION} ) + set( pyx_lang "CXX" ) + set( comment "Compiling Cython CXX source for ${_name}..." ) + endif() + + # Get the include directories. + get_source_file_property( pyx_location ${pyx_file} LOCATION ) + get_filename_component( pyx_path ${pyx_location} PATH ) + get_directory_property( cmake_include_directories DIRECTORY ${pyx_path} INCLUDE_DIRECTORIES ) + list( APPEND cython_include_directories ${cmake_include_directories} ) + list( APPEND pyx_locations "${pyx_location}" ) + + # Determine dependencies. + # Add the pxd file will the same name as the given pyx file. + unset( corresponding_pxd_file CACHE ) + find_file( corresponding_pxd_file ${pyx_file_basename}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} + NO_DEFAULT_PATH ) + if( corresponding_pxd_file ) + list( APPEND pxd_dependencies "${corresponding_pxd_file}" ) + endif() + + # pxd files to check for additional dependencies. + set( pxds_to_check "${pyx_file}" "${pxd_dependencies}" ) + set( pxds_checked "" ) + set( number_pxds_to_check 1 ) + while( ${number_pxds_to_check} GREATER 0 ) + foreach( pxd ${pxds_to_check} ) + list( APPEND pxds_checked "${pxd}" ) + list( REMOVE_ITEM pxds_to_check "${pxd}" ) + + # check for C header dependencies + file( STRINGS "${pxd}" extern_from_statements + REGEX "cdef[ ]+extern[ ]+from.*$" ) + foreach( statement ${extern_from_statements} ) + # Had trouble getting the quote in the regex + string( REGEX REPLACE "cdef[ ]+extern[ ]+from[ ]+[\"]([^\"]+)[\"].*" "\\1" header "${statement}" ) + unset( header_location CACHE ) + find_file( header_location ${header} PATHS ${cmake_include_directories} ) + if( header_location ) + list( FIND c_header_dependencies "${header_location}" header_idx ) + if( ${header_idx} LESS 0 ) + list( APPEND c_header_dependencies "${header_location}" ) + endif() + endif() + endforeach() + + # check for pxd dependencies + + # Look for cimport statements. + set( module_dependencies "" ) + file( STRINGS "${pxd}" cimport_statements REGEX cimport ) + foreach( statement ${cimport_statements} ) + if( ${statement} MATCHES from ) + string( REGEX REPLACE "from[ ]+([^ ]+).*" "\\1" module "${statement}" ) + else() + string( REGEX REPLACE "cimport[ ]+([^ ]+).*" "\\1" module "${statement}" ) + endif() + list( APPEND module_dependencies ${module} ) + endforeach() + list( REMOVE_DUPLICATES module_dependencies ) + # Add the module to the files to check, if appropriate. + foreach( module ${module_dependencies} ) + unset( pxd_location CACHE ) + find_file( pxd_location ${module}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} NO_DEFAULT_PATH ) + if( pxd_location ) + list( FIND pxds_checked ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( FIND pxds_to_check ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( APPEND pxds_to_check ${pxd_location} ) + list( APPEND pxd_dependencies ${pxd_location} ) + endif() # if it is not already going to be checked + endif() # if it has not already been checked + endif() # if pxd file can be found + endforeach() # for each module dependency discovered + endforeach() # for each pxd file to check + list( LENGTH pxds_to_check number_pxds_to_check ) + endwhile() + endforeach() # pyx_file + + # Set additional flags. + if( CYTHON_ANNOTATE ) + set( annotate_arg "--annotate" ) + endif() + + if( CYTHON_NO_DOCSTRINGS ) + set( no_docstrings_arg "--no-docstrings" ) + endif() + + if( "${CMAKE_BUILD_TYPE}" STREQUAL "Debug" OR + "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo" ) + set( cython_debug_arg "--gdb" ) + endif() + + # Include directory arguments. + list( REMOVE_DUPLICATES cython_include_directories ) + set( include_directory_arg "" ) + foreach( _include_dir ${cython_include_directories} ) + set( include_directory_arg ${include_directory_arg} "-I" "${_include_dir}" ) + endforeach() + + # Determining generated file name. + set( _generated_file "${CMAKE_CURRENT_BINARY_DIR}/${_name}.${extension}" ) + set_source_files_properties( ${_generated_file} PROPERTIES GENERATED TRUE ) + set( ${generated_file} ${_generated_file} PARENT_SCOPE ) + + list( REMOVE_DUPLICATES pxd_dependencies ) + list( REMOVE_DUPLICATES c_header_dependencies ) + + # Add the command to run the compiler. + add_custom_command( OUTPUT ${_generated_file} + COMMAND ${CYTHON_EXECUTABLE} + ARGS ${cxx_arg} ${include_directory_arg} + ${annotate_arg} ${no_docstrings_arg} ${cython_debug_arg} ${CYTHON_FLAGS} + --output-file ${_generated_file} ${pyx_locations} + DEPENDS ${pyx_locations} ${pxd_dependencies} + IMPLICIT_DEPENDS ${pyx_lang} ${c_header_dependencies} + COMMENT ${comment} + ) + + # Remove their visibility to the user. + set( corresponding_pxd_file "" CACHE INTERNAL "" ) + set( header_location "" CACHE INTERNAL "" ) + set( pxd_location "" CACHE INTERNAL "" ) +endfunction() + +# cython_add_module( src1 src2 ... srcN ) +# Build the Cython Python module. +function( cython_add_module _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + foreach( _file ${ARGN} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + list( APPEND pyx_module_sources ${_file} ) + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + compile_pyx( ${_name} generated_file ${pyx_module_sources} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + python_add_module( ${_name} ${generated_file} ${other_module_sources} ) + if( APPLE ) + set_target_properties( ${_name} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup" ) + else() + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ) + endif() +endfunction() + +include( CMakeParseArguments ) +# cython_add_standalone_executable( _name [MAIN_MODULE src3.py] src1 src2 ... srcN ) +# Creates a standalone executable the given sources. +function( cython_add_standalone_executable _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + set( main_module "" ) + cmake_parse_arguments( cython_arguments "" "MAIN_MODULE" "" ${ARGN} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + foreach( _file ${cython_arguments_UNPARSED_ARGUMENTS} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + get_filename_component( _file_we ${_file} NAME_WE ) + if( "${_file_we}" STREQUAL "${_name}" ) + set( main_module "${_file}" ) + elseif( NOT "${_file}" STREQUAL "${cython_arguments_MAIN_MODULE}" ) + set( PYTHON_MODULE_${_file_we}_static_BUILD_SHARED OFF ) + compile_pyx( "${_file_we}_static" generated_file "${_file}" ) + list( APPEND pyx_module_sources "${generated_file}" ) + endif() + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + + if( cython_arguments_MAIN_MODULE ) + set( main_module ${cython_arguments_MAIN_MODULE} ) + endif() + if( NOT main_module ) + message( FATAL_ERROR "main module not found." ) + endif() + get_filename_component( main_module_we "${main_module}" NAME_WE ) + set( CYTHON_FLAGS ${CYTHON_FLAGS} --embed ) + compile_pyx( "${main_module_we}_static" generated_file ${main_module} ) + add_executable( ${_name} ${generated_file} ${pyx_module_sources} ${other_module_sources} ) + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ${pyx_module_libs} ) +endfunction() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/doc b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/doc new file mode 120000 index 00000000000..ff474366ac9 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/doc @@ -0,0 +1 @@ +../doc/jsk_recognition_utils \ No newline at end of file diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/color_utils.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/color_utils.h new file mode 100644 index 00000000000..8a99ca72e11 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/color_utils.h @@ -0,0 +1,68 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_COLOR_UTILS_H_ +#define JSK_RECOGNITION_UTILS_COLOR_UTILS_H_ + +#include + +namespace jsk_recognition_utils +{ + /** + * @brief + * Convert std_msgs::ColorRGBA to cv::Scalar in RGB order. + * It expects 0-1 values in std_msgs::ColorRGBA. + */ + inline cv::Scalar colorROSToCVRGB(const std_msgs::ColorRGBA& ros_color) + { + return cv::Scalar(ros_color.r * 255, + ros_color.g * 255, + ros_color.b * 255); + } + + /** + * @brief + * Convert std_msgs::ColorRGBA to cv::Scalar in BGR order. + * It expects 0-1 values in std_msgs::ColorRGBA. + */ + inline cv::Scalar colorROSToCVBGR(const std_msgs::ColorRGBA& ros_color) + { + return cv::Scalar(ros_color.b * 255, + ros_color.g * 255, + ros_color.r * 255); + } +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/cv_utils.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/cv_utils.h new file mode 100644 index 00000000000..0c1963dee97 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/cv_utils.h @@ -0,0 +1,129 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_CV_UTILS_H_ +#define JSK_RECOGNITION_UTILS_CV_UTILS_H_ + +#include +#include + +namespace jsk_recognition_utils +{ + /** + * @brief + * simple wrapper for cv::calcHist. + * + * cv::MatND is a class to represent histogram in OpenCV 2.x. + * computeHistogram create 1-dimension cv::MatND. + */ + cv::MatND computeHistogram(const cv::Mat& input_image, int bin_size, + float min_value, float max_value, + const cv::Mat& mask_image); + + /** + * @brief + * convert cv::MatND to jsk_recognition_msgs::HistogramimageWithRangeBin array + */ + std::vector + cvMatNDToHistogramWithRangeBinArray(const cv::MatND& cv_hist, float min_value, float max_value); + + /** + * @brief + * convert jsk_recognition_msgs::HistogramimageWithRangeBin array to cv::MatND + */ + cv::MatND + HistogramWithRangeBinArrayTocvMatND( + const std::vector& histogram); + + /** + * @brief + * return true if left.count is larger than right.count. + */ + bool compareHistogramWithRangeBin(const jsk_recognition_msgs::HistogramWithRangeBin& left, + const jsk_recognition_msgs::HistogramWithRangeBin& right); + + /** + * @brief + * sort std::vector. + * largest value will be at the first element. + */ + void sortHistogramWithRangeBinArray(std::vector& bins); + + /** + * @brief + * extract top-N histograms. bins should be sorted. + * top_n_rate should be 0-1. + */ + std::vector + topNHistogramWithRangeBins(const std::vector& bins, + double top_n_rate); + + /** + * @brief + * draw bin to cv::Mat + */ + void + drawHistogramWithRangeBin(cv::Mat& image, + const jsk_recognition_msgs::HistogramWithRangeBin& bin, + float min_width_value, + float max_width_value, + float max_height_value, + cv::Scalar color); + + /** + * @brief + * convert label image to rgb one. + */ + void labelToRGB(const cv::Mat src, cv::Mat& dst); + + + /** + * @brief + * compute bounding rectangle of mask image. + */ + cv::Rect boundingRectOfMaskImage(const cv::Mat& image); + + /** + * @brief + * Check encodings. + */ + bool isBGR(const std::string& encoding); + bool isRGB(const std::string& encoding); + bool isBGRA(const std::string& encoding); + bool isRGBA(const std::string& encoding); + +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/convex_polygon.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/convex_polygon.h new file mode 100644 index 00000000000..c43393fb4db --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/convex_polygon.h @@ -0,0 +1,132 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_CONVEX_POLYGON_H_ +#define JSK_RECOGNITION_UTILS_GEO_CONVEX_POLYGON_H_ + +#include "jsk_recognition_utils/geo/polygon.h" +#include +#include +#include +#include "jsk_recognition_utils/pcl_util.h" + + +namespace jsk_recognition_utils +{ + class ConvexPolygon: public Polygon + { + public: + typedef boost::shared_ptr Ptr; + typedef Eigen::Vector3f Vertex; + typedef std::vector > Vertices; + // vertices should be CW + ConvexPolygon(const Vertices& vertices); + ConvexPolygon(const Vertices& vertices, + const std::vector& coefficients); + //virtual Polygon flip(); + virtual void project(const Eigen::Vector3f& p, Eigen::Vector3f& output); + virtual void project(const Eigen::Vector3d& p, Eigen::Vector3d& output); + virtual void project(const Eigen::Vector3d& p, Eigen::Vector3f& output); + virtual void project(const Eigen::Vector3f& p, Eigen::Vector3d& output); + virtual void projectOnPlane(const Eigen::Vector3f& p, + Eigen::Vector3f& output); + virtual void projectOnPlane(const Eigen::Affine3f& p, + Eigen::Affine3f& output); + virtual bool isProjectableInside(const Eigen::Vector3f& p); + // p should be a point on the plane + virtual ConvexPolygon flipConvex(); + virtual Eigen::Vector3f getCentroid(); + virtual Ptr magnify(const double scale_factor); + virtual Ptr magnifyByDistance(const double distance); + + static ConvexPolygon fromROSMsg(const geometry_msgs::Polygon& polygon); + static ConvexPolygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon& polygon); + bool distanceSmallerThan( + const Eigen::Vector3f& p, double distance_threshold); + bool distanceSmallerThan( + const Eigen::Vector3f& p, double distance_threshold, + double& output_distance); + bool allEdgesLongerThan(double thr); + double distanceFromVertices(const Eigen::Vector3f& p); + geometry_msgs::Polygon toROSMsg(); + protected: + + private: + }; + + template + ConvexPolygon::Ptr convexFromCoefficientsAndInliers( + const typename pcl::PointCloud::Ptr cloud, + const pcl::PointIndices::Ptr inliers, + const pcl::ModelCoefficients::Ptr coefficients) { + typedef typename pcl::PointCloud POINTCLOUD; + typename POINTCLOUD::Ptr projected_cloud(new pcl::PointCloud); + // check inliers has enough points + if (inliers->indices.size() == 0) { + return ConvexPolygon::Ptr(); + } + // project inliers based on coefficients + pcl::ProjectInliers proj; + proj.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); + proj.setInputCloud(cloud); + proj.setModelCoefficients(coefficients); + proj.setIndices(inliers); + proj.filter(*projected_cloud); + // compute convex with giant mutex + { + boost::mutex::scoped_lock lock(global_chull_mutex); + typename POINTCLOUD::Ptr convex_cloud(new pcl::PointCloud); + pcl::ConvexHull chull; + chull.setDimension(2); + chull.setInputCloud (projected_cloud); + chull.reconstruct (*convex_cloud); + if (convex_cloud->points.size() > 0) { + // convert pointcloud to vertices + Vertices vs; + for (size_t i = 0; i < convex_cloud->points.size(); i++) { + Eigen::Vector3f v(convex_cloud->points[i].getVector3fMap()); + vs.push_back(v); + } + return ConvexPolygon::Ptr(new ConvexPolygon(vs)); + } + else { + return ConvexPolygon::Ptr(); + } + } + } +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/cube.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/cube.h new file mode 100644 index 00000000000..f64c86cce57 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/cube.h @@ -0,0 +1,125 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_CUBE_H_ +#define JSK_RECOGNITION_UTILS_GEO_CUBE_H_ + +#include "jsk_recognition_utils/geo/polygon.h" +#include "jsk_recognition_utils/geo/convex_polygon.h" +#include + +namespace jsk_recognition_utils +{ + class Cube + { + public: + typedef boost::shared_ptr Ptr; + Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot); + Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot, + const std::vector& dimensions); + Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot, + const Eigen::Vector3f& dimensions); + Cube(const Eigen::Vector3f& pos, // centroid + const Line& line_a, const Line& line_b, const Line& line_c); + Cube(const jsk_recognition_msgs::BoundingBox& box); + virtual ~Cube(); + std::vector edges(); + ConvexPolygon::Ptr intersectConvexPolygon(Plane& plane); + std::vector getDimensions() const { return dimensions_; }; + void setDimensions(const std::vector& new_dimensions) { + dimensions_[0] = new_dimensions[0]; + dimensions_[1] = new_dimensions[1]; + dimensions_[2] = new_dimensions[2]; + } + jsk_recognition_msgs::BoundingBox toROSMsg(); + + /** + * @brief + * returns vertices as an array of Eigen::Vectro3f. + * The order of the vertices is: + * [1, 1, 1], [-1, 1, 1], [-1, -1, 1], [1, -1, 1], + * [1, 1, -1], [-1, 1, -1], [-1, -1, -1], [1, -1, -1]. + */ + Vertices vertices(); + + /** + * @brief + * returns vertices transformed by pose_offset. + */ + Vertices transformVertices(const Eigen::Affine3f& pose_offset); + + /** + * @brief + * returns all the 6 faces as Polygon::Ptr. + * TODO: is it should be ConvexPolygon? + */ + std::vector faces(); + + /** + * @brief + * compute minimum distance from point p to cube surface. + * + * Distance computation depends on Polygon::nearestPoint and + * this methods just searches a face which resutnrs the smallest + * distance. + */ + virtual Eigen::Vector3f nearestPoint(const Eigen::Vector3f& p, + double& distance); + protected: + Eigen::Vector3f pos_; + Eigen::Quaternionf rot_; + std::vector dimensions_; + + /** + * @brief + * A helper method to build polygon from 4 vertices. + */ + virtual Polygon::Ptr buildFace(const Eigen::Vector3f v0, + const Eigen::Vector3f v1, + const Eigen::Vector3f v2, + const Eigen::Vector3f v3); + + /** + * @brief + * A helper method to build vertex from x-y-z relatiev coordinates. + */ + virtual Eigen::Vector3f buildVertex(double i, double j, double k); + + private: + + }; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/cylinder.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/cylinder.h new file mode 100644 index 00000000000..d4eef19c465 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/cylinder.h @@ -0,0 +1,75 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_CYLINDER_H_ +#define JSK_RECOGNITION_UTILS_GEO_CYLINDER_H_ + +#include +#include +#include +#include +#include + +namespace jsk_recognition_utils +{ + class Cylinder // infinite + { + public: + typedef boost::shared_ptr Ptr; + Cylinder(Eigen::Vector3f point, Eigen::Vector3f direction, double radius); + + virtual void filterPointCloud(const pcl::PointCloud& cloud, + const double threshold, + pcl::PointIndices& output); + virtual void estimateCenterAndHeight(const pcl::PointCloud& cloud, + const pcl::PointIndices& indices, + Eigen::Vector3f& center, + double& height); + virtual void toMarker(visualization_msgs::Marker& marker, + const Eigen::Vector3f& center, + const Eigen::Vector3f& uz, + const double height); + virtual Eigen::Vector3f getDirection(); + virtual double getRadius() { return radius_; } + protected: + Eigen::Vector3f point_; + Eigen::Vector3f direction_; + double radius_; + private: + + }; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/grid_plane.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/grid_plane.h new file mode 100644 index 00000000000..500876fd558 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/grid_plane.h @@ -0,0 +1,171 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_GRID_PLANE_H_ +#define JSK_RECOGNITION_UTILS_GEO_GRID_PLANE_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "jsk_recognition_utils/geo/convex_polygon.h" +#include "jsk_recognition_utils/geo/cube.h" + +#include + +namespace jsk_recognition_utils +{ + /** + * @brief + * Grid based representation of planar region. + * + * Each cell represents a square region as belows: + * +--------+ + * | | + * | + | + * | | + * +--------+ + * + * The width and height of the cell is equivalent to resolution_, + * and the value of cells_ represents a center point. + * (i, j) means rectanglar region of (x, y) which satisfies followings: + * i * resolution - 0.5 * resolution <= x < i * resolution + 0.5 * resolution + * j * resolution - 0.5 * resolution <= y < j * resolution + 0.5 * resolution + * + * + */ + class GridPlane + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::tuple IndexPair; + typedef std::set IndexPairSet; + GridPlane(ConvexPolygon::Ptr plane, const double resolution); + virtual ~GridPlane(); + virtual GridPlane::Ptr clone(); // shallow copy + virtual size_t fillCellsFromPointCloud( + pcl::PointCloud::Ptr& cloud, + double distance_threshold); + virtual size_t fillCellsFromPointCloud( + pcl::PointCloud::Ptr& cloud, + double distance_threshold, + std::set& non_plane_indices); + virtual size_t fillCellsFromPointCloud( + pcl::PointCloud::Ptr& cloud, + double distance_threshold, + double normal_threshold, + std::set& non_plane_indices); + virtual void fillCellsFromCube(Cube& cube); + virtual double getResolution() { return resolution_; } + virtual jsk_recognition_msgs::SimpleOccupancyGrid toROSMsg(); + /** + * @brief + * Construct GridPlane object from + * jsk_recognition_msgs::SimpleOccupancyGrid. + */ + static GridPlane fromROSMsg( + const jsk_recognition_msgs::SimpleOccupancyGrid& rosmsg, + const Eigen::Affine3f& offset); + virtual bool isOccupied(const IndexPair& pair); + + /** + * @brief + * p should be local coordinate + */ + virtual bool isOccupied(const Eigen::Vector3f& p); + + /** + * @brief + * p should be global coordinate + */ + virtual bool isOccupiedGlobal(const Eigen::Vector3f& p); + + /** + * @brief + * Project 3-D point to GridPlane::IndexPair. + * p should be represented in local coordinates. + */ + virtual IndexPair projectLocalPointAsIndexPair(const Eigen::Vector3f& p); + + /** + * @brief + * Unproject GridPlane::IndexPair to 3-D local point. + */ + virtual Eigen::Vector3f unprojectIndexPairAsLocalPoint(const IndexPair& pair); + + /** + * @brief + * Unproject GridPlane::IndexPair to 3-D global point. + */ + virtual Eigen::Vector3f unprojectIndexPairAsGlobalPoint(const IndexPair& pair); + + /** + * @brief + * Add IndexPair to this instance. + */ + virtual void addIndexPair(IndexPair pair); + + /** + * @brief + * Erode grid cells with specified number of pixels + */ + virtual GridPlane::Ptr erode(int num); + + /** + * @brief + * return ConvexPolygon pointer of this instance. + */ + virtual ConvexPolygon::Ptr getPolygon() { return convex_; } + + /** + * @brief + * Dilate grid cells with specified number of pixels + */ + virtual GridPlane::Ptr dilate(int num); + protected: + ConvexPolygon::Ptr convex_; + IndexPairSet cells_; + double resolution_; + private: + + }; + +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/line.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/line.h new file mode 100644 index 00000000000..aa204fdf868 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/line.h @@ -0,0 +1,203 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_LINE_H_ +#define JSK_RECOGNITION_UTILS_GEO_LINE_H_ + +#include +#include +#include "jsk_recognition_utils/types.h" + +namespace jsk_recognition_utils +{ + /** + * @brief + * Class to represent 3-D straight line. + */ + class Line + { + public: + typedef boost::shared_ptr Ptr; + + /** + * @brief + * Construct a line from direction vector and a point on the line. + * + * @param direction direction of the line. non-normalized vector is allowed. + * @param origin point on the line. + */ + Line(const Eigen::Vector3f& direction, const Eigen::Vector3f& origin); + + /** + * @brief + * get normalized direction vector of the line and assign it to output. + */ + virtual void getDirection(Eigen::Vector3f& output) const; + + /** + * @brief + * get normalized direction vector of the line. + */ + virtual Eigen::Vector3f getDirection() const; + + /** + * @brief + * get origin of the line and assing it to output. + */ + virtual void getOrigin(Eigen::Vector3f& output) const; + + /** + * @brief + * get origin of the line. + */ + virtual Eigen::Vector3f getOrigin() const; + + /** + * @brief + * compute a distance to a point + */ + virtual double distanceToPoint(const Eigen::Vector3f& from) const; + + /** + * @brief + * compute a distance to a point and foot point will be assigned to foot. + */ + virtual double distanceToPoint(const Eigen::Vector3f& from, + Eigen::Vector3f& foot) const; + + /** + * @brief + * compute a distance to line. + */ + virtual double distance(const Line& other) const; + + /** + * @brief + * compute a point which gives perpendicular projection. + */ + virtual void foot(const Eigen::Vector3f& point, Eigen::Vector3f& output) const; + + /** + * @brief + * compute angle between a given line. + */ + virtual double angle(const Line& other) const; + + /** + * @brief + * return true if given line is parallel. + * angle_threshold is error tolerance. + */ + virtual bool isParallel(const Line& other, double angle_threshold = 0.1) const; + + /** + * @brief + * return true if given line is perpendicular. + * angle_threshold is error tolerance. + */ + virtual bool isPerpendicular(const Line& other, double angle_threshold = 0.1) const; + + /** + * @brief + * compute a middle line between given line. + */ + virtual Ptr midLine(const Line& other) const; + + /** + * @brief + * compute a line on a point, whose direction is same to the + * current line. + */ + virtual Ptr parallelLineOnAPoint(const Eigen::Vector3f& p) const; + + /** + * @brief + * Extract end points from the points on the lines. + */ + virtual PointPair findEndPoints(const Vertices& points) const; + + /** + * @ brief + * Let equation of line's scale factor a. + * + * x = a d + p + * where d is a normalized direction vector and p is a point on the vector. + */ + virtual double computeAlpha(const Point& p) const; + + /** + * @ brief + * Return true if given line is towards the same direction. + */ + virtual bool isSameDirection(const Line& other) const; + + /** + * @ brief + * flip direction of line. + */ + virtual Line::Ptr flip(); + + /** + * @brief + * compute a perpendicular line of two lines from origin_ + */ + virtual void parallelLineNormal(const Line& other, Eigen::Vector3f& output) const; + + /** + * @brief + * Instantiate Line from array of float. + */ + static Ptr fromCoefficients(const std::vector& coefficients); + + /** + * @brief + * Print Line information + */ + virtual void print(); + + /** + * @brief + * Compute a point on normal from alpha parameter. + */ + virtual void point(double alpha, Eigen::Vector3f& ouptut); + protected: + Eigen::Vector3f direction_; + Eigen::Vector3f origin_; + private: + }; + +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/plane.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/plane.h new file mode 100644 index 00000000000..cb570a37fb1 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/plane.h @@ -0,0 +1,93 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_PLANE_H_ +#define JSK_RECOGNITION_UTILS_GEO_PLANE_H_ + +#include +#include +#include +#include +#include "jsk_recognition_utils/types.h" + +namespace jsk_recognition_utils +{ + class Plane + { + public: + typedef boost::shared_ptr Ptr; + Plane(const std::vector& coefficients); + Plane(const boost::array& coefficients); + Plane(Eigen::Vector3f normal, double d); + Plane(Eigen::Vector3f normal, Eigen::Vector3f p); + virtual ~Plane(); + virtual Plane flip(); + virtual Plane::Ptr faceToOrigin(); + virtual bool isSameDirection(const Plane& another); + virtual bool isSameDirection(const Eigen::Vector3f& another_normal); + virtual double signedDistanceToPoint(const Eigen::Vector4f p); + virtual double distanceToPoint(const Eigen::Vector4f p); + virtual double signedDistanceToPoint(const Eigen::Vector3f p); + virtual double distanceToPoint(const Eigen::Vector3f p); + virtual double distance(const Plane& another); + virtual double angle(const Plane& another); + virtual double angle(const Eigen::Vector3f& vector); + virtual void project(const Eigen::Vector3f& p, Eigen::Vector3f& output); + virtual void project(const Eigen::Vector3d& p, Eigen::Vector3d& output); + virtual void project(const Eigen::Vector3d& p, Eigen::Vector3f& output); + virtual void project(const Eigen::Vector3f& p, Eigen::Vector3d& output); + virtual void project(const Eigen::Affine3d& pose, Eigen::Affine3d& output); + virtual void project(const Eigen::Affine3f& pose, Eigen::Affine3f& output); + virtual Eigen::Vector3f getNormal(); + virtual Eigen::Vector3f getPointOnPlane(); + virtual Plane transform(const Eigen::Affine3d& transform); + virtual Plane transform(const Eigen::Affine3f& transform); + virtual void toCoefficients(std::vector& output); + virtual std::vector toCoefficients(); + virtual double getD(); + virtual Eigen::Affine3f coordinates(); + protected: + virtual void initializeCoordinates(); + Eigen::Vector3f normal_; + double d_; + Eigen::Affine3f plane_coordinates_; + private: + }; + +} + +#endif + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/polygon.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/polygon.h new file mode 100644 index 00000000000..b17a8b14307 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/polygon.h @@ -0,0 +1,243 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_POLYGON_H_ +#define JSK_RECOGNITION_UTILS_GEO_POLYGON_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "jsk_recognition_utils/geo/plane.h" +#include "jsk_recognition_utils/geo/segment.h" +#include "jsk_recognition_utils/sensor_model/camera_depth_sensor.h" +#include "jsk_recognition_utils/pcl_conversion_util.h" +#include "jsk_recognition_utils/random_util.h" + +namespace jsk_recognition_utils +{ + class Polygon: public Plane + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::tuple PtrPair; + Polygon(const Vertices& vertices); + Polygon(const Vertices& vertices, + const std::vector& coefficients); + virtual ~Polygon(); + virtual std::vector decomposeToTriangles(); + virtual void clearTriangleDecompositionCache() { + cached_triangles_.clear(); + } + + virtual Eigen::Vector3f getNormalFromVertices(); + virtual bool isTriangle(); + Eigen::Vector3f randomSampleLocalPoint(boost::mt19937& random_generator); + virtual void getLocalMinMax(double& min_x, double& min_y, + double& max_x, double& max_y); + template + typename pcl::PointCloud::Ptr samplePoints(double grid_size) + { + typename pcl::PointCloud::Ptr + ret (new pcl::PointCloud); + double min_x, min_y, max_x, max_y; + getLocalMinMax(min_x, min_y, max_x, max_y); + // ROS_INFO("min_x: %f", min_x); + // ROS_INFO("min_y: %f", min_y); + // ROS_INFO("max_x: %f", max_x); + // ROS_INFO("max_y: %f", max_y); + // Decompose into triangle first for optimization + std::vector triangles = decomposeToTriangles(); + + for (double x = min_x; x < max_x; x += grid_size) { + for (double y = min_y; y < max_y; y += grid_size) { + Eigen::Vector3f candidate(x, y, 0); + Eigen::Vector3f candidate_global = coordinates() * candidate; + // check candidate is inside of the polygon or not + bool insidep = false; + for (size_t i = 0; i < triangles.size(); i++) { + if (triangles[i]->isInside(candidate_global)) { + insidep = true; + break; + } + } + if (insidep) { + PointT p; + p.x = candidate_global[0]; + p.y = candidate_global[1]; + p.z = candidate_global[2]; + p.normal_x = normal_[0]; + p.normal_y = normal_[1]; + p.normal_z = normal_[2]; + ret->points.push_back(p); + } + } + } + return ret; + } + + /** + * @brief + * get all the edges as point of Segment. + */ + std::vector edges() const; + + /** + * @brief + * Compute distance between point and this polygon. + */ + double distance(const Eigen::Vector3f& point); + + /** + * @brief + * Compute distance between point and this polygon. + * Nearest point on this polygon can be gotten. + */ + double distance(const Eigen::Vector3f& point, + Eigen::Vector3f& nearest_point); + + /** + * @brief + * Compute nearest point from p on this polygon. + * + * This method first project p onth the polygon and + * if the projected point is inside of polygon, + * the projected point is the nearest point. + * If not, distances between the point and edges are + * computed and search the nearest point. + * + * In case of searching edges, it is achieved in brute-force + * searching and if the number of edges is huge, it may take + * a lot of time to compute. + * + * This method cannot be used for const instance because + * triangle decomposition will change the cache in the instance. + * + * Distance between p and nearest point is stored in distance. + */ + virtual Eigen::Vector3f nearestPoint(const Eigen::Vector3f& p, + double& distance); + virtual size_t getNumVertices(); + virtual size_t getFarestPointIndex(const Eigen::Vector3f& O); + virtual Eigen::Vector3f directionAtPoint(size_t i); + virtual Eigen::Vector3f getVertex(size_t i); + virtual PointIndexPair getNeighborIndex(size_t index); + virtual Vertices getVertices() { return vertices_; }; + virtual double area(); + virtual bool isPossibleToRemoveTriangleAtIndex( + size_t index, + const Eigen::Vector3f& direction); + virtual PtrPair separatePolygon(size_t index); + /** + * @brief + * return true if p is inside of polygon. + * p should be in global coordinates. + */ + virtual bool isInside(const Eigen::Vector3f& p); + size_t previousIndex(size_t i); + size_t nextIndex(size_t i); + + static Polygon fromROSMsg(const geometry_msgs::Polygon& polygon); + static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon& polygon); + static Polygon createPolygonWithSkip(const Vertices& vertices); + + /** + * @brief + * convert jsk_recognition_msgs::PolygonArray + * to std::vector. + * It requires offset transformation in the 2nd argument. + */ + static std::vector fromROSMsg(const jsk_recognition_msgs::PolygonArray& msg, + const Eigen::Affine3f& offset = Eigen::Affine3f::Identity()); + + /** + * @brief + * transform Polygon with given transform. + * cached triangles is cleared. + */ + virtual void transformBy(const Eigen::Affine3d& transform); + + /** + * @brief + * transform Polygon with given transform. + * cached triangles is cleared. + */ + virtual void transformBy(const Eigen::Affine3f& transform); + + /** + * @brief + * generate mask image of the polygon. + * if all the points are outside of field-of-view, + * returns false. + */ + virtual bool maskImage(const jsk_recognition_utils::CameraDepthSensor& model, + cv::Mat& image) const; + + /** + * @brief + * draw line of polygons on image. + */ + virtual void drawLineToImage(const jsk_recognition_utils::CameraDepthSensor& model, + cv::Mat& image, + const cv::Scalar& color, + const int line_width = 1) const; + virtual bool isConvex(); + virtual Eigen::Vector3f centroid(); + template void boundariesToPointCloud( + pcl::PointCloud& output) { + output.points.resize(vertices_.size()); + for (size_t i = 0; i < vertices_.size(); i++) { + Eigen::Vector3f v = vertices_[i]; + PointT p; + p.x = v[0]; p.y = v[1]; p.z = v[2]; + output.points[i] = p; + } + output.height = 1; + output.width = output.points.size(); + } + + protected: + Vertices vertices_; + std::vector cached_triangles_; + private: + + }; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/polyline.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/polyline.h new file mode 100644 index 00000000000..49bfcf2a1c9 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/polyline.h @@ -0,0 +1,122 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_POLYLINE_H_ +#define JSK_RECOGNITION_UTILS_GEO_POLYLINE_H_ + +#include +#include +#include "jsk_recognition_utils/types.h" +#include "jsk_recognition_utils/geo/segment.h" +#include "visualization_msgs/Marker.h" + +namespace jsk_recognition_utils +{ + /** + * @brief + * Class to represent 3-D polyline (not closed). + */ + class PolyLine : public Line + { + public: + typedef boost::shared_ptr Ptr; + /** + * @brief + * Construct a polyline from points. + * The polyline consists of lines which starts with p[i] and ends with p[i+1]. + * @param points + */ + PolyLine(const std::vector < Eigen::Vector3f > &points); + + /** + * @ brief + * get the line positioned at index + * + * @param index position of the line + */ + virtual Segment::Ptr at(int index) const; + + /** + * @brief + * compute a distance to a point + */ + virtual double distance(const Eigen::Vector3f& point, + Eigen::Vector3f& foot_point) const; + virtual double distance(const Eigen::Vector3f& point) const; + + /** + * @brief + * compute a distance to a point, get various information + */ + virtual double distanceWithInfo(const Eigen::Vector3f& from, + Eigen::Vector3f& foot_point, + double& distance_to_goal, + int& foot_index, + double& foot_alpha) const; + + /** + * @brief + * get normalized direction vector of the line. + * + * @param index position of the line which returns direction + */ + virtual void getDirection(int index, Eigen::Vector3f& output) const; + virtual Eigen::Vector3f getDirection(int index) const; + + /** + * @brief + * get total length of the polyline + */ + virtual double length() const; + + /** + * @ brief + * flip direction of the polyline. + */ + virtual PolyLine::Ptr flipPolyLine() const; + + /** + * @ brief + * make marker message to display the polyline + */ + void toMarker(visualization_msgs::Marker& marker) const; + + friend std::ostream& operator<<(std::ostream& os, const PolyLine& pl); + protected: + std::vector< Segment::Ptr > segments; + }; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/segment.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/segment.h new file mode 100644 index 00000000000..13ecf51dc60 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo/segment.h @@ -0,0 +1,123 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_SEGMENT_H_ +#define JSK_RECOGNITION_UTILS_GEO_SEGMENT_H_ + +#include +#include "jsk_recognition_utils/geo/line.h" +#include "visualization_msgs/Marker.h" + +namespace jsk_recognition_utils +{ + class Plane; + /** + * @brief + * Class to represent 3-D straight line which has finite length. + */ + class Segment: public Line + { + public: + typedef boost::shared_ptr Ptr; + + /** + * @brief + * Construct a line from a start point and a goal point. + * + * @param from + * @param to + */ + Segment(const Eigen::Vector3f& from, const Eigen::Vector3f to); + + /** + * @brief + * get end of the line and assing it to output. + */ + virtual void getEnd(Eigen::Vector3f& output) const; + virtual Eigen::Vector3f getEnd() const; + + virtual void foot(const Eigen::Vector3f& point, Eigen::Vector3f& output) const; + virtual double dividingRatio(const Eigen::Vector3f& point) const; + virtual double distance(const Eigen::Vector3f& point) const; + virtual double distance(const Eigen::Vector3f& point, Eigen::Vector3f& foot_point) const; + virtual bool intersect(Plane& plane, Eigen::Vector3f& point) const; + virtual void midpoint(Eigen::Vector3f& midpoint) const; + //virtual double distance(const Segment& other); + friend std::ostream& operator<<(std::ostream& os, const Segment& seg); + + /** + * @brief + * compute a distance to a point + * @param from + * @param foot_point + * @param distance_to_goal + */ + virtual double distanceWithInfo(const Eigen::Vector3f& from, + Eigen::Vector3f& foot_point, + double &distance_to_goal) const; + + /** + * @brief + * return flipped line (line of opposite direction) + */ + virtual Segment::Ptr flipSegment() const; + + /** + * @brief + * return length of the line + */ + virtual double length() const; + + /** + * @brief + * make marker message to display the finite line + */ + void toMarker(visualization_msgs::Marker& marker) const; + + /** + * @brief + * is crossing with another line + */ + virtual bool isCross (const Line &ln, double distance_threshold = 1e-5) const; + virtual bool isCross (const Segment &ln, double distance_threshold = 1e-5) const; + + protected: + Eigen::Vector3f to_; + double length_; + private: + }; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo_util.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo_util.h new file mode 100644 index 00000000000..6aa73a43d86 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/geo_util.h @@ -0,0 +1,154 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GEO_UTIL_H_ +#define JSK_RECOGNITION_UTILS_GEO_UTIL_H_ +//#define BOOST_PARAMETER_MAX_ARITY 7 + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +//////////////////////////////////////////////////////// +// PCL headers +//////////////////////////////////////////////////////// +#include +#include +#include +#include +#include +#include +#include +#include + +#include "jsk_recognition_utils/pcl_util.h" +#include "jsk_recognition_utils/random_util.h" + +#include +#include "jsk_recognition_utils/sensor_model/camera_depth_sensor.h" +#include "jsk_recognition_utils/types.h" +#include "jsk_recognition_utils/geo/line.h" +#include "jsk_recognition_utils/geo/segment.h" +#include "jsk_recognition_utils/geo/polyline.h" +#include "jsk_recognition_utils/geo/plane.h" +#include "jsk_recognition_utils/geo/polygon.h" +#include "jsk_recognition_utils/geo/convex_polygon.h" +#include "jsk_recognition_utils/geo/cube.h" +#include "jsk_recognition_utils/geo/cylinder.h" +#include "jsk_recognition_utils/geo/grid_plane.h" + +// Utitlity macros +inline void ROS_INFO_EIGEN_VECTOR3(const std::string& prefix, + const Eigen::Vector3f& v) { + ROS_INFO("%s: [%f, %f, %f]", prefix.c_str(), v[0], v[1], v[2]); +} + +namespace jsk_recognition_utils +{ + //////////////////////////////////////////////////////// + // compute quaternion from 3 unit vector + // these vector should be normalized and diagonal + //////////////////////////////////////////////////////// + Eigen::Quaternionf rotFrom3Axis(const Eigen::Vector3f& ex, + const Eigen::Vector3f& ey, + const Eigen::Vector3f& ez); + /** + * @brief + * Compute PointCloud from Vertices + */ + template + typename pcl::PointCloud::Ptr verticesToPointCloud(const Vertices& v) + { + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + for (size_t i = 0; i < v.size(); i++) { + PointT p; + // Do not use pointFromVectorToXYZ in order not to depend on + // pcl_conversion_util + //pointFromVectorToXYZ(v[i], p); + p.x = v[i][0]; + p.y = v[i][1]; + p.z = v[i][2]; + cloud->points.push_back(p); + } + return cloud; + } + + /** + * @brief + * Compute Vertices from PointCloud + */ + template + Vertices pointCloudToVertices(const pcl::PointCloud& cloud) + { + Vertices vs; + for (size_t i = 0; i < cloud.points.size(); i++) { + Eigen::Vector3f p(cloud.points[i].getVector3fMap()); + vs.push_back(p); + } + return vs; + } + + // geoemtry classes + + std::vector convertToPlanes( + std::vector); + + template + jsk_recognition_msgs::BoundingBox boundingBoxFromPointCloud(const pcl::PointCloud& cloud) + { + Eigen::Vector4f minpt, maxpt; + pcl::getMinMax3D(cloud, minpt, maxpt); + jsk_recognition_msgs::BoundingBox bbox; + bbox.dimensions.x = std::abs(minpt[0] - maxpt[0]); + bbox.dimensions.y = std::abs(minpt[1] - maxpt[1]); + bbox.dimensions.z = std::abs(minpt[2] - maxpt[2]); + bbox.pose.position.x = (minpt[0] + maxpt[0]) / 2.0; + bbox.pose.position.y = (minpt[1] + maxpt[1]) / 2.0; + bbox.pose.position.z = (minpt[2] + maxpt[2]) / 2.0; + bbox.pose.orientation.w = 1.0; + return bbox; + } +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/grid_index.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/grid_index.h new file mode 100644 index 00000000000..8df07caf107 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/grid_index.h @@ -0,0 +1,61 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_GRID_INDEX_H_ +#define JSK_RECOGNITION_UTILS_GRID_INDEX_H_ + +#include + +namespace jsk_recognition_utils +{ + + class GridIndex + { + public: + typedef boost::shared_ptr Ptr; + + GridIndex(); + GridIndex(int _x, int _y); + virtual ~GridIndex(); + int x; + int y; + protected: + private: + + }; + +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/grid_line.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/grid_line.h new file mode 100644 index 00000000000..19ca6d89cf2 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/grid_line.h @@ -0,0 +1,102 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ +#ifndef JSK_RECOGNITION_UTILS_GRID_LINE_H_ +#define JSK_RECOGNITION_UTILS_GRID_LINE_H_ +#include +#include +#include "jsk_recognition_utils/grid_index.h" + +#include +#include + +namespace jsk_recognition_utils +{ + class GridLine + { + public: + typedef boost::shared_ptr Ptr; + GridLine(const pcl::PointXYZRGB a, const pcl::PointXYZRGB b); + virtual ~GridLine(); + virtual bool penetrateGrid(const pcl::PointXYZRGB& A, + const pcl::PointXYZRGB& B, + const pcl::PointXYZRGB& C, + const pcl::PointXYZRGB& D); + virtual bool penetrateGrid(const Eigen::Vector3f A, + const Eigen::Vector3f B, + const Eigen::Vector3f C, + const Eigen::Vector3f D) + { + // std::cout << "checking " + // << "[" << from[0] << ", " << from[1] << ", " << from[2] << "]" + // << "--" + // << "[" << to[0] << ", " << to[1] << ", " << to[2] << "]" + // << std::endl; + // std::cout << " penetrates " + // << "[" << A[0] << ", " << A[1] << ", " << A[2] << "], " + // << "[" << B[0] << ", " << B[1] << ", " << B[2] << "], " + // << "[" << C[0] << ", " << C[1] << ", " << C[2] << "], " + // << "[" << D[0] << ", " << D[1] << ", " << D[2] << "]" << std::endl; + Eigen::Vector3f Across = (A - from).cross(d_); + Eigen::Vector3f Bcross = (B - from).cross(d_); + Eigen::Vector3f Ccross = (C - from).cross(d_); + Eigen::Vector3f Dcross = (D - from).cross(d_); + bool ab_direction = Across.dot(Bcross) < 0; + bool ac_direction = Across.dot(Ccross) < 0; + bool ad_direction = Across.dot(Dcross) < 0; + bool bc_direction = Bcross.dot(Ccross) < 0; + if (Across.norm() == 0 || + Bcross.norm() == 0 || + Ccross.norm() == 0 || + Dcross.norm() == 0) { + return true; + } + else if ((ab_direction == ac_direction) && + (ab_direction == ad_direction) + && (ab_direction == bc_direction)) { + return false; + } + else { + return true; + } + } + + const Eigen::Vector3f from; + const Eigen::Vector3f to; + protected: + const Eigen::Vector3f d_; + }; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/grid_map.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/grid_map.h new file mode 100644 index 00000000000..2f78ef8a7d3 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/grid_map.h @@ -0,0 +1,132 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ +#ifndef JSK_RECOGNITION_UTILS_GRID_MAP_H_ +#define JSK_RECOGNITION_UTILS_GRID_MAP_H_ +#include + +#include "jsk_recognition_utils/grid_index.h" +#include "jsk_recognition_utils/grid_line.h" +#include +#include +#include +#include +#include +#include + +#include "jsk_recognition_utils/geo_util.h" +#include + +namespace jsk_recognition_utils +{ + + // infinity range, might be slow... + class GridMap + { + public: + typedef boost::shared_ptr Ptr; + typedef std::set RowIndices; + typedef std::map Columns; + typedef Columns::iterator ColumnIterator; + typedef std::set::iterator RowIterator; + GridMap(double resolution, const std::vector& coefficients); + virtual ~GridMap(); + virtual void registerPoint(const pcl::PointXYZRGB& point); + virtual std::vector registerLine(const pcl::PointXYZRGB& from, const pcl::PointXYZRGB& to); + virtual void removeIndex(const GridIndex::Ptr& index); + virtual void registerPointCloud(pcl::PointCloud::Ptr cloud); + virtual GridIndex::Ptr registerIndex(const GridIndex::Ptr& index); + virtual GridIndex::Ptr registerIndex(const int x, const int y); + virtual void pointToIndex(const pcl::PointXYZRGB& point, GridIndex::Ptr index); + virtual void pointToIndex(const Eigen::Vector3f& point, GridIndex::Ptr index); + virtual void indicesToPointCloud(const std::vector& indices, + pcl::PointCloud::Ptr cloud); + virtual bool getValue(const GridIndex::Ptr& index); + virtual bool getValue(const GridIndex& index); + virtual bool getValue(const int x, const int y); + virtual void gridToPoint(GridIndex::Ptr index, Eigen::Vector3f& pos); + virtual void gridToPoint(const GridIndex& index, Eigen::Vector3f& pos); + virtual void gridToPoint2(const GridIndex& index, Eigen::Vector3f& pos); + virtual void fillRegion(const Eigen::Vector3f& start, std::vector& output); + virtual void fillRegion(const GridIndex::Ptr start, std::vector& output); + // toMsg does not fill header, be carefull + virtual void originPose(Eigen::Affine3f& output); + virtual void originPose(Eigen::Affine3d& output); + virtual void toMsg(jsk_recognition_msgs::SparseOccupancyGrid& grid); + virtual Plane toPlane(); + virtual Plane::Ptr toPlanePtr(); + virtual void vote(); + virtual unsigned int getVoteNum(); + virtual void setGeneration(unsigned int generation); + virtual unsigned int getGeneration(); + virtual std::vector getCoefficients(); + virtual bool isBinsOccupied(const Eigen::Vector3f& p); + virtual int normalizedWidth(); + virtual int normalizedHeight(); + virtual boost::tuple minMaxX(); + virtual boost::tuple minMaxY(); + virtual int widthOffset(); + virtual int heightOffset(); + virtual int normalizedIndex(int width_offset, int height_offset, + int step, + int elem_size, + int original_x, int original_y); + virtual cv::Mat toImage(); + virtual bool check4Neighbor(int x, int y); + virtual ConvexPolygon::Ptr toConvexPolygon(); + virtual pcl::PointCloud::Ptr toPointCloud(); + virtual void decrease(int i); + virtual void add(GridMap& other); + protected: + virtual void decreaseOne(); + + double resolution_; + Eigen::Vector3f O_; + + // plane parameter + Eigen::Vector3f normal_; + double d_; + + Eigen::Vector3f ex_, ey_; + + std::vector lines_; + Columns data_; + unsigned int vote_; + unsigned int generation_; + private: + }; + +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl/color_histogram.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl/color_histogram.h new file mode 100644 index 00000000000..956558b757a --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl/color_histogram.h @@ -0,0 +1,253 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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/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. + *********************************************************************/ +/* + * color_histogram.h + * Author: Yuki Furuta + */ + +#ifndef JSK_RECOGNITION_UTILS_COLOR_HISTOGRAM_H__ +#define JSK_RECOGNITION_UTILS_COLOR_HISTOGRAM_H__ + +#include +#include +#include +#include +#include + +namespace jsk_recognition_utils { + enum HistogramPolicy { + HUE = 0, + HUE_AND_SATURATION + }; + + enum ComparePolicy { + CORRELATION = 0, + BHATTACHARYYA, + INTERSECTION, + CHISQUARE, + KL_DIVERGENCE + }; + + struct PointXYZHLS { + double x, y, z; + double h, l, s; + }; + + inline void + HSV2HLS(const pcl::PointXYZHSV& hsv, PointXYZHLS& hls) { + hls.x = hsv.x; hls.y = hsv.y; hls.z = hsv.z; + hls.h = hsv.h; + hls.l = (2.0 - hsv.s) * hsv.v; + hls.s = hsv.s * hsv.v; + hls.s /= (hls.l <= 1.0) ? hls.l : 2.0 - hls.l; + hls.l /= 2.0; + } + + inline int + getHistogramBin(const double& val, const int& step, + const double& min, const double& max) + { + int idx = std::floor((val - min) / (max - min) * step); + if (idx >= step) return step - 1; + else if (idx <= 0) return 0; + else return idx; + } + + inline void + normalizeHistogram(std::vector& histogram) + { + float sum = 0.0f; + for (size_t i = 0; i < histogram.size(); ++i) + sum += histogram[i]; + if (sum != 0.0) { + for (size_t i = 0; i < histogram.size(); ++i) + histogram[i] /= sum; + } + } + + inline void + computeColorHistogram1d(const pcl::PointCloud& cloud, + std::vector& histogram, + const int bin_size, + const double white_threshold=0.1, + const double black_threshold=0.1) + { + histogram.resize(bin_size + 2, 0.0f); // h + w + b + int white_index = bin_size; + int black_index = bin_size + 1; + + for (size_t i = 0; i < cloud.points.size(); ++i) { + PointXYZHLS p; + HSV2HLS(cloud.points[i], p); + if (p.l > 1.0 - white_threshold) + histogram[white_index] += 1.0f; + else if (p.l < black_threshold) + histogram[black_index] += 1.0f; + else { + int h_bin = getHistogramBin(p.h, bin_size, 0.0, 360.0); + histogram[h_bin] += 1.0f; + } + } + + normalizeHistogram(histogram); + } + + inline void + computeColorHistogram2d(const pcl::PointCloud& cloud, + std::vector& histogram, + const int bin_size_per_channel, + const double white_threshold=0.1, + const double black_threshold=0.1) + { + histogram.resize(bin_size_per_channel * bin_size_per_channel + 2, 0.0f); // h * s + w + b + int white_index = bin_size_per_channel * bin_size_per_channel; + int black_index = bin_size_per_channel * bin_size_per_channel + 1; + + // vote + for (size_t i = 0; i < cloud.points.size(); ++i) { + PointXYZHLS p; + HSV2HLS(cloud.points[i], p); + if (p.l < white_threshold) + histogram[white_index] += 1.0f; + else if (p.l > 1.0 - black_threshold) + histogram[black_index] += 1.0f; + else { + int h_bin = getHistogramBin(p.h, bin_size_per_channel, 0.0, 360.0); + int s_bin = getHistogramBin(p.s, bin_size_per_channel, 0.0, 1.0); + histogram[s_bin * bin_size_per_channel + h_bin] += 1.0f; + } + } + + normalizeHistogram(histogram); + } + + inline void + rotateHistogram1d(const std::vector& input, + std::vector& output, + const double degree) + { + size_t bin_size = input.size() - 2; + int offset = std::floor(degree / 360.0 * bin_size); + output.resize(input.size()); + for (size_t h = 0; h < bin_size; ++h) { + int hout = h + offset % bin_size; + output[hout] = input[h]; + } + + // copy white + black + int index = bin_size; + output[index] = input[index]; + output[index+1] = input[index+1]; + } + + inline void + rotateHistogram2d(const std::vector& input, + std::vector& output, + const double degree) + { + size_t bin_size = (size_t)(std::sqrt(input.size() - 2)); + int offset = std::floor(degree / 360.0 * bin_size); + output.resize(input.size()); + for (size_t s = 0; s < bin_size; ++s) { + for (size_t h = 0; h < bin_size; ++h) { + int hout = h + offset % bin_size; + output[s * bin_size + hout] = input[s * bin_size + h]; + } + } + + // copy white + black + int index = bin_size * bin_size; + output[index] = input[index]; + output[index+1] = input[index+1]; + } + + inline bool + compareHistogram(const std::vector& input, + const std::vector& reference, + const ComparePolicy policy, + double& distance) + { + if (input.size() != reference.size()) { + ROS_ERROR("Mismatch histogram bin size"); + return false; + } + + distance = 0.0; + size_t len = input.size(); + if (policy == CHISQUARE) { + for (size_t i = 0; i < len; ++i) { + double a = input[i] - reference[i], b = input[i]; + if (std::fabs(b) > DBL_EPSILON) distance += a * a / b; + } + } else if (policy == CORRELATION) { + double s1 = 0.0, s2 = 0.0, s11 = 0.0, s12 = 0.0, s22 = 0.0; + for (size_t i = 0; i < len; ++i) { + double a = input[i], b = reference[i]; + s11 += a*a; s12 += a*b; s22 += b*b; + s1 += a; s2 += b; + } + double num = s12 - s1*s2/len; + double denom2 = (s11 - s1 * s1 / len) * (s22 - s2 * s2 / len); + distance = std::fabs(denom2) > DBL_EPSILON ? num / std::sqrt(denom2) : 1.0; + } else if (policy == INTERSECTION) { + for (size_t i = 0; i < len; ++i) { + distance += std::min(input[i], reference[i]); + } + } else if (policy == BHATTACHARYYA) { + double s1 = 0.0, s2 = 0.0; + for (size_t i = 0; i < len; ++i) { + distance += std::sqrt(input[i] * reference[i]); + s1 += input[i]; s2 += reference[i]; + } + s1 *= s2; + s1 = std::fabs(s1) > DBL_EPSILON ? 1.0 / std::sqrt(s1) : 1.0; + distance = std::sqrt(std::max(1.0 - distance * s1, 0.0)); + } else if (policy == KL_DIVERGENCE) { + for (size_t i = 0; i < len; ++i) { + double p = input[i], q = reference[i]; + if (std::fabs(p) <= DBL_EPSILON) continue; + if (std::fabs(q) <= DBL_EPSILON) q = 1e-10; + distance += p * std::log(p / q); + } + } else { + ROS_ERROR("Invalid compare policy"); + return false; + } + + return true; + } +} // namespace + +#endif // JSK_RECOGNITION_UTILS_COLOR_HISTOGRAM_H__ diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl/ear_clipping_patched.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl/ear_clipping_patched.h new file mode 100644 index 00000000000..c4b90405551 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl/ear_clipping_patched.h @@ -0,0 +1,127 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_PCL_ROS_EAR_CLIPPING_PATCHED_H_ +#define JSK_PCL_ROS_EAR_CLIPPING_PATCHED_H_ + +#include +#include + +namespace pcl +{ + + /** \brief The ear clipping triangulation algorithm. + * The code is inspired by Flavien Brebion implementation, which is + * in n^3 and does not handle holes. + * \author Nicolas Burrus + * \ingroup surface + */ + class PCL_EXPORTS EarClippingPatched : public MeshProcessing + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + using MeshProcessing::input_mesh_; + using MeshProcessing::initCompute; + /** \brief Empty constructor */ + EarClippingPatched () : MeshProcessing (), points_ () + { + }; + + protected: + /** \brief a Pointer to the point cloud data. */ + pcl::PointCloud::Ptr points_; + + /** \brief This method should get called before starting the actual computation. */ + bool + initCompute (); + + /** \brief The actual surface reconstruction method. + * \param[out] output the output polygonal mesh + */ + void + performProcessing (pcl::PolygonMesh &output); + + /** \brief Triangulate one polygon. + * \param[in] vertices the set of vertices + * \param[out] output the resultant polygonal mesh + */ + void + triangulate (const Vertices& vertices, PolygonMesh& output); + + /** \brief Compute the signed area of a polygon. + * \param[in] vertices the vertices representing the polygon + */ + float + area (const std::vector& vertices); + + /** \brief Check if the triangle (u,v,w) is an ear. + * \param[in] u the first triangle vertex + * \param[in] v the second triangle vertex + * \param[in] w the third triangle vertex + * \param[in] vertices a set of input vertices + */ + bool + isEar (int u, int v, int w, const std::vector& vertices); + + /** \brief Check if p is inside the triangle (u,v,w). + * \param[in] u the first triangle vertex + * \param[in] v the second triangle vertex + * \param[in] w the third triangle vertex + * \param[in] p the point to check + */ + bool + isInsideTriangle (const Eigen::Vector3f& u, + const Eigen::Vector3f& v, + const Eigen::Vector3f& w, + const Eigen::Vector3f& p); + + /** \brief Compute the cross product between 2D vectors. + * \param[in] p1 the first 2D vector + * \param[in] p2 the first 2D vector + */ + float + crossProduct (const Eigen::Vector2f& p1, const Eigen::Vector2f& p2) const + { + return p1[0]*p2[1] - p1[1]*p2[0]; + } + + }; + +} + + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl_conversion_util.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl_conversion_util.h new file mode 100644 index 00000000000..af598544de3 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl_conversion_util.h @@ -0,0 +1,247 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_PCL_CONVERSION_UTIL_H_ +#define JSK_RECOGNITION_UTILS_PCL_CONVERSION_UTIL_H_ +#include +#include +#include +#include +#include +#include +#include +#include +#if ROS_VERSION_MINIMUM(1, 10, 0) +// hydro and later +typedef pcl_msgs::PointIndices PCLIndicesMsg; +typedef pcl_msgs::ModelCoefficients PCLModelCoefficientMsg; +#else +// groovy +typedef pcl::PointIndices PCLIndicesMsg; +typedef pcl::ModelCoefficients PCLModelCoefficientMsg; +#endif + +#include + +namespace jsk_recognition_utils +{ + + /** @brief + * Convert pcl::RangeImage to cv::Mat. Distance is normalized + * to 0-1 and colorized. + * + * @param range_image instance of pcl::RangeImage + * @param mat instance of cv::Mat, converted cv::Mat is set into + * this argument. + */ + void rangeImageToCvMat(const pcl::RangeImage& range_image, + cv::Mat& mat); + + template + void pointFromXYZToVector(const FromT& msg, + ToT& p) + { + p[0] = msg.x; p[1] = msg.y; p[2] = msg.z; + } + + template + void pointFromVectorToXYZ(const FromT& p, + ToT& msg) + { + msg.x = p[0]; msg.y = p[1]; msg.z = p[2]; + } + + template + void pointFromXYZToXYZ(const FromT& from, + ToT& to) + { + to.x = from.x; to.y = from.y; to.z = from.z; + } + + template + void pointFromVectorToVector(const FromT& from, + ToT& to) + { + to[0] = from[0]; to[1] = from[1]; to[2] = from[2]; + } + + template + void convertMatrix4(const FromT& from, + ToT& to) + { + for (size_t i = 0; i < 4; i++) { + for (size_t j = 0; j < 4; j++) { + to(i, j) = from(i, j); + } + } + } + + void convertEigenAffine3(const Eigen::Affine3d& from, + Eigen::Affine3f& to); + void convertEigenAffine3(const Eigen::Affine3f& from, + Eigen::Affine3d& to); + + template + void markerMsgToPointCloud(const visualization_msgs::Marker& input_marker, + int sample_nums, + pcl::PointCloud& output_cloud) + { + std::vector cumulative_areas; + double total_area = 0; + + if (input_marker.points.size() != input_marker.colors.size()){ + ROS_ERROR("Color and Points nums is different in markerMsgToPointCloud"); + return; + } + + //Gether the triangle areas + for (int i = 0; i < (int)input_marker.points.size()/3; i++){ + geometry_msgs::Point p0_1; + p0_1.x = input_marker.points[i*3].x - input_marker.points[i*3+2].x; + p0_1.y = input_marker.points[i*3].y - input_marker.points[i*3+2].y; + p0_1.z = input_marker.points[i*3].z - input_marker.points[i*3+2].z; + geometry_msgs::Point p1_2; + p1_2.x = input_marker.points[i*3+1].x - input_marker.points[i*3+2].x; + p1_2.y = input_marker.points[i*3+1].y - input_marker.points[i*3+2].y; + p1_2.z = input_marker.points[i*3+1].z - input_marker.points[i*3+2].z; + geometry_msgs::Point outer_product; + outer_product.x = p0_1.y * p1_2.z - p0_1.z * p1_2.y; + outer_product.y = p0_1.z * p1_2.x - p0_1.x * p1_2.z; + outer_product.z = p0_1.x * p1_2.y - p0_1.y * p1_2.x; + double tmp_triangle_area = abs(sqrt( pow(outer_product.x*1000, 2) + pow(outer_product.y*1000, 2) + pow(outer_product.z*1000, 2)))/2; + total_area += tmp_triangle_area; + cumulative_areas.push_back(total_area); + } + + //Gether Random sampling points in propotion to area size + for(int i = 0; i < sample_nums; i++){ + double r = rand() * (1.0 / (RAND_MAX + 1.0)) * total_area; + std::vector::iterator low = std::lower_bound (cumulative_areas.begin (), cumulative_areas.end (), r); + int index = int(low - cumulative_areas.begin ()); + + //Get Target Triangle + PointT p; + std_msgs::ColorRGBA color; + geometry_msgs::Point p0 = input_marker.points[index*3]; + geometry_msgs::Point p1 = input_marker.points[index*3+1]; + geometry_msgs::Point p2 = input_marker.points[index*3+2]; + std_msgs::ColorRGBA c0= input_marker.colors[index*3]; + std_msgs::ColorRGBA c1 = input_marker.colors[index*3+1]; + std_msgs::ColorRGBA c2 = input_marker.colors[index*3+2]; + r = rand() * (1.0 / (RAND_MAX + 1.0)); + + geometry_msgs::Point point_on_p1_p2; + point_on_p1_p2.x = p1.x*r + p2.x*(1.0 - r); + point_on_p1_p2.y = p1.y*r + p2.y*(1.0 - r); + point_on_p1_p2.z = p1.z*r + p2.z*(1.0 - r); + + color.r = c1.r*r + c2.r*(1.0 - r); + color.g = c1.g*r + c2.g*(1.0 - r); + color.b = c1.b*r + c2.b*(1.0 - r); + + r = sqrt(rand() * (1.0 / (RAND_MAX + 1.0))); + geometry_msgs::Point target; + target.x = point_on_p1_p2.x*r + p0.x*(1.0 - r); + target.y = point_on_p1_p2.y*r + p0.y*(1.0 - r); + target.z = point_on_p1_p2.z*r + p0.z*(1.0 - r); + color.r = color.r*r + c0.r*(1.0 - r); + color.g = color.g*r + c0.g*(1.0 - r); + color.b = color.b*r + c0.b*(1.0 - r); + p.x = target.x; + p.y = target.y; + p.z = target.z; + p.r = color.r * 256; + p.g = color.g * 256; + p.b = color.b * 256; + + output_cloud.points.push_back(p); + } + output_cloud.width = sample_nums; + output_cloud.height = 1; + } + + inline bool isValidPoint(const pcl::PointXYZ& p) + { + return !std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z); + } + + template + inline bool isValidPoint(const PointT& p) + { + return !std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z); + } + +} +// extend pcl_conversions package's toPCL and fromPCL functions +namespace pcl_conversions +{ + std::vector + convertToPCLPointIndices(const std::vector& cluster_indices); + + std::vector + convertToPCLModelCoefficients( + const std::vector& coefficients); + + std::vector + convertToROSPointIndices( + const std::vector cluster_indices, + const std_msgs::Header& header); + + std::vector + convertToROSPointIndices( + const std::vector cluster_indices, + const std_msgs::Header& header); + + std::vector + convertToROSModelCoefficients( + const std::vector& coefficients, + const std_msgs::Header& header); + +} + +namespace tf +{ + // for eigen float + void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Affine3f& eigen); + void poseEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Pose& msg); + void transformMsgToEigen(const geometry_msgs::Transform& msg, Eigen::Affine3f& eigen); + void transformEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Transform& msg); + void transformTFToEigen(const tf::Transform& t, Eigen::Affine3f& eigen); + void transformEigenToTF(Eigen::Affine3f& eigen , tf::Transform& t); + void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3f& e); + void vectorEigenToTF(const Eigen::Vector3f& e, tf::Vector3& t); +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl_ros_util.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl_ros_util.h new file mode 100644 index 00000000000..b6d7e7aa93f --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl_ros_util.h @@ -0,0 +1,139 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + + +/** + * This file defines several utilities for pcl <--> ros bridging. + */ + +#ifndef JSK_RECOGNITION_UTILS_PCL_ROS_UTIL_H_ +#define JSK_RECOGNITION_UTILS_PCL_ROS_UTIL_H_ + +#include + +#include +#include + +#include +#include +#include +#include + +#include "jsk_recognition_utils/pcl_conversion_util.h" + +namespace jsk_recognition_utils +{ + /** + * @brief + * Convert pcl::PointIndices to pcl_msgs::PointIndices + * and publish it with overriding header. + */ + void publishPointIndices(ros::Publisher& pub, + const pcl::PointIndices& indices, + const std_msgs::Header& header); + + /** + * @brief + * Return true if a and b are the same frame_id + */ + bool isSameFrameId(const std::string& a, const std::string& b); + + /** + * @brief + * Return true if a and b have the same frame_id + */ + bool isSameFrameId(const std_msgs::Header& a, const std_msgs::Header& b); + + /** + * @brief + * Return true if a and b have the same frame_id + */ + template + bool isSameFrameId(const T1& a, const T2& b) + { + return isSameFrameId(a.header, b.header); + } + + /** + * @brief + * check if sensor_msgs/PointCloud2 message has the specified field. + */ + bool hasField(const std::string& field_name, const sensor_msgs::PointCloud2& msg); + + /** + * @brief + * Crop point cloud with jsk_recognition_msgs/BoundingBox. + */ + template + void + cropPointCloud(const typename pcl::PointCloud::Ptr& cloud, + const jsk_recognition_msgs::BoundingBox& bbox_msg, + std::vector* indices, + bool extract_removed_indices=false) + { + if (cloud->header.frame_id != bbox_msg.header.frame_id) + { + fprintf(stderr, "Frame id of input cloud and bounding box must be same. Cloud: %s, BoundingBox: %s.", + cloud->header.frame_id.c_str(), bbox_msg.header.frame_id.c_str()); + return; + } + pcl::CropBox crop_box(/*extract_removed_indices=*/extract_removed_indices); + + crop_box.setInputCloud(cloud); + + Eigen::Affine3f box_pose; + tf::poseMsgToEigen(bbox_msg.pose, box_pose); + crop_box.setTranslation(box_pose.translation()); + float roll, pitch, yaw; + pcl::getEulerAngles(box_pose, roll, pitch, yaw); + crop_box.setRotation(Eigen::Vector3f(roll, pitch, yaw)); + + Eigen::Vector4f max_point(bbox_msg.dimensions.x / 2, + bbox_msg.dimensions.y / 2, + bbox_msg.dimensions.z / 2, + 0); + Eigen::Vector4f min_point(-bbox_msg.dimensions.x / 2, + -bbox_msg.dimensions.y / 2, + -bbox_msg.dimensions.z / 2, + 0); + crop_box.setMax(max_point); + crop_box.setMin(min_point); + + crop_box.filter(*indices); + } + +} // namespace jsk_recognition_utils + +#endif // JSK_RECOGNITION_UTILS_PCL_ROS_UTIL_H_ diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl_util.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl_util.h new file mode 100644 index 00000000000..502fb43181d --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/pcl_util.h @@ -0,0 +1,227 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_PCL_UTIL_H_ +#define JSK_RECOGNITION_UTILS_PCL_UTIL_H_ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +namespace jsk_recognition_utils +{ + Eigen::Affine3f affineFromYAMLNode(const YAML::Node& pose); + + std::vector addIndices(const std::vector& a, + const std::vector& b); + pcl::PointIndices::Ptr addIndices(const pcl::PointIndices& a, + const pcl::PointIndices& b); + // substract indices like: a - b + std::vector subIndices(const std::vector& a, + const std::vector& b); + pcl::PointIndices::Ptr subIndices(const pcl::PointIndices& a, + const pcl::PointIndices& b); + + template + std::vector ::Ptr> + convertToPointCloudArray(const typename pcl::PointCloud::Ptr& cloud, + const std::vector& indices) + { + pcl::ExtractIndices extract; + extract.setInputCloud(cloud); + std::vector ::Ptr> cloud_array; + for (size_t i = 0; i < indices.size(); i++) { + typename pcl::PointCloud ::Ptr + segment (new pcl::PointCloud); + extract.setIndices(indices[i]); + extract.filter(*segment); + cloud_array.push_back(segment); + } + return cloud_array; + } + + + template + pcl::PointCloud::Ptr convertToXYZCloud(const pcl::PointCloud& cloud) + { + pcl::PointCloud::Ptr output(new pcl::PointCloud); + output->points.resize(cloud.points.size()); + for (size_t i = 0; i < cloud.points.size(); i++) { + pcl::PointXYZ p; + p.x = cloud.points[i].x; + p.y = cloud.points[i].y; + p.z = cloud.points[i].z; + output->points[i] = p; + } + return output; + } + + template + void appendVector(std::vector& a, const std::vector& b) + { + for (size_t i = 0; i < b.size(); i++) { + a.push_back(b[i]); + } + } + + // select color out of 20 colors + std_msgs::ColorRGBA colorCategory20(int i); + + class Counter + { + public: + typedef boost::accumulators::accumulator_set< + double, + boost::accumulators::stats > Accumulator; + virtual void add(double v); + virtual double mean(); + virtual double min(); + virtual double max(); + virtual int count(); + virtual double variance(); + protected: + Accumulator acc_; + }; + + //////////////////////////////////////////////////////// + // Graph utility function + //////////////////////////////////////////////////////// + + //////////////////////////////////////////////////////// + // buildGroupFromGraphMap (recursive function) + // This function retrieves a directional graph, and build + // a set of the indices where can be arrived from a specified + // vertex. + // + // graph_map := A map representeing edges of the graph. + // The graph is bidirectional graph. + // the key means "from vertex" and the value + // means the "to indices" from the key vertex. + // from_index := The index to pay attension + // to_indices := The "to indices" from from_index + // output_set := result + //////////////////////////////////////////////////////// + typedef std::map > IntegerGraphMap; + + void buildGroupFromGraphMap(IntegerGraphMap graph_map, + const int from_index, + std::vector& to_indices, + std::set& output_set); + void _buildGroupFromGraphMap(IntegerGraphMap graph_map, + const int from_index, + std::vector& to_indices, + std::set& output_set); + + //////////////////////////////////////////////////////// + // buildAllGraphSetFromGraphMap + // get all the list of set represented in graph_map + //////////////////////////////////////////////////////// + void buildAllGroupsSetFromGraphMap(IntegerGraphMap graph_map, + std::vector >& output_sets); + + //////////////////////////////////////////////////////// + // addSet(A, B) + // add two set like A = A + B + //////////////////////////////////////////////////////// + template + void addSet(std::set& output, + const std::set& new_set) + { + typedef typename std::set Set; + typedef typename Set::iterator Iterator; + for (Iterator it = new_set.begin(); + it != new_set.end(); + ++it) { + output.insert(*it); + } + } + + //////////////////////////////////////////////////////// + // SeriesedBoolean + // store boolean value to limited buffer + // and return true if all the values are true. + //////////////////////////////////////////////////////// + class SeriesedBoolean + { + public: + typedef boost::shared_ptr Ptr; + SeriesedBoolean(const int buf_len); + virtual ~SeriesedBoolean(); + virtual void addValue(bool val); + virtual bool getValue(); + virtual bool isAllTrueFilled(); + virtual void clear(); + protected: + private: + boost::circular_buffer buf_; + const int buf_len_; + }; + + //////////////////////////////////////////////////////// + // TimeredDiagnosticUpdater + // useful wrapper of DiagnosticUpdater. + //////////////////////////////////////////////////////// + typedef jsk_topic_tools::TimeredDiagnosticUpdater TimeredDiagnosticUpdater; + + extern boost::mutex global_chull_mutex; + +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/random_util.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/random_util.h new file mode 100644 index 00000000000..6aaed0808c6 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/random_util.h @@ -0,0 +1,57 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + + +#ifndef JSK_RECOGNITION_UTILS_RANDOM_UTIL_H_ +#define JSK_RECOGNITION_UTILS_RANDOM_UTIL_H_ +#include + +namespace jsk_recognition_utils +{ + /** + * @brief + * Return a random value according to gaussian distribution. + * If variance is zero, it just returns mean. + */ + double randomGaussian(double mean, double var, boost::mt19937& gen); + + /** + * @brief + * Return a random value according to uniform distribution. + */ + double randomUniform(double min, double max, boost::mt19937& gen); +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/rgb_colors.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/rgb_colors.h new file mode 100644 index 00000000000..f8878f8d26e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/rgb_colors.h @@ -0,0 +1,207 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_RGB_COLORS_H_ +#define JSK_RECOGNITION_UTILS_RGB_COLORS_H_ + +#include "jsk_recognition_utils/rgb_colors.h" +#include + + +namespace jsk_recognition_utils +{ + + /** + * @brief + * 146 rgb colors + */ + enum Colors { + ALICEBLUE, + ANTIQUEWHITE, + AQUA, + AQUAMARINE, + AZURE, + BEIGE, + BISQUE, + BLACK, + BLANCHEDALMOND, + BLUE, + BLUEVIOLET, + BROWN, + BURLYWOOD, + CADETBLUE, + CHARTREUSE, + CHOCOLATE, + CORAL, + CORNFLOWERBLUE, + CORNSILK, + CRIMSON, + CYAN, + DARKBLUE, + DARKCYAN, + DARKGOLDENROD, + DARKGRAY, + DARKGREEN, + DARKGREY, + DARKKHAKI, + DARKMAGENTA, + DARKOLIVEGREEN, + DARKORANGE, + DARKORCHID, + DARKRED, + DARKSALMON, + DARKSEAGREEN, + DARKSLATEBLUE, + DARKSLATEGRAY, + DARKSLATEGREY, + DARKTURQUOISE, + DARKVIOLET, + DEEPPINK, + DEEPSKYBLUE, + DIMGRAY, + DIMGREY, + DODGERBLUE, + FIREBRICK, + FLORALWHITE, + FORESTGREEN, + FUCHSIA, + GAINSBORO, + GHOSTWHITE, + GOLD, + GOLDENROD, + GRAY, + GREEN, + GREENYELLOW, + GREY, + HONEYDEW, + HOTPINK, + INDIANRED, + INDIGO, + IVORY, + KHAKI, + LAVENDER, + LAVENDERBLUSH, + LAWNGREEN, + LEMONCHIFFON, + LIGHTBLUE, + LIGHTCORAL, + LIGHTCYAN, + LIGHTGOLDENRODYELLOW, + LIGHTGRAY, + LIGHTGREEN, + LIGHTGREY, + LIGHTPINK, + LIGHTSALMON, + LIGHTSEAGREEN, + LIGHTSKYBLUE, + LIGHTSLATEGRAY, + LIGHTSLATEGREY, + LIGHTSTEELBLUE, + LIGHTYELLOW, + LIME, + LIMEGREEN, + LINEN, + MAGENTA, + MAROON, + MEDIUMAQUAMARINE, + MEDIUMBLUE, + MEDIUMORCHID, + MEDIUMPURPLE, + MEDIUMSEAGREEN, + MEDIUMSLATEBLUE, + MEDIUMSPRINGGREEN, + MEDIUMTURQUOISE, + MEDIUMVIOLETRED, + MIDNIGHTBLUE, + MINTCREAM, + MISTYROSE, + MOCCASIN, + NAVAJOWHITE, + NAVY, + OLDLACE, + OLIVE, + OLIVEDRAB, + ORANGE, + ORANGERED, + ORCHID, + PALEGOLDENROD, + PALEGREEN, + PALEVIOLETRED, + PAPAYAWHIP, + PEACHPUFF, + PERU, + PINK, + PLUM, + POWDERBLUE, + PURPLE, + RED, + ROSYBROWN, + ROYALBLUE, + SADDLEBROWN, + SALMON, + SANDYBROWN, + SEAGREEN, + SEASHELL, + SIENNA, + SILVER, + SKYBLUE, + SLATEBLUE, + SLATEGRAY, + SLATEGREY, + SNOW, + SPRINGGREEN, + STEELBLUE, + TAN, + TEAL, + THISTLE, + TOMATO, + TURQUOISE, + VIOLET, + WHEAT, + WHITE, + WHITESMOKE, + YELLOW, + YELLOWGREEN, + }; + + /** + * @brief + * get rgb color with enum. + */ + cv::Vec3d getRGBColor(const int color); + +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model/camera_depth_sensor.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model/camera_depth_sensor.h new file mode 100644 index 00000000000..29d407638ee --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model/camera_depth_sensor.h @@ -0,0 +1,133 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_CAMERA_DEPTH_SENSOR_H_ +#define JSK_RECOGNITION_UTILS_CAMERA_DEPTH_SENSOR_H_ + +#include "jsk_recognition_utils/sensor_model/pointcloud_sensor_model.h" +#include +#include + +namespace jsk_recognition_utils +{ + class CameraDepthSensor: public PointCloudSensorModel + { + public: + typedef boost::shared_ptr Ptr; + CameraDepthSensor() {} + + virtual void setCameraInfo(const sensor_msgs::CameraInfo& info) + { + camera_info_ = info; + model_.fromCameraInfo(info); + } + + /** + * @brief + * get instance of image_geometry::PinholeCameraModel. + */ + virtual image_geometry::PinholeCameraModel getPinholeCameraModel() const + { + return model_; + } + + /** + * @brief + * return true if point p is inside of field-of-view. + */ + virtual bool isInside(const cv::Point& p) const + { + return (p.x >= 0 && p.x < camera_info_.width && + p.y >= 0 && p.y < camera_info_.height); + } + + /** + * @brief + * Return the expected number of points according to distance and area. + * it is calculated according to: + * f^2\frac{s}{r^2} + **/ + virtual double expectedPointCloudNum(double distance, double area) const + { + double f = model_.fx(); + return f*f / (distance*distance) * area; + } + + /** + * @brief + * return an image from internal camera parameter. + */ + virtual cv::Mat image(const int type) const + { + return cv::Mat::zeros(camera_info_.height, camera_info_.width, type); + } + + /** + * @brief + * width of camera in pixels. + */ + virtual int width() const + { + return camera_info_.width; + } + + /** + * @brief + * height of camera in pixels. + */ + virtual int height() const + { + return camera_info_.height; + } + + /** + * @brief + * force point to be inside of field of view. + */ + virtual cv::Point limit2DPoint(const cv::Point& p) const + { + return cv::Point(std::min(std::max(p.x, 0), width()), + std::min(std::max(p.y, 0), height())); + } + + protected: + image_geometry::PinholeCameraModel model_; + sensor_msgs::CameraInfo camera_info_; + private: + + }; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model/pointcloud_sensor_model.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model/pointcloud_sensor_model.h new file mode 100644 index 00000000000..c821739b240 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model/pointcloud_sensor_model.h @@ -0,0 +1,62 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_POINTCLOUD_SENSOR_MODEL_H_ +#define JSK_RECOGNITION_UTILS_POINTCLOUD_SENSOR_MODEL_H_ + +namespace jsk_recognition_utils +{ + /** + * @brief + * Super class for sensor model. + * It provides pure virtual method for common interfaces. + */ + class PointCloudSensorModel + { + public: + typedef boost::shared_ptr Ptr; + + /** + * @brief + * Return the expected number of points according to distance and area. + */ + virtual double expectedPointCloudNum(double distance, double area) const = 0; + protected: + private: + + }; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model/spindle_laser_sensor.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model/spindle_laser_sensor.h new file mode 100644 index 00000000000..3b6f9706567 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model/spindle_laser_sensor.h @@ -0,0 +1,85 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_SPINDLE_LASER_SENSOR_H_ +#define JSK_RECOGNITION_UTILS_SPINDLE_LASER_SENSOR_H_ + +#include "jsk_recognition_utils/sensor_model/pointcloud_sensor_model.h" + +namespace jsk_recognition_utils +{ + class SpindleLaserSensor: public PointCloudSensorModel + { + public: + typedef boost::shared_ptr Ptr; + + SpindleLaserSensor(const double min_angle, const double max_angle, + const double laser_freq, + const size_t point_sample): + min_angle_(min_angle), max_angle_(max_angle), + laser_freq_(laser_freq), + point_sample_(point_sample) { } + + virtual void setSpindleVelocity(const double velocity) + { + spindle_velocity_ = spindle_velocity; + } + + /** + * @brief + * Return the expected number of points according to distance and area. + * it is calculated according to: + * \frac{N}{2 \pi \Delta \phi}\frac{1}{r^2}s + * \Delta \phi = \frac{2 \pi}{\omega} + */ + virtual double expectedPointCloudNum(double distance, double area) const + { + assert(spindle_velocity_ != 0.0); + double dphi = 2.0 * M_PI / spindle_velocity_; + return point_sample_ * laser_freq_ / (2.0 * dphi) / (distance * distance) * area; + } + + protected: + + double spindle_velocity_; + double min_angle_; + double max_angle_; + size_t point_sample_; + private: + + }; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model_utils.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model_utils.h new file mode 100644 index 00000000000..720e35acbb8 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/sensor_model_utils.h @@ -0,0 +1,69 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_SENSOR_MODEL_UTILS_H_ +#define JSK_RECOGNITION_UTILS_SENSOR_MODEL_UTILS_H_ + +#include +#include "jsk_recognition_utils/geo_util.h" + +namespace jsk_recognition_utils +{ + + /** + * @brief + * Project 3d point represented in Eigen::Vector3f to 2d point + * using model. + */ + inline cv::Point + project3DPointToPixel(const image_geometry::PinholeCameraModel& model, + const Eigen::Vector3f& p) + { + return model.project3dToPixel(cv::Point3d(p[0], p[1], p[2])); + } + + /** + * @brief + * Project array of 3d point represented in Eigen::Vector3f to 2d point + * using model. + */ + std::vector + project3DPointstoPixel(const image_geometry::PinholeCameraModel& model, + const Vertices& vertices); + +} + + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/tf_listener_singleton.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/tf_listener_singleton.h new file mode 100644 index 00000000000..30248be9aa3 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/tf_listener_singleton.h @@ -0,0 +1,66 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + + +#ifndef JSK_RECOGNITION_UTILS_TF_LISTENER_SINGLETON_H_ +#define JSK_RECOGNITION_UTILS_TF_LISTENER_SINGLETON_H_ + +#include + +namespace jsk_recognition_utils +{ + class TfListenerSingleton + { + public: + static tf::TransformListener* getInstance(); + static void destroy(); + protected: + static tf::TransformListener* instance_; + static boost::mutex mutex_; + private: + TfListenerSingleton(TfListenerSingleton const&){}; + TfListenerSingleton& operator=(TfListenerSingleton const&){}; + }; + + // tf Utility + tf::StampedTransform lookupTransformWithDuration( + tf::TransformListener* listener, + const std::string& to_frame, + const std::string& from_frame, + const ros::Time& stamp, + ros::Duration duration); +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/time_util.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/time_util.h new file mode 100644 index 00000000000..32980a92a85 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/time_util.h @@ -0,0 +1,85 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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 +#include +#include + +namespace jsk_recognition_utils +{ + class WallDurationTimer; + + class ScopedWallDurationReporter + { + public: + typedef boost::shared_ptr Ptr; + ScopedWallDurationReporter(WallDurationTimer* parent); + ScopedWallDurationReporter(WallDurationTimer* parent, + ros::Publisher& pub_latest, + ros::Publisher& pub_average); + virtual ~ScopedWallDurationReporter(); + virtual void setIsPublish(bool); + virtual void setIsEnabled(bool); + protected: + WallDurationTimer* parent_; + ros::WallTime start_time_; + ros::Publisher pub_latest_, pub_average_; + bool is_publish_; + bool is_enabled_; + private: + + }; + + class WallDurationTimer + { + public: + typedef boost::shared_ptr Ptr; + WallDurationTimer(const int max_num); + virtual void report(ros::WallDuration& duration); + virtual ScopedWallDurationReporter reporter(); + virtual ScopedWallDurationReporter reporter( + ros::Publisher& pub_latest, + ros::Publisher& pub_average); + virtual void clearBuffer(); + virtual double meanSec(); + virtual double latestSec(); + virtual size_t sampleNum(); + protected: + const int max_num_; + boost::circular_buffer buffer_; + private: + }; + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/types.h b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/types.h new file mode 100644 index 00000000000..10d5b54fc2b --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/include/jsk_recognition_utils/types.h @@ -0,0 +1,53 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#ifndef JSK_RECOGNITION_UTILS_TYPES_H_ +#define JSK_RECOGNITION_UTILS_TYPES_H_ + +#include +#include +#include + +namespace jsk_recognition_utils +{ + typedef Eigen::Vector3f Point; + typedef Eigen::Vector3f Vertex; + typedef std::vector > Vertices; + typedef boost::tuple PointPair; + typedef boost::tuple PointIndexPair; +} + +#endif diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/add_bounding_box_array.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/add_bounding_box_array.py new file mode 100755 index 00000000000..8a0f92f6614 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/add_bounding_box_array.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python + +from jsk_recognition_msgs.msg import BoundingBoxArray +from jsk_topic_tools import ConnectionBasedTransport +import message_filters +import rospy + + +class AddBoundingBoxArray(ConnectionBasedTransport): + def __init__(self): + super(AddBoundingBoxArray, self).__init__() + self._pub = self.advertise('~output', BoundingBoxArray, queue_size=1) + + def subscribe(self): + topics = rospy.get_param('~topics') + self.subs = [] + for tp in topics: + sub = message_filters.Subscriber(tp, BoundingBoxArray) + self.subs.append(sub) + use_async = rospy.get_param('~approximate_sync', False) + queue_size = rospy.get_param('~queue_size', 100) + if use_async: + slop = rospy.get_param('~slop', 0.1) + sync = message_filters.ApproximateTimeSynchronizer( + self.subs, queue_size, slop) + else: + sync = message_filters.TimeSynchronizer(self.subs, queue_size) + sync.registerCallback(self._decompose) + + def unsubscribe(self): + for sub in self.subs: + sub.unregister() + + def _decompose(self, *bboxes_msgs): + out_msg = BoundingBoxArray() + out_msg.header = bboxes_msgs[0].header + for bboxes_msg in bboxes_msgs: + out_msg.boxes.extend(bboxes_msg.boxes) + self._pub.publish(out_msg) + + +if __name__ == '__main__': + rospy.init_node('add_bounding_box_array') + app = AddBoundingBoxArray() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/add_cluster_indices.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/add_cluster_indices.py new file mode 100755 index 00000000000..7331328fc93 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/add_cluster_indices.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from jsk_recognition_msgs.msg import ClusterPointIndices +from jsk_topic_tools import ConnectionBasedTransport +import message_filters +import rospy + + +class AddClusterIndices(ConnectionBasedTransport): + def __init__(self): + super(AddClusterIndices, self).__init__() + self._pub = self.advertise('~output', ClusterPointIndices, + queue_size=1) + + def subscribe(self): + topics = rospy.get_param('~topics') + self.subs = [] + for tp in topics: + sub = message_filters.Subscriber(tp, ClusterPointIndices) + self.subs.append(sub) + use_async = rospy.get_param('~approximate_sync', False) + queue_size = rospy.get_param('~queue_size', 100) + if use_async: + slop = rospy.get_param('~slop', 0.1) + sync = message_filters.ApproximateTimeSynchronizer( + self.subs, queue_size, slop) + else: + sync = message_filters.TimeSynchronizer(self.subs, queue_size) + sync.registerCallback(self._decompose) + + def unsubscribe(self): + for sub in self.subs: + sub.unregister() + + def _decompose(self, *cpi_msgs): + out_msg = ClusterPointIndices() + out_msg.header = cpi_msgs[0].header + for cpi_msg in cpi_msgs: + out_msg.cluster_indices.extend(cpi_msg.cluster_indices) + self._pub.publish(out_msg) + + +if __name__ == '__main__': + rospy.init_node('add_cluster_indices') + app = AddClusterIndices() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/bounding_box_array_publisher.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/bounding_box_array_publisher.py new file mode 100755 index 00000000000..dc6d631a2c6 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/bounding_box_array_publisher.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python + +import sys + +from geometry_msgs.msg import Point +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Vector3 +from jsk_recognition_msgs.msg import BoundingBox +from jsk_recognition_msgs.msg import BoundingBoxArray +import rospy +from tf.transformations import quaternion_from_euler + + +class BoundingBoxArrayPublisher(object): + + def __init__(self): + self.seq = 0 + + self.frame_id = rospy.get_param('~frame_id') + if (rospy.has_param('~positions') or + rospy.has_param('~rotations') or + rospy.has_param('~dimensions')): + # Deprecated bounding box pose/dimension specification + rospy.logwarn("DEPRECATION WARNING: Rosparam '~positions', " + "'~rotations' and '~dimensions' are being " + "deprecated. Please use '~boxes' instead.") + positions = rospy.get_param('~positions') + rotations = rospy.get_param('~rotations') + dimensions = rospy.get_param('~dimensions') + if len(rotations) != len(positions): + rospy.logfatal('Number of ~rotations is expected as {}, but {}' + .format(len(positions), len(rotations))) + sys.exit(1) + if len(dimensions) != len(positions): + rospy.logfatal('Number of ~dimensions is expected as {}, but {}' + .format(len(positions), len(dimensions))) + sys.exit(1) + self.boxes = [] + for pos, rot, dim in zip(positions, rotations, dimensions): + self.boxes.append({ + 'position': pos, + 'rotation': rot, + 'dimension': dim, + }) + else: + self.boxes = rospy.get_param('~boxes') + self.pub = rospy.Publisher('~output', BoundingBoxArray, queue_size=1) + + rate = rospy.get_param('~rate', 1) + self.timer = rospy.Timer(rospy.Duration(1. / rate), self.publish) + + def publish(self, event): + bbox_array_msg = BoundingBoxArray() + bbox_array_msg.header.seq = self.seq + bbox_array_msg.header.frame_id = self.frame_id + bbox_array_msg.header.stamp = event.current_real + for box in self.boxes: + pos = box['position'] + rot = box.get('rotation', [0, 0, 0]) + qua = quaternion_from_euler(*rot) + dim = box['dimension'] + + bbox_msg = BoundingBox() + bbox_msg.header.seq = self.seq + bbox_msg.header.frame_id = self.frame_id + bbox_msg.header.stamp = event.current_real + bbox_msg.pose.position = Point(*pos) + bbox_msg.pose.orientation = Quaternion(*qua) + bbox_msg.dimensions = Vector3(*dim) + + bbox_array_msg.boxes.append(bbox_msg) + + self.pub.publish(bbox_array_msg) + + +if __name__ == '__main__': + rospy.init_node('bounding_box_array_publisher') + BoundingBoxArrayPublisher() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/image_16uc1_to_32fc1.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/image_16uc1_to_32fc1.py new file mode 100755 index 00000000000..e34c5f47198 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/image_16uc1_to_32fc1.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import numpy as np + +import cv_bridge +from jsk_topic_tools import ConnectionBasedTransport +import rospy +from sensor_msgs.msg import Image + + +class Image16UC1To32FC1(ConnectionBasedTransport): + + def __init__(self): + super(self.__class__, self).__init__() + self.pub = self.advertise('~output', Image, queue_size=1) + + def subscribe(self): + self.sub = rospy.Subscriber('~input', Image, self.transport) + + def unsubscribe(self): + self.sub.unregister() + + def transport(self, msg): + if msg.height * msg.width == 0: + return + bridge = cv_bridge.CvBridge() + img_16uc1 = bridge.imgmsg_to_cv2(msg, desired_encoding='16UC1') + img_32fc1 = (img_16uc1.astype(np.float64) / (2**16)).astype(np.float32) + out_msg = bridge.cv2_to_imgmsg(img_32fc1, '32FC1') + out_msg.header = msg.header + self.pub.publish(out_msg) + + +if __name__ == '__main__': + rospy.init_node('image_16uc1_to_32fc1') + Image16UC1To32FC1() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/polygon_array_publisher.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/polygon_array_publisher.py new file mode 100755 index 00000000000..776b425f473 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/polygon_array_publisher.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy +from geometry_msgs.msg import PolygonStamped, Point32 +from jsk_recognition_msgs.msg import PolygonArray + + +class PolygonArrayPublisher(object): + def __init__(self): + publish_rate = rospy.get_param("~publish_rate", 1.0) + + frame_id = rospy.get_param("~frame_id") + param = rospy.get_param("~polygons") + self.msg = self.parse_params(param, frame_id) + + self.pub_polygons = rospy.Publisher("~output", PolygonArray, queue_size=1) + + self.timer = rospy.Timer(rospy.Duration(1. / publish_rate), self.publish) + + def parse_params(self, param, frame_id): + # validate params + assert isinstance(param, list), "polygons must be list" + + has_label = any("label" in p for p in param) + has_likelihood = any("likelihood" in p for p in param) + + for polygon in param: + assert "points" in polygon and isinstance(polygon["points"], list),\ + "each polygon must have at least 1 'points'" + for point in polygon["points"]: + assert len(point) == 3,\ + "element of 'points' must be list of 3 numbers" + if has_label: + assert "label" in polygon, "missing 'label'" + if has_likelihood: + assert "likelihood" in polygon, "missing 'likelihood'" + + # parse params + msg = PolygonArray() + msg.header.frame_id = frame_id + for polygon in param: + ps = PolygonStamped() + ps.header.frame_id = frame_id + ps.polygon.points = [Point32(*v) for v in polygon["points"]] + msg.polygons.append(ps) + if has_label: + msg.labels.append(polygon["label"]) + if has_likelihood: + msg.likelihood.append(polygon["likelihood"]) + + return msg + + def publish(self, event): + # update timestamp + self.msg.header.stamp = event.current_real + for p in self.msg.polygons: + p.header.stamp = event.current_real + + self.pub_polygons.publish(self.msg) + +if __name__ == '__main__': + rospy.init_node("polygon_array_publisher") + p = PolygonArrayPublisher() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/polygon_array_to_polygon.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/polygon_array_to_polygon.py new file mode 100755 index 00000000000..728e337939e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/polygon_array_to_polygon.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import dynamic_reconfigure.server +from geometry_msgs.msg import PolygonStamped +from jsk_recognition_msgs.msg import PolygonArray +from jsk_topic_tools import ConnectionBasedTransport +from jsk_topic_tools.log_utils import logerr_throttle +from jsk_recognition_utils.cfg import PolygonArrayToPolygonConfig +import rospy + + +class PolygonArrayToPolygon(ConnectionBasedTransport): + + def __init__(self): + super(self.__class__, self).__init__() + self.pub = self.advertise('~output', PolygonStamped, queue_size=1) + dynamic_reconfigure.server.Server(PolygonArrayToPolygonConfig, + self._config_callback) + + def subscribe(self): + self.sub = rospy.Subscriber('~input', PolygonArray, self._convert) + + def unsubscribe(self): + self.sub.unregister() + + def _config_callback(self, config, level): + self.index = config.index + return config + + def _convert(self, msg): + polygon_msg = PolygonStamped() + polygon_msg.header = msg.header + if self.index < 0: + return + elif self.index < len(msg.polygons): + polygon_msg = msg.polygons[self.index] + self.pub.publish(polygon_msg) + else: + logerr_throttle(10, + 'Invalid index {} is specified ' + 'for polygon array whose size is {}' + .format(self.index, len(msg.polygons))) + + +if __name__ == '__main__': + rospy.init_node('polygon_array_to_polygon') + PolygonArrayToPolygon() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/pose_array_to_pose.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/pose_array_to_pose.py new file mode 100755 index 00000000000..ea2976d070b --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/pose_array_to_pose.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import dynamic_reconfigure.server +from geometry_msgs.msg import PoseArray +from geometry_msgs.msg import PoseStamped +from jsk_topic_tools import ConnectionBasedTransport +from jsk_topic_tools.log_utils import logerr_throttle +from jsk_recognition_utils.cfg import PoseArrayToPoseConfig +import rospy + + +class PoseArrayToPose(ConnectionBasedTransport): + + def __init__(self): + super(self.__class__, self).__init__() + self.pub = self.advertise('~output', PoseStamped, queue_size=1) + dynamic_reconfigure.server.Server(PoseArrayToPoseConfig, + self._config_callback) + + def subscribe(self): + self.sub = rospy.Subscriber('~input', PoseArray, self._convert) + + def unsubscribe(self): + self.sub.unregister() + + def _config_callback(self, config, level): + self.index = config.index + return config + + def _convert(self, msg): + pose_msg = PoseStamped() + pose_msg.header = msg.header + if self.index < 0: + return + elif self.index < len(msg.poses): + pose_msg.pose = msg.poses[self.index] + self.pub.publish(pose_msg) + else: + logerr_throttle(10, + 'Invalid index {} is specified ' + 'for pose array whose size is {}' + .format(self.index, len(msg.poses))) + + +if __name__ == '__main__': + rospy.init_node('pose_array_to_pose') + PoseArrayToPose() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/rect_array_to_polygon_array.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/rect_array_to_polygon_array.py new file mode 100755 index 00000000000..1faae707177 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/rect_array_to_polygon_array.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python + +import dynamic_reconfigure.server +from geometry_msgs.msg import PolygonStamped +from geometry_msgs.msg import Point32 +from jsk_recognition_msgs.msg import PolygonArray +from jsk_recognition_msgs.msg import RectArray +from jsk_topic_tools import ConnectionBasedTransport +from jsk_recognition_utils.cfg import PolygonArrayToPolygonConfig +import rospy + + +class RectArrayToPolygonArray(ConnectionBasedTransport): + + def __init__(self): + super(RectArrayToPolygonArray, self).__init__() + self.pub = self.advertise('~output', PolygonArray, queue_size=1) + dynamic_reconfigure.server.Server(PolygonArrayToPolygonConfig, + self._config_callback) + + def subscribe(self): + self.sub = rospy.Subscriber('~input', RectArray, self._convert) + + def unsubscribe(self): + self.sub.unregister() + + def _config_callback(self, config, level): + self.index = config.index + return config + + def _convert(self, msg): + polys_msg = PolygonArray() + polys_msg.header = msg.header + for rect in msg.rects: + poly_msg = PolygonStamped() + poly_msg.header = msg.header + pt0 = Point32(x=rect.x, y=rect.y) + pt1 = Point32(x=rect.x + rect.width, y=rect.y + rect.height) + poly_msg.polygon.points.append(pt0) + poly_msg.polygon.points.append(pt1) + polys_msg.polygons.append(poly_msg) + self.pub.publish(polys_msg) + + +if __name__ == '__main__': + rospy.init_node('rect_array_to_polygon_array') + RectArrayToPolygonArray() + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/static_virtual_camera.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/static_virtual_camera.py new file mode 100755 index 00000000000..a32482d64de --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/node_scripts/static_virtual_camera.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python + +import cv2 +try: + from scipy.misc import face + img = face()[:, :, ::-1] +except ImportError: + import numpy as np + from scipy.misc import lena + img = cv2.cvtColor(lena().astype(np.uint8), cv2.COLOR_GRAY2BGR) + +import cv_bridge +import rospy +from sensor_msgs.msg import Image +from sensor_msgs.msg import CameraInfo + + +def timer_cb(event): + imgmsg.header.stamp = rospy.Time.now() + info_msg.header.stamp = imgmsg.header.stamp + pub_img.publish(imgmsg) + pub_info.publish(info_msg) + + +if __name__ == '__main__': + rospy.init_node('static_virtual_camera') + + pub_img = rospy.Publisher('~image_color', Image, queue_size=1) + pub_info = rospy.Publisher('~camera_info', CameraInfo, queue_size=1) + + bridge = cv_bridge.CvBridge() + imgmsg = bridge.cv2_to_imgmsg(img, encoding='bgr8') + imgmsg.header.frame_id = 'static_virtual_camera' + + info_msg = CameraInfo() + info_msg.header.frame_id = imgmsg.header.frame_id + height, width = img.shape[:2] + info_msg.height = height + info_msg.width = width + + rospy.Timer(rospy.Duration(0.1), timer_cb) + rospy.spin() diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/package.xml b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/package.xml new file mode 100644 index 00000000000..da25f120c12 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/package.xml @@ -0,0 +1,51 @@ + + + jsk_recognition_utils + 1.2.6 + C++ library about sensor model, geometrical modeling and perception. + Ryohei Ueda + + http://github.com/jsk-ros-pkg/jsk_recognition + http://github.com/jsk-ros-pkg/jsk_recognition/issues + http://jsk-docs.readthedocs.io/en/latest/jsk_recognition/jsk_recognition_utils + + BSD + + catkin + cython + eigen_conversions + dynamic_reconfigure + geometry_msgs + image_geometry + jsk_recognition_msgs + jsk_topic_tools + pcl_msgs + pcl_ros + sensor_msgs + std_msgs + tf2_ros + tf + tf_conversions + visualization_msgs + yaml-cpp + message_generation + eigen_conversions + geometry_msgs + image_geometry + jsk_recognition_msgs + jsk_topic_tools + pcl_msgs + pcl_ros + python-skimage + sensor_msgs + std_msgs + tf2_ros + tf_conversions + tf + visualization_msgs + yaml-cpp + message_runtime + jsk_tools + + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/CMakeLists.txt b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/CMakeLists.txt new file mode 100644 index 00000000000..e880580c825 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/CMakeLists.txt @@ -0,0 +1,20 @@ +if(NOT DEFINED Numpy_INCLUDE_DIRS) + # Get Numpy include directories + execute_process( + COMMAND python -c "import sys, numpy; sys.stdout.write(numpy.get_include())" + OUTPUT_VARIABLE Numpy_INCLUDE_DIRS + RESULT_VARIABLE retcode) + if(NOT ${retcode} EQUAL 0) + message(FATAL_ERROR "Failed to get Numpy include dirs by numpy.get_include(). Exit code: ${retcode}") + endif() +endif() +# Compile nms.pyx +include_directories(${Numpy_INCLUDE_DIRS}) +cython_add_module(nms nms.pyx) +set_target_properties(nms PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}) +cython_add_module(color color.pyx) +set_target_properties(color PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}) +install(TARGETS nms color + ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/__init__.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/__init__.py new file mode 100644 index 00000000000..67b1c5a0f7f --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/__init__.py @@ -0,0 +1,23 @@ +from jsk_recognition_utils import chainermodels +from jsk_recognition_utils import color +from jsk_recognition_utils import conversations +from jsk_recognition_utils import feature +from jsk_recognition_utils import mask +from jsk_recognition_utils import visualize +from jsk_recognition_utils import geometry + + +bounding_box_msg_to_aabb = conversations.bounding_box_msg_to_aabb +rects_msg_to_ndarray = conversations.rects_msg_to_ndarray + +BagOfFeatures = feature.BagOfFeatures +decompose_descriptors_with_label = feature.decompose_descriptors_with_label + +bounding_rect_of_mask = mask.bounding_rect_of_mask +descent_closing = mask.descent_closing + +centerize = visualize.centerize +colorize_cluster_indices = visualize.colorize_cluster_indices +get_tile_image = visualize.get_tile_image + +get_overlap_of_aabb = geometry.get_overlap_of_aabb diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/__init__.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/__init__.py new file mode 100644 index 00000000000..ad41983df91 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/__init__.py @@ -0,0 +1,27 @@ +from jsk_recognition_utils.chainermodels import alexnet +from jsk_recognition_utils.chainermodels import alexnet_batch_normalization +from jsk_recognition_utils.chainermodels import vgg16 +from jsk_recognition_utils.chainermodels import vgg16_batch_normalization +from jsk_recognition_utils.chainermodels import vgg16_fast_rcnn +from jsk_recognition_utils.chainermodels import vgg_cnn_m_1024 +from jsk_recognition_utils.chainermodels import resnet + +# AlexNet Object Recognition Network +AlexNet = alexnet.AlexNet +AlexNetBatchNormalization = alexnet_batch_normalization.AlexNetBatchNormalization # NOQA + +# VGG16 Object Recognition Network +VGG16 = vgg16.VGG16 +VGG16BatchNormalization = vgg16_batch_normalization.VGG16BatchNormalization + +# FastRCNN +VGG16FastRCNN = vgg16_fast_rcnn.VGG16FastRCNN +VGG_CNN_M_1024 = vgg_cnn_m_1024.VGG_CNN_M_1024 + +# ResNet +ResNet50 = resnet.ResNet50 +ResNet50Feature = resnet.ResNet50Feature +ResNet101 = resnet.ResNet101 +ResNet101Feature = resnet.ResNet101Feature +ResNet152 = resnet.ResNet152 +ResNet152Feature = resnet.ResNet152Feature diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/alexnet.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/alexnet.py new file mode 100644 index 00000000000..48d7f8c42ca --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/alexnet.py @@ -0,0 +1,38 @@ +import chainer +import chainer.functions as F +import chainer.links as L + + +class AlexNet(chainer.Chain): + + def __init__(self, n_class=1000): + super(AlexNet, self).__init__( + conv1=L.Convolution2D(3, 96, 11, stride=4), + conv2=L.Convolution2D(96, 256, 5, pad=2), + conv3=L.Convolution2D(256, 384, 3, pad=1), + conv4=L.Convolution2D(384, 384, 3, pad=1), + conv5=L.Convolution2D(384, 256, 3, pad=1), + fc6=L.Linear(9216, 4096), + fc7=L.Linear(4096, 4096), + fc8=L.Linear(4096, n_class)) + + def __call__(self, x, t=None): + h = F.local_response_normalization(self.conv1(x)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = F.local_response_normalization(self.conv2(h)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = F.relu(self.conv3(h)) + h = F.relu(self.conv4(h)) + h = F.max_pooling_2d(F.relu(self.conv5(h)), 3, stride=2) + h = F.dropout(F.relu(self.fc6(h))) + h = F.dropout(F.relu(self.fc7(h))) + h = self.fc8(h) + + self.pred = F.softmax(h) + if t is None: + assert not chainer.config.train + return + + self.loss = F.softmax_cross_entropy(h, t) + self.accuracy = F.accuracy(h, t) + return self.loss diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/alexnet_batch_normalization.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/alexnet_batch_normalization.py new file mode 100644 index 00000000000..7d78f4a21fa --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/alexnet_batch_normalization.py @@ -0,0 +1,39 @@ +import chainer +import chainer.functions as F +import chainer.links as L + + +class AlexNetBatchNormalization(chainer.Chain): + def __init__(self, n_class=1000): + super(AlexNetBatchNormalization, self).__init__( + conv1=L.Convolution2D(3, 96, 11, stride=4), + bn1=L.BatchNormalization(96), + conv2=L.Convolution2D(96, 256, 5, pad=2), + bn2=L.BatchNormalization(256), + conv3=L.Convolution2D(256, 384, 3, pad=1), + conv4=L.Convolution2D(384, 384, 3, pad=1), + conv5=L.Convolution2D(384, 256, 3, pad=1), + fc6=L.Linear(9216, 4096), + fc7=L.Linear(4096, 4096), + fc8=L.Linear(4096, n_class)) + + def __call__(self, x, t=None): + h = self.bn1(self.conv1(x)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = self.bn2(self.conv2(h)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = F.relu(self.conv3(h)) + h = F.relu(self.conv4(h)) + h = F.max_pooling_2d(F.relu(self.conv5(h)), 3, stride=2) + h = F.dropout(F.relu(self.fc6(h))) + h = F.dropout(F.relu(self.fc7(h))) + h = self.fc8(h) + + self.pred = F.softrmax(h) + if t is None: + assert not chainer.config.train + return + + self.loss = F.softmax_cross_entropy(h, t) + self.accuracy = F.accuracy(h, t) + return self.loss diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/__init__.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/__init__.py new file mode 100644 index 00000000000..2db1ea575a2 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/__init__.py @@ -0,0 +1,6 @@ +from .resnet50 import ResNet50 # NOQA +from .resnet50 import ResNet50Feature # NOQA +from .resnet101 import ResNet101 # NOQA +from .resnet101 import ResNet101Feature # NOQA +from .resnet152 import ResNet152 # NOQA +from .resnet152 import ResNet152Feature # NOQA diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/resnet101.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/resnet101.py new file mode 100644 index 00000000000..dcd5746aa91 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/resnet101.py @@ -0,0 +1,133 @@ +# Modified work: Copyright (c) 2017 Kentaro Wada. +# Original work: https://github.com/yasunorikudo/chainer-ResNet + +import chainer +import chainer.functions as F +import chainer.links as L + + +class BottleNeckA(chainer.Chain): + + def __init__(self, in_size, ch, out_size, stride=2): + initialW = chainer.initializers.HeNormal() + super(BottleNeckA, self).__init__( + conv1=L.Convolution2D( + in_size, ch, 1, stride, 0, nobias=True, initialW=initialW), + bn1=L.BatchNormalization(ch), + conv2=L.Convolution2D( + ch, ch, 3, 1, 1, nobias=True, initialW=initialW), + bn2=L.BatchNormalization(ch), + conv3=L.Convolution2D( + ch, out_size, 1, 1, 0, nobias=True, initialW=initialW), + bn3=L.BatchNormalization(out_size), + + conv4=L.Convolution2D( + in_size, out_size, 1, stride, 0, + nobias=True, initialW=initialW), + bn4=L.BatchNormalization(out_size), + ) + + def __call__(self, x): + h1 = F.relu(self.bn1(self.conv1(x))) + h1 = F.relu(self.bn2(self.conv2(h1))) + h1 = self.bn3(self.conv3(h1)) + h2 = self.bn4(self.conv4(x)) + + return F.relu(h1 + h2) + + +class BottleNeckB(chainer.Chain): + + def __init__(self, in_size, ch): + initialW = chainer.initializers.HeNormal() + super(BottleNeckB, self).__init__( + conv1=L.Convolution2D( + in_size, ch, 1, 1, 0, nobias=True, initialW=initialW), + bn1=L.BatchNormalization(ch), + conv2=L.Convolution2D( + ch, ch, 3, 1, 1, nobias=True, initialW=initialW), + bn2=L.BatchNormalization(ch), + conv3=L.Convolution2D( + ch, in_size, 1, 1, 0, nobias=True, initialW=initialW), + bn3=L.BatchNormalization(in_size), + ) + + def __call__(self, x): + h = F.relu(self.bn1(self.conv1(x))) + h = F.relu(self.bn2(self.conv2(h))) + h = self.bn3(self.conv3(h)) + + return F.relu(h + x) + + +class Block(chainer.Chain): + + def __init__(self, layer, in_size, ch, out_size, stride=2): + super(Block, self).__init__() + links = [('a', BottleNeckA(in_size, ch, out_size, stride))] + for i in range(layer - 1): + links += [('b{}'.format(i + 1), BottleNeckB(out_size, ch))] + + for l in links: + self.add_link(*l) + self.forward = links + + def __call__(self, x): + for name, _ in self.forward: + f = getattr(self, name) + x = f(x) + + return x + + +class ResNet101(chainer.Chain): + + insize = 224 + + def __init__(self): + initialW = chainer.initializers.HeNormal() + super(ResNet101, self).__init__( + conv1=L.Convolution2D( + 3, 64, 7, 2, 3, nobias=True, initialW=initialW), + bn1=L.BatchNormalization(64), + res2=Block(3, 64, 64, 256, 1), + res3=Block(4, 256, 128, 512), + res4=Block(23, 512, 256, 1024), + res5=Block(3, 1024, 512, 2048), + fc=L.Linear(2048, 1000), + ) + + def clear(self): + self.loss = None + self.accuracy = None + + def __call__(self, x, t=None): + self.clear() + h = self.bn1(self.conv1(x)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = self.res2(h) + h = self.res3(h) + h = self.res4(h) + h = self.res5(h) + h = F.average_pooling_2d(h, 7, stride=1) + h = self.fc(h) + + if t is None: + return h + + self.loss = F.softmax_cross_entropy(h, t) + self.accuracy = F.accuracy(h, t) + return self.loss + + +class ResNet101Feature(ResNet101): + + def __call__(self, x): + h = self.bn1(self.conv1(x)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = self.res2(h) + h = self.res3(h) + h = self.res4(h) + h = self.res5(h) + h = F.average_pooling_2d(h, 7, stride=1) + return h diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/resnet152.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/resnet152.py new file mode 100644 index 00000000000..ebe158fc254 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/resnet152.py @@ -0,0 +1,133 @@ +# Modified work: Copyright (c) 2017 Kentaro Wada. +# Original work: https://github.com/yasunorikudo/chainer-ResNet + +import chainer +import chainer.functions as F +import chainer.links as L + + +class BottleNeckA(chainer.Chain): + + def __init__(self, in_size, ch, out_size, stride=2): + initialW = chainer.initializers.HeNormal() + super(BottleNeckA, self).__init__( + conv1=L.Convolution2D( + in_size, ch, 1, stride, 0, nobias=True, initialW=initialW), + bn1=L.BatchNormalization(ch), + conv2=L.Convolution2D( + ch, ch, 3, 1, 1, nobias=True, initialW=initialW), + bn2=L.BatchNormalization(ch), + conv3=L.Convolution2D( + ch, out_size, 1, 1, 0, nobias=True, initialW=initialW), + bn3=L.BatchNormalization(out_size), + + conv4=L.Convolution2D( + in_size, out_size, 1, stride, 0, + nobias=True, initialW=initialW), + bn4=L.BatchNormalization(out_size), + ) + + def __call__(self, x): + h1 = F.relu(self.bn1(self.conv1(x))) + h1 = F.relu(self.bn2(self.conv2(h1))) + h1 = self.bn3(self.conv3(h1)) + h2 = self.bn4(self.conv4(x)) + + return F.relu(h1 + h2) + + +class BottleNeckB(chainer.Chain): + + def __init__(self, in_size, ch): + initialW = chainer.initializers.HeNormal() + super(BottleNeckB, self).__init__( + conv1=L.Convolution2D( + in_size, ch, 1, 1, 0, nobias=True, initialW=initialW), + bn1=L.BatchNormalization(ch), + conv2=L.Convolution2D( + ch, ch, 3, 1, 1, nobias=True, initialW=initialW), + bn2=L.BatchNormalization(ch), + conv3=L.Convolution2D( + ch, in_size, 1, 1, 0, nobias=True, initialW=initialW), + bn3=L.BatchNormalization(in_size), + ) + + def __call__(self, x): + h = F.relu(self.bn1(self.conv1(x))) + h = F.relu(self.bn2(self.conv2(h))) + h = self.bn3(self.conv3(h)) + + return F.relu(h + x) + + +class Block(chainer.Chain): + + def __init__(self, layer, in_size, ch, out_size, stride=2): + super(Block, self).__init__() + links = [('a', BottleNeckA(in_size, ch, out_size, stride))] + for i in range(layer - 1): + links += [('b{}'.format(i + 1), BottleNeckB(out_size, ch))] + + for l in links: + self.add_link(*l) + self.forward = links + + def __call__(self, x): + for name, _ in self.forward: + f = getattr(self, name) + x = f(x) + + return x + + +class ResNet152(chainer.Chain): + + insize = 224 + + def __init__(self): + initialW = chainer.initializers.HeNormal() + super(ResNet152, self).__init__( + conv1=L.Convolution2D( + 3, 64, 7, 2, 3, nobias=True, initialW=initialW), + bn1=L.BatchNormalization(64), + res2=Block(3, 64, 64, 256, 1), + res3=Block(8, 256, 128, 512), + res4=Block(36, 512, 256, 1024), + res5=Block(3, 1024, 512, 2048), + fc=L.Linear(2048, 1000), + ) + + def clear(self): + self.loss = None + self.accuracy = None + + def __call__(self, x, t=None): + self.clear() + h = self.bn1(self.conv1(x)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = self.res2(h) + h = self.res3(h) + h = self.res4(h) + h = self.res5(h) + h = F.average_pooling_2d(h, 7, stride=1) + h = self.fc(h) + + if t is None: + return h + + self.loss = F.softmax_cross_entropy(h, t) + self.accuracy = F.accuracy(h, t) + return self.loss + + +class ResNet152Feature(ResNet152): + + def __call__(self, x): + h = self.bn1(self.conv1(x)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = self.res2(h) + h = self.res3(h) + h = self.res4(h) + h = self.res5(h) + h = F.average_pooling_2d(h, 7, stride=1) + return h diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/resnet50.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/resnet50.py new file mode 100644 index 00000000000..64567dbef9d --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/resnet/resnet50.py @@ -0,0 +1,130 @@ +# Modified work: Copyright (c) 2017 Kentaro Wada. +# Original work: https://github.com/yasunorikudo/chainer-ResNet + +import chainer +import chainer.functions as F +import chainer.links as L + + +class BottleNeckA(chainer.Chain): + def __init__(self, in_size, ch, out_size, stride=2): + initialW = chainer.initializers.HeNormal() + super(BottleNeckA, self).__init__( + conv1=L.Convolution2D( + in_size, ch, 1, stride, 0, nobias=True, initialW=initialW), + bn1=L.BatchNormalization(ch), + conv2=L.Convolution2D( + ch, ch, 3, 1, 1, nobias=True, initialW=initialW), + bn2=L.BatchNormalization(ch), + conv3=L.Convolution2D( + ch, out_size, 1, 1, 0, nobias=True, initialW=initialW), + bn3=L.BatchNormalization(out_size), + + conv4=L.Convolution2D( + in_size, out_size, 1, stride, 0, + nobias=True, initialW=initialW), + bn4=L.BatchNormalization(out_size), + ) + + def __call__(self, x): + h1 = F.relu(self.bn1(self.conv1(x))) + h1 = F.relu(self.bn2(self.conv2(h1))) + h1 = self.bn3(self.conv3(h1)) + h2 = self.bn4(self.conv4(x)) + + return F.relu(h1 + h2) + + +class BottleNeckB(chainer.Chain): + def __init__(self, in_size, ch): + initialW = chainer.initializers.HeNormal() + super(BottleNeckB, self).__init__( + conv1=L.Convolution2D( + in_size, ch, 1, 1, 0, nobias=True, initialW=initialW), + bn1=L.BatchNormalization(ch), + conv2=L.Convolution2D( + ch, ch, 3, 1, 1, nobias=True, initialW=initialW), + bn2=L.BatchNormalization(ch), + conv3=L.Convolution2D( + ch, in_size, 1, 1, 0, nobias=True, initialW=initialW), + bn3=L.BatchNormalization(in_size), + ) + + def __call__(self, x): + h = F.relu(self.bn1(self.conv1(x))) + h = F.relu(self.bn2(self.conv2(h))) + h = self.bn3(self.conv3(h)) + + return F.relu(h + x) + + +class Block(chainer.Chain): + def __init__(self, layer, in_size, ch, out_size, stride=2): + super(Block, self).__init__() + links = [('a', BottleNeckA(in_size, ch, out_size, stride))] + for i in range(layer - 1): + links += [('b{}'.format(i + 1), BottleNeckB(out_size, ch))] + + for l in links: + self.add_link(*l) + self.forward = links + + def __call__(self, x): + for name, _ in self.forward: + f = getattr(self, name) + x = f(x) + + return x + + +class ResNet50(chainer.Chain): + + insize = 224 + + def __init__(self): + initialW = chainer.initializers.HeNormal() + super(ResNet50, self).__init__( + conv1=L.Convolution2D( + 3, 64, 7, 2, 3, nobias=True, initialW=initialW), + bn1=L.BatchNormalization(64), + res2=Block(3, 64, 64, 256, 1), + res3=Block(4, 256, 128, 512), + res4=Block(6, 512, 256, 1024), + res5=Block(3, 1024, 512, 2048), + fc=L.Linear(2048, 1000), + ) + + def clear(self): + self.loss = None + self.accuracy = None + + def __call__(self, x, t=None): + self.clear() + h = self.bn1(self.conv1(x)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = self.res2(h) + h = self.res3(h) + h = self.res4(h) + h = self.res5(h) + h = F.average_pooling_2d(h, 7, stride=1) + h = self.fc(h) + + if t is None: + return h + + self.loss = F.softmax_cross_entropy(h, t) + self.accuracy = F.accuracy(h, t) + return self.loss + + +class ResNet50Feature(ResNet50): + + def __call__(self, x): + h = self.bn1(self.conv1(x)) + h = F.max_pooling_2d(F.relu(h), 3, stride=2) + h = self.res2(h) + h = self.res3(h) + h = self.res4(h) + h = self.res5(h) + h = F.average_pooling_2d(h, 7, stride=1) + return h diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/roi_pooling_2d.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/roi_pooling_2d.py new file mode 100644 index 00000000000..f80fdc2f30a --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/roi_pooling_2d.py @@ -0,0 +1,334 @@ +# Modified work: +# ----------------------------------------------------------------------------- +# Copyright (c) 2015 Preferred Infrastructure, Inc. +# Copyright (c) 2015 Preferred Networks, Inc. +# ----------------------------------------------------------------------------- + +# Original work of _roi_pooling_slice, forward_cpu and backward_cpu: +# ----------------------------------------------------------------------------- +# Copyright 2014 Nervana Systems Inc. +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ----------------------------------------------------------------------------- + +# Original work of forward_gpu and backward_gpu: +# ----------------------------------------------------------------------------- +# Fast R-CNN +# Copyright (c) 2015 Microsoft +# Licensed under The MIT License [see fast-rcnn/LICENSE for details] +# Written by Ross Girshick +# ----------------------------------------------------------------------------- + +import numpy +import six + +from chainer import cuda +from chainer import function +from chainer.utils import type_check + + +def _roi_pooling_slice(size, stride, max_size, roi_offset): + start = int(numpy.floor(size * stride)) + end = int(numpy.ceil((size + 1) * stride)) + + start = min(max(start + roi_offset, 0), max_size) + end = min(max(end + roi_offset, 0), max_size) + + return slice(start, end), end - start + + +class ROIPooling2D(function.Function): + + """RoI pooling over a set of 2d planes.""" + + def __init__(self, outh, outw, spatial_scale): + self.outh, self.outw = outh, outw + self.spatial_scale = spatial_scale + + def check_type_forward(self, in_types): + type_check.expect(in_types.size() == 2) + + x_type, roi_type = in_types + type_check.expect( + x_type.dtype == numpy.float32, + x_type.ndim == 4, + roi_type.dtype == numpy.float32, + roi_type.ndim == 2, + roi_type.shape[1] == 5, + ) + + def forward_cpu(self, inputs): + bottom_data, bottom_rois = inputs + n_rois, channels, height, width = bottom_data.shape + top_data = numpy.empty((n_rois, channels, self.outh, self.outw), + dtype=numpy.float32) + self.argmax_data = numpy.empty_like(top_data).astype(numpy.int32) + + for i_roi in six.moves.range(n_rois): + idx, xmin, ymin, xmax, ymax = bottom_rois[i_roi] + xmin = int(round(xmin * self.spatial_scale)) + xmax = int(round(xmax * self.spatial_scale)) + ymin = int(round(ymin * self.spatial_scale)) + ymax = int(round(ymax * self.spatial_scale)) + roi_width = max(xmax - xmin + 1, 1) + roi_height = max(ymax - ymin + 1, 1) + strideh = 1. * roi_height / self.outh + stridew = 1. * roi_width / self.outw + + for outh in six.moves.range(self.outh): + sliceh, lenh = _roi_pooling_slice( + outh, strideh, height, ymin) + if sliceh.stop <= sliceh.start: + continue + for outw in six.moves.range(self.outw): + slicew, lenw = _roi_pooling_slice( + outw, stridew, width, xmin) + if slicew.stop <= slicew.start: + continue + roi_data = bottom_data[int(idx), :, sliceh, slicew]\ + .reshape(channels, -1) + top_data[i_roi, :, outh, outw] =\ + numpy.max(roi_data, axis=1) + + # get the max idx respect to feature_maps coordinates + max_idx_slice = numpy.unravel_index( + numpy.argmax(roi_data, axis=1), (lenh, lenw)) + max_idx_slice_h = max_idx_slice[0] + sliceh.start + max_idx_slice_w = max_idx_slice[1] + slicew.start + max_idx_slice = max_idx_slice_h * width + max_idx_slice_w + self.argmax_data[i_roi, :, outh, outw] = max_idx_slice + return top_data, + + def forward_gpu(self, inputs): + bottom_data, bottom_rois = inputs + channels, height, width = bottom_data.shape[1:] + n_rois = bottom_rois.shape[0] + top_data = cuda.cupy.empty((n_rois, channels, self.outh, + self.outw), dtype=numpy.float32) + self.argmax_data = cuda.cupy.empty_like(top_data).astype(numpy.int32) + cuda.cupy.ElementwiseKernel( + ''' + raw float32 bottom_data, float32 spatial_scale, int32 channels, + int32 height, int32 width, int32 pooled_height, int32 pooled_width, + raw float32 bottom_rois + ''', + 'float32 top_data, int32 argmax_data', + ''' + // pos in output filter + int pw = i % pooled_width; + int ph = (i / pooled_width) % pooled_height; + int c = (i / pooled_width / pooled_height) % channels; + int num = i / pooled_width / pooled_height / channels; + + int roi_batch_ind = bottom_rois[num * 5 + 0]; + int roi_start_w = round(bottom_rois[num * 5 + 1] * spatial_scale); + int roi_start_h = round(bottom_rois[num * 5 + 2] * spatial_scale); + int roi_end_w = round(bottom_rois[num * 5 + 3] * spatial_scale); + int roi_end_h = round(bottom_rois[num * 5 + 4] * spatial_scale); + + // Force malformed ROIs to be 1x1 + int roi_width = max(roi_end_w - roi_start_w + 1, 1); + int roi_height = max(roi_end_h - roi_start_h + 1, 1); + float bin_size_h = static_cast(roi_height) + / static_cast(pooled_height); + float bin_size_w = static_cast(roi_width) + / static_cast(pooled_width); + + int hstart = static_cast(floor(static_cast(ph) + * bin_size_h)); + int wstart = static_cast(floor(static_cast(pw) + * bin_size_w)); + int hend = static_cast(ceil(static_cast(ph + 1) + * bin_size_h)); + int wend = static_cast(ceil(static_cast(pw + 1) + * bin_size_w)); + + // Add roi offsets and clip to input boundaries + hstart = min(max(hstart + roi_start_h, 0), height); + hend = min(max(hend + roi_start_h, 0), height); + wstart = min(max(wstart + roi_start_w, 0), width); + wend = min(max(wend + roi_start_w, 0), width); + bool is_empty = (hend <= hstart) || (wend <= wstart); + + // Define an empty pooling region to be zero + float maxval = is_empty ? 0 : -1E+37; + // If nothing is pooled, argmax=-1 causes nothing to be backprop'd + int maxidx = -1; + int data_offset = (roi_batch_ind * channels + c) * height * width; + for (int h = hstart; h < hend; ++h) { + for (int w = wstart; w < wend; ++w) { + int bottom_index = h * width + w; + if (bottom_data[data_offset + bottom_index] > maxval) { + maxval = bottom_data[data_offset + bottom_index]; + maxidx = bottom_index; + } + } + } + top_data = maxval; + argmax_data = maxidx; + ''', 'roi_poolig_2d_fwd' + )(bottom_data, self.spatial_scale, channels, height, width, + self.outh, self.outw, bottom_rois, top_data, + self.argmax_data) + + return top_data, + + def backward_cpu(self, inputs, gy): + bottom_data, bottom_rois = inputs + n_rois, channels, height, width = bottom_data.shape + bottom_delta = numpy.zeros_like(bottom_data, dtype=numpy.float32) + + for i_roi in six.moves.range(n_rois): + idx, xmin, ymin, xmax, ymax = bottom_rois[i_roi] + idx = int(idx) + xmin = int(round(xmin * self.spatial_scale)) + xmax = int(round(xmax * self.spatial_scale)) + ymin = int(round(ymin * self.spatial_scale)) + ymax = int(round(ymax * self.spatial_scale)) + roi_width = max(xmax - xmin + 1, 1) + roi_height = max(ymax - ymin + 1, 1) + + strideh = float(roi_height) / float(self.outh) + stridew = float(roi_width) / float(self.outw) + + # iterate all the w, h (from feature map) that fall into this ROIs + for w in six.moves.range(xmin, xmax + 1): + for h in six.moves.range(ymin, ymax + 1): + phstart = int(numpy.floor(float(h - ymin) / strideh)) + phend = int(numpy.ceil(float(h - ymin + 1) / strideh)) + pwstart = int(numpy.floor(float(w - xmin) / stridew)) + pwend = int(numpy.ceil(float(w - xmin + 1) / stridew)) + + phstart = min(max(phstart, 0), self.outh) + phend = min(max(phend, 0), self.outh) + pwstart = min(max(pwstart, 0), self.outw) + pwend = min(max(pwend, 0), self.outw) + + for ph in six.moves.range(phstart, phend): + for pw in six.moves.range(pwstart, pwend): + max_idx_tmp = self.argmax_data[i_roi, :, ph, pw] + for c in six.moves.range(channels): + if max_idx_tmp[c] == (h * width + w): + bottom_delta[idx, c, h, w] += \ + gy[0][i_roi, c, ph, pw] + return bottom_delta, None + + def backward_gpu(self, inputs, gy): + bottom_data, bottom_rois = inputs + channels, height, width = bottom_data.shape[1:] + bottom_diff = cuda.cupy.zeros_like(bottom_data, dtype=numpy.float32) + cuda.cupy.ElementwiseKernel( + ''' + raw float32 top_diff, raw int32 argmax_data, int32 num_rois, + float32 spatial_scale, int32 channels, int32 height, int32 width, + int32 pooled_height, int32 pooled_width, raw float32 bottom_rois + ''', + 'float32 bottom_diff', + ''' + int w = i % width; + int h = (i / width) % height; + int c = (i / (width * height)) % channels; + int num = i / (width * height * channels); + + float gradient = 0; + // Accumulate gradient over all ROIs that pooled this element + for (int roi_n = 0; roi_n < num_rois; ++roi_n) { + // Skip if ROI's batch index doesn't match num + if (num != static_cast(bottom_rois[roi_n * 5])) { + continue; + } + + int roi_start_w = round(bottom_rois[roi_n * 5 + 1] + * spatial_scale); + int roi_start_h = round(bottom_rois[roi_n * 5 + 2] + * spatial_scale); + int roi_end_w = round(bottom_rois[roi_n * 5 + 3] + * spatial_scale); + int roi_end_h = round(bottom_rois[roi_n * 5 + 4] + * spatial_scale); + + // Skip if ROI doesn't include (h, w) + const bool in_roi = (w >= roi_start_w && w <= roi_end_w && + h >= roi_start_h && h <= roi_end_h); + if (!in_roi) { + continue; + } + + int offset = (roi_n * channels + c) * pooled_height + * pooled_width; + + // Compute feasible set of pooled units that could have pooled + // this bottom unit + + // Force malformed ROIs to be 1x1 + int roi_width = max(roi_end_w - roi_start_w + 1, 1); + int roi_height = max(roi_end_h - roi_start_h + 1, 1); + + float bin_size_h = static_cast(roi_height) + / static_cast(pooled_height); + float bin_size_w = static_cast(roi_width) + / static_cast(pooled_width); + + int phstart = floor(static_cast(h - roi_start_h) + / bin_size_h); + int phend = ceil(static_cast(h - roi_start_h + 1) + / bin_size_h); + int pwstart = floor(static_cast(w - roi_start_w) + / bin_size_w); + int pwend = ceil(static_cast(w - roi_start_w + 1) + / bin_size_w); + + phstart = min(max(phstart, 0), pooled_height); + phend = min(max(phend, 0), pooled_height); + pwstart = min(max(pwstart, 0), pooled_width); + pwend = min(max(pwend, 0), pooled_width); + + for (int ph = phstart; ph < phend; ++ph) { + for (int pw = pwstart; pw < pwend; ++pw) { + int index_ = ph * pooled_width + pw + offset; + if (argmax_data[index_] == (h * width + w)) { + gradient += top_diff[index_]; + } + } + } + } + bottom_diff = gradient; + ''', 'roi_pooling_2d_bwd' + )(gy[0], self.argmax_data, bottom_rois.shape[0], self.spatial_scale, + channels, height, width, self.outh, self.outw, + bottom_rois, bottom_diff) + + return bottom_diff, None + + +def roi_pooling_2d(x, rois, outh, outw, spatial_scale): + """Spatial Region of Interest (ROI) pooling function. + + This function acts similarly to :class:`~functions.MaxPooling2D`, but + it computes the maximum of input spatial patch for each channel + with the region of interest. + + Args: + x (~chainer.Variable): Input variable. + rois (~chainer.Variable): Input roi variable. + outh (int): Height of output image after pooled. + outw (int): Width of output image after pooled. + spatial_scale (float): Scale of the roi is resized. + + Returns: + ~chainer.Variable: Ouptut variable. + + See the original paper proposing ROIPooling: + `Fast R-CNN `_. + + """ + return ROIPooling2D(outh, outw, spatial_scale)(x, rois) diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg16.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg16.py new file mode 100644 index 00000000000..b25df232e49 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg16.py @@ -0,0 +1,68 @@ +import chainer +import chainer.functions as F +import chainer.links as L + + +class VGG16(chainer.Chain): + + def __init__(self, n_class): + super(VGG16, self).__init__( + conv1_1=L.Convolution2D(3, 64, 3, stride=1, pad=1), + conv1_2=L.Convolution2D(64, 64, 3, stride=1, pad=1), + + conv2_1=L.Convolution2D(64, 128, 3, stride=1, pad=1), + conv2_2=L.Convolution2D(128, 128, 3, stride=1, pad=1), + + conv3_1=L.Convolution2D(128, 256, 3, stride=1, pad=1), + conv3_2=L.Convolution2D(256, 256, 3, stride=1, pad=1), + conv3_3=L.Convolution2D(256, 256, 3, stride=1, pad=1), + + conv4_1=L.Convolution2D(256, 512, 3, stride=1, pad=1), + conv4_2=L.Convolution2D(512, 512, 3, stride=1, pad=1), + conv4_3=L.Convolution2D(512, 512, 3, stride=1, pad=1), + + conv5_1=L.Convolution2D(512, 512, 3, stride=1, pad=1), + conv5_2=L.Convolution2D(512, 512, 3, stride=1, pad=1), + conv5_3=L.Convolution2D(512, 512, 3, stride=1, pad=1), + + fc6=L.Linear(25088, 4096), + fc7=L.Linear(4096, 4096), + fc8=L.Linear(4096, n_class) + ) + + def __call__(self, x, t=None): + h = F.relu(self.conv1_1(x)) + h = F.relu(self.conv1_2(h)) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.conv2_1(h)) + h = F.relu(self.conv2_2(h)) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.conv3_1(h)) + h = F.relu(self.conv3_2(h)) + h = F.relu(self.conv3_3(h)) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.conv4_1(h)) + h = F.relu(self.conv4_2(h)) + h = F.relu(self.conv4_3(h)) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.conv5_1(h)) + h = F.relu(self.conv5_2(h)) + h = F.relu(self.conv5_3(h)) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.dropout(F.relu(self.fc6(h)), ratio=0.5) + h = F.dropout(F.relu(self.fc7(h)), ratio=0.5) + h = self.fc8(h) + + self.pred = F.softmax(h) + if t is None: + assert not chainer.config.train + return + + self.loss = F.softmax_cross_entropy(h, t) + self.acc = F.accuracy(h, t) + return self.loss diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg16_batch_normalization.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg16_batch_normalization.py new file mode 100644 index 00000000000..9f5244a28da --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg16_batch_normalization.py @@ -0,0 +1,84 @@ +import chainer +import chainer.functions as F +import chainer.links as L + + +class VGG16BatchNormalization(chainer.Chain): + + def __init__(self, n_class=1000): + super(self.__class__, self).__init__( + conv1_1=L.Convolution2D(3, 64, 3, stride=1, pad=1), + bn1_1=L.BatchNormalization(64), + conv1_2=L.Convolution2D(64, 64, 3, stride=1, pad=1), + bn1_2=L.BatchNormalization(64), + + conv2_1=L.Convolution2D(64, 128, 3, stride=1, pad=1), + bn2_1=L.BatchNormalization(128), + conv2_2=L.Convolution2D(128, 128, 3, stride=1, pad=1), + bn2_2=L.BatchNormalization(128), + + conv3_1=L.Convolution2D(128, 256, 3, stride=1, pad=1), + bn3_1=L.BatchNormalization(256), + conv3_2=L.Convolution2D(256, 256, 3, stride=1, pad=1), + bn3_2=L.BatchNormalization(256), + conv3_3=L.Convolution2D(256, 256, 3, stride=1, pad=1), + bn3_3=L.BatchNormalization(256), + + conv4_1=L.Convolution2D(256, 512, 3, stride=1, pad=1), + bn4_1=L.BatchNormalization(512), + conv4_2=L.Convolution2D(512, 512, 3, stride=1, pad=1), + bn4_2=L.BatchNormalization(512), + conv4_3=L.Convolution2D(512, 512, 3, stride=1, pad=1), + bn4_3=L.BatchNormalization(512), + + conv5_1=L.Convolution2D(512, 512, 3, stride=1, pad=1), + bn5_1=L.BatchNormalization(512), + conv5_2=L.Convolution2D(512, 512, 3, stride=1, pad=1), + bn5_2=L.BatchNormalization(512), + conv5_3=L.Convolution2D(512, 512, 3, stride=1, pad=1), + bn5_3=L.BatchNormalization(512), + + fc6=L.Linear(25088, 4096), + fc7=L.Linear(4096, 4096), + fc8=L.Linear(4096, n_class) + ) + + def __call__(self, x, t=None): + h = F.relu(self.bn1_1(self.conv1_1(x))) + h = F.relu(self.bn1_2(self.conv1_2(h))) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.bn2_1(self.conv2_1(h))) + h = F.relu(self.bn2_2(self.conv2_2(h))) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.bn3_1(self.conv3_1(h))) + h = F.relu(self.bn3_2(self.conv3_2(h))) + h = F.relu(self.bn3_3(self.conv3_3(h))) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.bn4_1(self.conv4_1(h))) + h = F.relu(self.bn4_2(self.conv4_2(h))) + h = F.relu(self.bn4_3(self.conv4_3(h))) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.bn5_1(self.conv5_1(h))) + h = F.relu(self.bn5_2(self.conv5_2(h))) + h = F.relu(self.bn5_3(self.conv5_3(h))) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.dropout(F.relu(self.fc6(h)), ratio=0.5) + h = F.dropout(F.relu(self.fc7(h)), ratio=0.5) + h = self.fc8(h) + fc8 = h + + self.pred = F.softmax(h) + + if t is None: + assert not chainer.config.train + return + + self.loss = F.softmax_cross_entropy(fc8, t) + self.acc = F.accuracy(self.pred, t) + + return self.loss diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg16_fast_rcnn.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg16_fast_rcnn.py new file mode 100644 index 00000000000..57402f33b43 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg16_fast_rcnn.py @@ -0,0 +1,63 @@ +import chainer +import chainer.functions as F +import chainer.links as L + + +class VGG16FastRCNN(chainer.Chain): + + def __init__(self): + super(self.__class__, self).__init__( + conv1_1=L.Convolution2D(3, 64, 3, stride=1, pad=1), + conv1_2=L.Convolution2D(64, 64, 3, stride=1, pad=1), + + conv2_1=L.Convolution2D(64, 128, 3, stride=1, pad=1), + conv2_2=L.Convolution2D(128, 128, 3, stride=1, pad=1), + + conv3_1=L.Convolution2D(128, 256, 3, stride=1, pad=1), + conv3_2=L.Convolution2D(256, 256, 3, stride=1, pad=1), + conv3_3=L.Convolution2D(256, 256, 3, stride=1, pad=1), + + conv4_1=L.Convolution2D(256, 512, 3, stride=1, pad=1), + conv4_2=L.Convolution2D(512, 512, 3, stride=1, pad=1), + conv4_3=L.Convolution2D(512, 512, 3, stride=1, pad=1), + + conv5_1=L.Convolution2D(512, 512, 3, stride=1, pad=1), + conv5_2=L.Convolution2D(512, 512, 3, stride=1, pad=1), + conv5_3=L.Convolution2D(512, 512, 3, stride=1, pad=1), + + fc6=L.Linear(25088, 4096), + fc7=L.Linear(4096, 4096), + cls_score=L.Linear(4096, 21), + bbox_pred=L.Linear(4096, 84) + ) + + def __call__(self, x, rois): + h = F.relu(self.conv1_1(x)) + h = F.relu(self.conv1_2(h)) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.conv2_1(h)) + h = F.relu(self.conv2_2(h)) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.conv3_1(h)) + h = F.relu(self.conv3_2(h)) + h = F.relu(self.conv3_3(h)) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.conv4_1(h)) + h = F.relu(self.conv4_2(h)) + h = F.relu(self.conv4_3(h)) + h = F.max_pooling_2d(h, 2, stride=2) + + h = F.relu(self.conv5_1(h)) + h = F.relu(self.conv5_2(h)) + h = F.relu(self.conv5_3(h)) + h = F.roi_pooling_2d(h, rois, 7, 7, spatial_scale=0.0625) + + h = F.dropout(F.relu(self.fc6(h)), ratio=0.5) + h = F.dropout(F.relu(self.fc7(h)), ratio=0.5) + cls_score = F.softmax(self.cls_score(h)) + bbox_pred = self.bbox_pred(h) + + return cls_score, bbox_pred diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg_cnn_m_1024.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg_cnn_m_1024.py new file mode 100644 index 00000000000..74b127eca68 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/chainermodels/vgg_cnn_m_1024.py @@ -0,0 +1,71 @@ +import chainer +from chainer import cuda +import chainer.functions as F +import chainer.links as L +from chainer import Variable +from distutils.version import LooseVersion + + +class VGG_CNN_M_1024(chainer.Chain): + + def __init__(self, n_class=21, bg_label=-1): + super(VGG_CNN_M_1024, self).__init__( + conv1=L.Convolution2D(3, 96, ksize=7, stride=2), + conv2=L.Convolution2D(96, 256, ksize=5, stride=2, pad=1), + conv3=L.Convolution2D(256, 512, ksize=3, stride=1, pad=1), + conv4=L.Convolution2D(512, 512, ksize=3, stride=1, pad=1), + conv5=L.Convolution2D(512, 512, ksize=3, stride=1, pad=1), + fc6=L.Linear(18432, 4096), + fc7=L.Linear(4096, 1024), + cls_score=L.Linear(1024, n_class), + bbox_pred=L.Linear(1024, 4 * n_class) + ) + self.n_class = n_class + self.bg_label = bg_label + + def __call__(self, x, rois, t=None): + h = self.conv1(x) + h = F.relu(h) + h = F.local_response_normalization(h, n=5, k=2, alpha=5e-4, beta=.75) + h = F.max_pooling_2d(h, ksize=3, stride=2) + + h = self.conv2(h) + h = F.relu(h) + h = F.local_response_normalization(h, n=5, k=2, alpha=5e-4, beta=.75) + h = F.max_pooling_2d(h, ksize=3, stride=2) + + h = self.conv3(h) + h = F.relu(h) + + h = self.conv4(h) + h = F.relu(h) + + h = self.conv5(h) + h = F.relu(h) + + h = F.roi_pooling_2d(h, rois, 6, 6, spatial_scale=0.0625) + + h = self.fc6(h) + h = F.relu(h) + h = F.dropout(h, ratio=.5) + + h = self.fc7(h) + h = F.relu(h) + h = F.dropout(h, ratio=.5) + + h_cls_score = self.cls_score(h) + cls_score = F.softmax(h_cls_score) + bbox_pred = self.bbox_pred(h) + + if t is None: + assert not chainer.config.train + return cls_score, bbox_pred + + t_cls, t_bbox = t + self.cls_loss = F.softmax_cross_entropy(h_cls_score, t_cls) + self.bbox_loss = F.smooth_l1_loss(bbox_pred, t_bbox) + + xp = cuda.get_array_module(x.data) + lambda_ = (0.5 * (t_cls.data != self.bg_label)).astype(xp.float32) + L = self.cls_loss + F.sum(lambda_ * self.bbox_loss) + return L diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/color.pyx b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/color.pyx new file mode 100644 index 00000000000..0e6b493f0b3 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/color.pyx @@ -0,0 +1,31 @@ +import numpy as np +cimport numpy as np + + +def bitget(int byteval, int idx): + return ((byteval & (1 << idx)) != 0) + + +def labelcolormap(int N=256): + """Color map (RGB) considering label numbers N. + + Args: + N (``int``): Number of labels. + """ + cdef np.ndarray[np.float32_t, ndim=2] cmap = np.zeros((N, 3), + dtype=np.float32) + cdef int i, j + cdef int id + cdef int r, g, b + for i in range(0, N): + id = i + r, g, b = 0, 0, 0 + for j in range(0, 8): + r = np.bitwise_or(r, (bitget(id, 0) << 7-j)) + g = np.bitwise_or(g, (bitget(id, 1) << 7-j)) + b = np.bitwise_or(b, (bitget(id, 2) << 7-j)) + id = (id >> 3) + cmap[i, 0] = r / 255. + cmap[i, 1] = g / 255. + cmap[i, 2] = b / 255. + return cmap diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/conversations.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/conversations.py new file mode 100644 index 00000000000..e77a39bd1f4 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/conversations.py @@ -0,0 +1,28 @@ +import numpy as np + + +def rects_msg_to_ndarray(rects_msg): + rects = np.zeros((len(rects_msg.rects), 4), dtype=np.float32) + for i, r in enumerate(rects_msg.rects): + xmin = r.x + ymin = r.y + xmax = r.x + r.width + ymax = r.y + r.height + rects[i] = [xmin, ymin, xmax, ymax] + return rects + + +def bounding_box_msg_to_aabb(bbox_msg): + center_x = bbox_msg.pose.position.x + center_y = bbox_msg.pose.position.y + center_z = bbox_msg.pose.position.z + dim_x = bbox_msg.dimensions.x + dim_y = bbox_msg.dimensions.y + dim_z = bbox_msg.dimensions.z + x1 = center_x - dim_x / 2. + x2 = center_x + dim_x / 2. + y1 = center_y - dim_y / 2. + y2 = center_y + dim_y / 2. + z1 = center_z - dim_z / 2. + z2 = center_z + dim_z / 2. + return x1, y1, z1, x2, y2, z2 diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/depth.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/depth.py new file mode 100644 index 00000000000..f7769b166db --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/depth.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import numpy as np +from skimage.segmentation import slic +from skimage.feature import peak_local_max +from skimage.morphology import binary_closing + +from jsk_recognition_utils.mask import descent_closing + + +def split_fore_background(depth_img, footprint=None): + if footprint is None: + footprint = np.ones((3, 3)) + segments = slic(depth_img) + + local_maxi = peak_local_max( + depth_img, labels=segments, footprint=footprint, indices=False) + + fg_mask = descent_closing(local_maxi, init_selem=np.ones((3, 3)), n_times=6) + bg_mask = ~fg_mask + return fg_mask, bg_mask diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/feature.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/feature.py new file mode 100644 index 00000000000..a76be71d202 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/feature.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import numpy as np +from sklearn.cluster import MiniBatchKMeans +from sklearn.neighbors import NearestNeighbors + + +class BagOfFeatures(object): + def __init__(self, hist_size=500): + self.nn = None + self.hist_size = hist_size + + def fit(self, X): + """Fit features and extract bag of features""" + k = self.hist_size + km = MiniBatchKMeans(n_clusters=k, init_size=3*k, max_iter=300) + km.fit(X) + nn = NearestNeighbors(n_neighbors=1) + nn.fit(km.cluster_centers_) + self.nn = nn + + def transform(self, X): + return np.vstack([self.make_hist(xi.reshape((-1, 128))) for xi in X]) + + def make_hist(self, descriptors): + """Make histogram for one image""" + nn = self.nn + if nn is None: + raise ValueError('must fit features before making histogram') + indices = nn.kneighbors(descriptors, return_distance=False) + histogram = np.zeros(self.hist_size) + for idx in np.unique(indices): + mask = indices == idx + histogram[idx] = mask.sum() # count the idx + indices = indices[mask == False] + return histogram + + +def decompose_descriptors_with_label(descriptors, positions, label_img, + skip_zero_label=False): + descriptors = descriptors.reshape((-1, 128)) + positions = positions.reshape((-1, 2)) + assert descriptors.shape[0] == positions.shape[0] + + decomposed = {} + positions = np.round(positions).astype(int) + labels = label_img[positions[:, 1], positions[:, 0]] + for label in np.unique(labels): + if skip_zero_label and (label == 0): + continue + decomposed[label] = descriptors[labels == label].reshape(-1) + + return decomposed diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/geometry.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/geometry.py new file mode 100644 index 00000000000..cc4c142e62f --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/geometry.py @@ -0,0 +1,28 @@ +def get_overlap_of_aabb(box1, box2, return_volumes=False): + x11, y11, z11, x12, y12, z12 = box1 + dim_x1 = x12 - x11 + dim_y1 = y12 - y11 + dim_z1 = z12 - z11 + + x21, y21, z21, x22, y22, z22 = box2 + dim_x2 = x22 - x21 + dim_y2 = y22 - y21 + dim_z2 = z22 - z21 + + if ((x11 <= x22 and x12 >= x21) and + (y11 <= y22 and y12 >= y21) and + (z11 <= z22 and z12 >= z21)): + # has intersect + dim_x3 = min(x12, x22) - max(x11, x21) + dim_y3 = min(y12, y22) - max(y11, y21) + dim_z3 = min(z12, z22) - max(z11, z21) + else: + dim_x3 = dim_y3 = dim_z3 = 0 + + intersect = dim_x3 * dim_y3 * dim_z3 + union = (dim_x1 * dim_y1 * dim_z1) + (dim_x2 * dim_y2 * dim_z2) - intersect + iu = 1. * intersect / union + if return_volumes: + return iu, intersect, union + else: + return iu diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/mask.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/mask.py new file mode 100644 index 00000000000..86f11d585ee --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/mask.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import numpy as np +from skimage.morphology import binary_closing + + +def bounding_rect_of_mask(img, mask): + where = np.argwhere(mask) + (y_start, x_start), (y_stop, x_stop) = where.min(0), where.max(0) + 1 + return img[y_start:y_stop, x_start:x_stop] + + +def descent_closing(mask, init_selem, n_times): + S = init_selem.shape + for i in xrange(n_times): + selem = np.ones((S[0] * (n_times - i), S[1] * (n_times - i))) + mask = binary_closing(mask, selem=selem) + return mask diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/nms.pyx b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/nms.pyx new file mode 100644 index 00000000000..a6c99237ff8 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/nms.pyx @@ -0,0 +1,76 @@ +# Original work: +# -------------------------------------------------------- +# Fast R-CNN +# Copyright (c) 2015 Microsoft +# Licensed under The MIT License [see LICENSE for details] +# Written by Ross Girshick +# https://github.com/rbgirshick/fast-rcnn +# -------------------------------------------------------- + +# Modified work: +# -------------------------------------------------------- +# Copyright (c) 2016 JSK Lab. +# -------------------------------------------------------- + +import numpy as np +cimport numpy as np + +cdef inline np.float32_t max(np.float32_t a, np.float32_t b): + return a if a >= b else b + +cdef inline np.float32_t min(np.float32_t a, np.float32_t b): + return a if a <= b else b + +def nms(np.ndarray[np.float32_t, ndim=2] dets, np.float thresh): + """Non-maximum supression (NMS) baseline.""" + cdef np.ndarray[np.float32_t, ndim=1] x1 = dets[:, 0] + cdef np.ndarray[np.float32_t, ndim=1] y1 = dets[:, 1] + cdef np.ndarray[np.float32_t, ndim=1] x2 = dets[:, 2] + cdef np.ndarray[np.float32_t, ndim=1] y2 = dets[:, 3] + cdef np.ndarray[np.float32_t, ndim=1] scores = dets[:, 4] + + cdef np.ndarray[np.float32_t, ndim=1] areas = (x2 - x1 + 1) * (y2 - y1 + 1) + cdef np.ndarray[np.int_t, ndim=1] order = scores.argsort()[::-1] + + cdef int ndets = dets.shape[0] + cdef np.ndarray[np.int_t, ndim=1] suppressed = \ + np.zeros((ndets), dtype=np.int) + + # nominal indices + cdef int _i, _j + # sorted indices + cdef int i, j + # temp variables for box i's (the box currently under consideration) + cdef np.float32_t ix1, iy1, ix2, iy2, iarea + # variables for computing overlap with box j (lower scoring box) + cdef np.float32_t xx1, yy1, xx2, yy2 + cdef np.float32_t w, h + cdef np.float32_t inter, ovr + + keep = [] + for _i in range(ndets): + i = order[_i] + if suppressed[i] == 1: + continue + keep.append(i) + ix1 = x1[i] + iy1 = y1[i] + ix2 = x2[i] + iy2 = y2[i] + iarea = areas[i] + for _j in range(_i + 1, ndets): + j = order[_j] + if suppressed[j] == 1: + continue + xx1 = max(ix1, x1[j]) + yy1 = max(iy1, y1[j]) + xx2 = min(ix2, x2[j]) + yy2 = min(iy2, y2[j]) + w = max(0.0, xx2 - xx1 + 1) + h = max(0.0, yy2 - yy1 + 1) + inter = w * h + ovr = inter / (iarea + areas[j] - inter) + if ovr >= thresh: + suppressed[j] = 1 + + return keep diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/visualize.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/visualize.py new file mode 100644 index 00000000000..9a3df66543e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/python/jsk_recognition_utils/visualize.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from __future__ import division +import math + +import cv2 +import numpy as np +import PIL + + +def centerize(src, dst_shape, margin_color=None): + """Centerize image for specified image size + + @param src: image to centerize + @param dst_shape: image shape (height, width) or (height, width, channel) + """ + if src.shape[:2] == dst_shape[:2]: + return src + centerized = np.zeros(dst_shape, dtype=src.dtype) + if margin_color is not None: + centerized[:, :] = margin_color + pad_vertical, pad_horizontal = 0, 0 + h, w = src.shape[:2] + dst_h, dst_w = dst_shape[:2] + if h < dst_h: + pad_vertical = (dst_h - h) // 2 + if w < dst_w: + pad_horizontal = (dst_w - w) // 2 + centerized[pad_vertical:pad_vertical+h, + pad_horizontal:pad_horizontal+w] = src + return centerized + +def _tile_images(imgs, tile_shape, concatenated_image, margin_color=None): + """Concatenate images whose sizes are same. + + @param imgs: image list which should be concatenated + @param tile_shape: shape for which images should be concatenated + @param concatenated_image: returned image. if it is None, new image will be created. + """ + x_num, y_num = tile_shape + one_width = imgs[0].shape[1] + one_height = imgs[0].shape[0] + if concatenated_image is None: + concatenated_image = np.zeros((one_height * y_num, one_width * x_num, 3), + dtype=np.uint8) + if margin_color is not None: + concatenated_image[:, :] = margin_color + for y in range(y_num): + for x in range(x_num): + i = x + y * x_num + if i >= len(imgs): + pass + else: + concatenated_image[y*one_height:(y+1)*one_height,x*one_width:(x+1)*one_width,] = imgs[i] + return concatenated_image + + +def get_tile_image(imgs, tile_shape=None, result_img=None, margin_color=None): + """Concatenate images whose sizes are different. + + @param imgs: image list which should be concatenated + @param tile_shape: shape for which images should be concatenated + @param result_img: numpy array to put result image + """ + def get_tile_shape(img_num): + x_num = 0 + y_num = int(math.sqrt(img_num)) + while x_num * y_num < img_num: + x_num += 1 + return x_num, y_num + + if tile_shape is None: + tile_shape = get_tile_shape(len(imgs)) + + # get max tile size to which each image should be resized + max_height, max_width = np.inf, np.inf + for img in imgs: + max_height = min([max_height, img.shape[0]]) + max_width = min([max_width, img.shape[1]]) + + # resize and concatenate images + for i, img in enumerate(imgs): + h, w = img.shape[:2] + h_scale, w_scale = max_height / h, max_width / w + scale = min([h_scale, w_scale]) + h, w = int(scale * h), int(scale * w) + img = cv2.resize(img, (w, h)) + img = centerize(img, (max_height, max_width, 3), + margin_color=margin_color) + imgs[i] = img + return _tile_images(imgs, tile_shape, result_img, + margin_color=margin_color) + + +def colorize_cluster_indices(image, cluster_indices, alpha=0.3, image_alpha=1): + from skimage.color import rgb2gray + from skimage.color import gray2rgb + from skimage.util import img_as_float + from skimage.color.colorlabel import DEFAULT_COLORS + from skimage.color.colorlabel import color_dict + image = img_as_float(rgb2gray(image)) + image = gray2rgb(image) * image_alpha + (1 - image_alpha) + height, width = image.shape[:2] + + n_colors = len(DEFAULT_COLORS) + indices_to_color = np.zeros((height * width, 3)) + for i, indices in enumerate(cluster_indices): + color = color_dict[DEFAULT_COLORS[i % n_colors]] + indices_to_color[indices] = color + indices_to_color = indices_to_color.reshape((height, width, 3)) + result = indices_to_color * alpha + image * (1 - alpha) + result = (result * 255).astype(np.uint8) + return result diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/config/sample_bounding_box_array_publisher.rviz b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/config/sample_bounding_box_array_publisher.rviz new file mode 100644 index 00000000000..c1b120712ab --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/config/sample_bounding_box_array_publisher.rviz @@ -0,0 +1,122 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /BoundingBoxArray1 + Splitter Ratio: 0.544444 + Tree Height: 566 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: BoundingBoxArray + Topic: /bounding_box_array_publisher/output + Unreliable: false + Value: true + alpha: 0.8 + color: 25; 255; 0 + coloring: Auto + line width: 0.005 + only edge: false + show coords: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.09505 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.436994 + Y: 0.0773152 + Z: 0.912857 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.390398 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.03358 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f500fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 30 + Y: 30 diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/config/sample_polygon_array_publisher.rviz b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/config/sample_polygon_array_publisher.rviz new file mode 100644 index 00000000000..87b9a21155e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/config/sample_polygon_array_publisher.rviz @@ -0,0 +1,142 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PolygonArray1 + Splitter Ratio: 0.5 + Tree Height: 1066 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: jsk_rviz_plugin/PolygonArray + Color: 25; 255; 0 + Enabled: true + Name: PolygonArray + Topic: /bounding_box_array_publisher/output + Unreliable: false + Value: true + coloring: Auto + normal length: 0.1 + only border: true + show normal: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + map: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + base_link: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 5.17919 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.02994 + Y: 0.312564 + Z: 0.968804 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.170398 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.63041 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1402 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001d0000004d0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000004d0000000f400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000148000004d0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000004d0000000d300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000083700000052fc0100000002fb0000000800540069006d0065010000000000000837000004b900fffffffb0000000800540069006d0065010000000000000450000000000000000000000513000004d000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2103 + X: 71 + Y: 24 diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_add_bounding_box_array.launch b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_add_bounding_box_array.launch new file mode 100644 index 00000000000..fd84c1613e3 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_add_bounding_box_array.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + approximate_sync: true + slop: 0.9 # input topics are slow (~1Hz) + topics: + - bboxes_a/bounding_box_array_publisher/output + - bboxes_b/bounding_box_array_publisher/output + + + + + + + topic_0: /add_bounding_box_array/output + timeout_0: 30 + + + + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_bounding_box_array_publisher.launch b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_bounding_box_array_publisher.launch new file mode 100644 index 00000000000..9890cd78c1c --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_bounding_box_array_publisher.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + frame_id: shelf_base + boxes: + - position: [-0.22, 0.280, 0.361] + dimension: [0.37, 0.248, 0.218] + - position: [-0.22, 0, 0.361] + dimension: [0.37, 0.306, 0.218] + - position: [-0.22, -0.280, 0.361] + dimension: [0.37, 0.248, 0.218] + - position: [-0.22, 0.280, 0.116] + dimension: [0.37, 0.248, 0.192] + - position: [-0.22, 0, 0.116] + dimension: [0.37, 0.306, 0.192] + - position: [-0.22, -0.280, 0.116] + dimension: [0.37, 0.248, 0.192] + - position: [-0.22, 0.280, -0.116] + dimension: [0.37, 0.248, 0.192] + - position: [-0.22, 0, -0.116] + dimension: [0.37, 0.306, 0.192] + - position: [-0.22, -0.280, -0.116] + dimension: [0.37, 0.248, 0.192] + - position: [-0.22, 0.280, -0.361] + dimension: [0.37, 0.248, 0.218] + - position: [-0.22, 0, -0.361] + dimension: [0.37, 0.306, 0.218] + - position: [-0.22, -0.280, -0.361] + dimension: [0.37, 0.248, 0.218] + + + + + + + + + + + + topic_0: /bounding_box_array_publisher/output + timeout_0: 10 + + + + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_polygon_array_publisher.launch b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_polygon_array_publisher.launch new file mode 100644 index 00000000000..4fddc100e3d --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_polygon_array_publisher.launch @@ -0,0 +1,48 @@ + + + + + + + + frame_id: base_link + polygons: + - points: + - [1.0, -1.0, 0.0] + - [2.0, -1.0, 0.0] + - [2.0, 1.0, 0.0] + - [1.0, 1.0, 0.0] + label: 1 + likelihood: 0.8 + - points: + - [1.0, -1.0, 0.5] + - [2.0, -1.0, 0.5] + - [2.0, 1.0, 0.5] + - [1.0, 1.0, 0.5] + label: 2 + likelihood: 0.6 + - points: + - [1.0, -1.0, 1.0] + - [2.0, -1.0, 1.0] + - [2.0, 1.0, 1.0] + - [1.0, 1.0, 1.0] + label: 3 + likelihood: 0.4 + - points: + - [0.0, 1.0, 0.0] + - [0.8, 1.0, 0.0] + - [0.8, 1.0, 1.0] + - [0.0, 1.0, 1.0] + label: 4 + likelihood: 0.8 + + + + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_static_virtual_camera.launch b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_static_virtual_camera.launch new file mode 100644 index 00000000000..3b5a050f499 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/sample/sample_static_virtual_camera.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/setup.py b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/setup.py new file mode 100755 index 00000000000..dbe13254350 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/setup.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from distutils.core import setup + +from setuptools import find_packages +from catkin_pkg.python_setup import generate_distutils_setup + + +d = generate_distutils_setup( + packages=find_packages('python'), + package_dir={'': 'python'}, +) + +setup(**d) diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/cv_utils.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/cv_utils.cpp new file mode 100644 index 00000000000..8a276909198 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/cv_utils.cpp @@ -0,0 +1,204 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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 "jsk_recognition_utils/cv_utils.h" +#include "jsk_recognition_utils/rgb_colors.h" +#include + +namespace enc = sensor_msgs::image_encodings; + +namespace jsk_recognition_utils +{ + cv::MatND computeHistogram(const cv::Mat& input_image, int bin_size, + float min_value, float max_value, + const cv::Mat& mask_image) + { + int channels[] = {0}; + cv::MatND hist; + int hist_size[] = {bin_size}; + float range[] = {min_value, max_value}; + const float* ranges[] = {range}; + cv::calcHist(&input_image, 1, channels, mask_image, + hist, 1, hist_size, + ranges, true, false); + return hist; + } + + std::vector + cvMatNDToHistogramWithRangeBinArray(const cv::MatND& cv_hist, float min_value, float max_value) + { + std::vector bins(cv_hist.total()); + const float bin_width = (max_value - min_value) / cv_hist.total(); + for (size_t i = 0; i < cv_hist.total(); i++) { + const float left = i * bin_width + min_value; + const float right = (i + 1) * bin_width + min_value; + jsk_recognition_msgs::HistogramWithRangeBin bin; + bin.min_value = left; + bin.max_value = right; + bin.count = cv_hist.at(0, i); + bins[i] = bin; + } + return bins; + } + + cv::MatND + HistogramWithRangeBinArrayTocvMatND( + const std::vector& histogram) + { + cv::MatND ret(1, histogram.size(), CV_32F); + for (size_t i = 0; i < histogram.size(); i++) { + jsk_recognition_msgs::HistogramWithRangeBin bin = histogram[i]; + ret.at(0, i) = bin.count; + } + return ret; + } + + bool compareHistogramWithRangeBin(const jsk_recognition_msgs::HistogramWithRangeBin& left, + const jsk_recognition_msgs::HistogramWithRangeBin& right) + { + return left.count > right.count; + } + + void sortHistogramWithRangeBinArray(std::vector& bins) + { + std::sort(bins.begin(), bins.end(), compareHistogramWithRangeBin); + } + + std::vector + topNHistogramWithRangeBins(const std::vector& bins, + double top_n_rate) + { + int sum = 0; + for (size_t i = 0; i < bins.size(); i++) { + sum += bins[i].count; + } + const int target_sum = sum * top_n_rate; + std::vector top_n_bins; + top_n_bins.reserve(bins.size()); + + int current_sum = 0; + for (size_t i = 0; i < bins.size(); i++) { + jsk_recognition_msgs::HistogramWithRangeBin bin = bins[i]; + if (current_sum >= target_sum) { + return top_n_bins; + } + top_n_bins.push_back(bin); + current_sum += bins[i].count; + } + return top_n_bins; + } + + void + drawHistogramWithRangeBin(cv::Mat& image, + const jsk_recognition_msgs::HistogramWithRangeBin& bin, + float min_width_value, + float max_width_value, + float max_height_value, + cv::Scalar color) + { + if (max_height_value == 0.0) { + return; + } + const int height = image.rows; + const int width = image.cols; + const int left = (bin.min_value - min_width_value) / (max_width_value - min_width_value) * width; + const int right = (bin.max_value - min_width_value) / (max_width_value - min_width_value) * width; + const int top = bin.count / max_height_value * height; + if (bin.count == 0 || top == 0 || left == right || left < 0 || right >= width || top > height) { + return; + } + + cv::rectangle(image, cv::Point(left, height), cv::Point(right, height - top), + color, CV_FILLED); + } + + void labelToRGB(const cv::Mat src, cv::Mat& dst) + { + dst = cv::Mat::zeros(src.rows, src.cols, CV_8UC3); + for (size_t j = 0; j < src.rows; ++j) { + for (size_t i = 0; i < src.cols; ++i) { + int label = src.at(j, i); + if (label == 0) { // background label + dst.at(j, i) = cv::Vec3b(0, 0, 0); + } + else { + cv::Vec3d rgb = jsk_recognition_utils::getRGBColor(label); + dst.at(j, i) = cv::Vec3b(int(rgb[0] * 255), int(rgb[1] * 255), int(rgb[2] * 255)); + } + } + } + } + + cv::Rect boundingRectOfMaskImage(const cv::Mat& image) + { + int min_x = image.cols; + int min_y = image.rows; + int max_x = 0; + int max_y = 0; + for (int j = 0; j < image.rows; j++) { + for (int i = 0; i < image.cols; i++) { + if (image.at(j, i) != 0) { + min_x = std::min(min_x, i); + min_y = std::min(min_y, j); + max_x = std::max(max_x, i); + max_y = std::max(max_y, j); + } + } + } + return cv::Rect(min_x, min_y, std::max(max_x - min_x, 0), std::max(max_y - min_y, 0)); + } + + // Utility functions for inspecting an encoding string + bool isBGR(const std::string& encoding) + { + return encoding == enc::BGR8 || encoding == enc::BGR16; + } + + bool isRGB(const std::string& encoding) + { + return encoding == enc::RGB8 || encoding == enc::RGB16; + } + + bool isBGRA(const std::string& encoding) + { + return encoding == enc::BGRA8 || encoding == enc::BGRA16; + } + + bool isRGBA(const std::string& encoding) + { + return encoding == enc::RGBA8 || encoding == enc::RGBA16; + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/convex_polygon.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/convex_polygon.cpp new file mode 100644 index 00000000000..3e0e0232740 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/convex_polygon.cpp @@ -0,0 +1,294 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#define BOOST_PARAMETER_MAX_ARITY 7 + +#include "jsk_recognition_utils/geo/convex_polygon.h" +#include "jsk_recognition_utils/geo_util.h" + +namespace jsk_recognition_utils +{ + ConvexPolygon::ConvexPolygon(const Vertices& vertices): + Polygon(vertices) + { + + } + + ConvexPolygon::ConvexPolygon(const Vertices& vertices, + const std::vector& coefficients): + Polygon(vertices, coefficients) + { + + } + + void ConvexPolygon::projectOnPlane( + const Eigen::Vector3f& p, Eigen::Vector3f& output) + { + Plane::project(p, output); + } + + void ConvexPolygon::projectOnPlane( + const Eigen::Affine3f& pose, Eigen::Affine3f& output) + { + Eigen::Vector3f p(pose.translation()); + Eigen::Vector3f output_p; + projectOnPlane(p, output_p); + // ROS_INFO("[ConvexPolygon::projectOnPlane] p: [%f, %f, %f]", + // p[0], p[1], p[2]); + // ROS_INFO("[ConvexPolygon::projectOnPlane] output_p: [%f, %f, %f]", + // output_p[0], output_p[1], output_p[2]); + Eigen::Quaternionf rot; + rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(), + coordinates().rotation() * Eigen::Vector3f::UnitZ()); + Eigen::Quaternionf coords_rot(coordinates().rotation()); + Eigen::Quaternionf pose_rot(pose.rotation()); + // ROS_INFO("[ConvexPolygon::projectOnPlane] rot: [%f, %f, %f, %f]", + // rot.x(), rot.y(), rot.z(), rot.w()); + // ROS_INFO("[ConvexPolygon::projectOnPlane] coords_rot: [%f, %f, %f, %f]", + // coords_rot.x(), coords_rot.y(), coords_rot.z(), coords_rot.w()); + // ROS_INFO("[ConvexPolygon::projectOnPlane] pose_rot: [%f, %f, %f, %f]", + // pose_rot.x(), pose_rot.y(), pose_rot.z(), pose_rot.w()); + // ROS_INFO("[ConvexPolygon::projectOnPlane] normal: [%f, %f, %f]", normal_[0], normal_[1], normal_[2]); + // Eigen::Affine3f::Identity() * + // output.translation() = Eigen::Translation3f(output_p); + // output.rotation() = rot * pose.rotation(); + //output = Eigen::Translation3f(output_p) * rot * pose.rotation(); + output = Eigen::Affine3f(rot * pose.rotation()); + output.pretranslate(output_p); + // Eigen::Vector3f projected_point = output * Eigen::Vector3f(0, 0, 0); + // ROS_INFO("[ConvexPolygon::projectOnPlane] output: [%f, %f, %f]", + // projected_point[0], projected_point[1], projected_point[2]); + } + + ConvexPolygon ConvexPolygon::flipConvex() + { + Vertices new_vertices; + std::reverse_copy(vertices_.begin(), vertices_.end(), + std::back_inserter(new_vertices)); + std::vector reversed_coefficients(4); + reversed_coefficients[0] = - normal_[0]; + reversed_coefficients[1] = - normal_[1]; + reversed_coefficients[2] = - normal_[2]; + reversed_coefficients[3] = - d_; + + return ConvexPolygon(new_vertices, reversed_coefficients); + } + + void ConvexPolygon::project(const Eigen::Vector3f& p, Eigen::Vector3f& output) + { + Eigen::Vector3f point_on_plane; + Plane::project(p, point_on_plane); + // check point_ is inside or not + if (isInside(point_on_plane)) { + output = point_on_plane; + } + else { + // find the minimum foot point + double min_distance = DBL_MAX; + Eigen::Vector3f min_point; + for (size_t i = 0; i < vertices_.size() - 1; i++) { + Segment seg(vertices_[i], vertices_[i + 1]); + Eigen::Vector3f foot; + double distance = seg.distanceToPoint(p, foot); + if (distance < min_distance) { + min_distance = distance; + min_point = foot; + } + } + output = min_point; + } + } + + void ConvexPolygon::project(const Eigen::Vector3d& p, Eigen::Vector3d& output) + { + Eigen::Vector3f output_f; + Eigen::Vector3f p_f(p[0], p[1], p[2]); + project(p_f, output_f); + pointFromVectorToVector(output_f, output); + } + + void ConvexPolygon::project(const Eigen::Vector3d& p, Eigen::Vector3f& output) + { + Eigen::Vector3f p_f(p[0], p[1], p[2]); + project(p_f, output); + } + + void ConvexPolygon::project(const Eigen::Vector3f& p, Eigen::Vector3d& output) + { + Eigen::Vector3f output_f; + project(p, output_f); + pointFromVectorToVector(output_f, output); + } + + + Eigen::Vector3f ConvexPolygon::getCentroid() + { + Eigen::Vector3f ret(0, 0, 0); + for (size_t i = 0; i < vertices_.size(); i++) { + ret = ret + vertices_[i]; + } + return ret / vertices_.size(); + } + + ConvexPolygon ConvexPolygon::fromROSMsg(const geometry_msgs::Polygon& polygon) + { + Vertices vertices; + for (size_t i = 0; i < polygon.points.size(); i++) { + Eigen::Vector3f p; + pointFromXYZToVector( + polygon.points[i], p); + vertices.push_back(p); + } + return ConvexPolygon(vertices); + } + + ConvexPolygon::Ptr ConvexPolygon::fromROSMsgPtr(const geometry_msgs::Polygon& polygon) + { + Vertices vertices; + for (size_t i = 0; i < polygon.points.size(); i++) { + Eigen::Vector3f p; + pointFromXYZToVector( + polygon.points[i], p); + vertices.push_back(p); + } + return ConvexPolygon::Ptr(new ConvexPolygon(vertices)); + } + + bool ConvexPolygon::distanceSmallerThan(const Eigen::Vector3f& p, + double distance_threshold) + { + double dummy_distance; + return distanceSmallerThan(p, distance_threshold, dummy_distance); + } + + bool ConvexPolygon::distanceSmallerThan(const Eigen::Vector3f& p, + double distance_threshold, + double& output_distance) + { + // first check distance as Plane rather than Convex + double plane_distance = distanceToPoint(p); + if (plane_distance > distance_threshold) { + output_distance = plane_distance; + return false; + } + + Eigen::Vector3f foot_point; + project(p, foot_point); + double convex_distance = (p - foot_point).norm(); + output_distance = convex_distance; + return convex_distance < distance_threshold; + } + + bool ConvexPolygon::allEdgesLongerThan(double thr) + { + for (size_t i = 0; i < vertices_.size(); i++) { + Eigen::Vector3f p_k = vertices_[i]; + Eigen::Vector3f p_k_1; + if (i == vertices_.size() - 1) { + p_k_1 = vertices_[0]; + } + else { + p_k_1 = vertices_[i + 1]; + } + if ((p_k - p_k_1).norm() < thr) { + return false; + } + } + return true; + } + + double ConvexPolygon::distanceFromVertices(const Eigen::Vector3f& p) + { + double min_distance = DBL_MAX; + for (size_t i = 0; i < vertices_.size(); i++) { + Eigen::Vector3f v = vertices_[i]; + double d = (p - v).norm(); + if (d < min_distance) { + min_distance = d; + } + } + return min_distance; + } + + ConvexPolygon::Ptr ConvexPolygon::magnifyByDistance(const double distance) + { + // compute centroid + Eigen::Vector3f c = centroid(); + Vertices new_vertices(vertices_.size()); + for (size_t i = 0; i < vertices_.size(); i++) { + new_vertices[i] = (vertices_[i] - c).normalized() * distance + vertices_[i]; + // ROS_INFO("old v: [%f, %f, %f]", vertices_[i][0], vertices_[i][1], vertices_[i][2]); + // ROS_INFO("new v: [%f, %f, %f]", new_vertices[i][0], new_vertices[i][1], new_vertices[i][2]); + // ROS_INFO(""); + } + + ConvexPolygon::Ptr ret (new ConvexPolygon(new_vertices)); + return ret; + } + + ConvexPolygon::Ptr ConvexPolygon::magnify(const double scale_factor) + { + // compute centroid + Eigen::Vector3f c = centroid(); + Vertices new_vertices; + for (size_t i = 0; i < vertices_.size(); i++) { + new_vertices.push_back((vertices_[i] - c) * scale_factor + c); + } + ConvexPolygon::Ptr ret (new ConvexPolygon(new_vertices)); + return ret; + } + + geometry_msgs::Polygon ConvexPolygon::toROSMsg() + { + geometry_msgs::Polygon polygon; + for (size_t i = 0; i < vertices_.size(); i++) { + geometry_msgs::Point32 ros_point; + ros_point.x = vertices_[i][0]; + ros_point.y = vertices_[i][1]; + ros_point.z = vertices_[i][2]; + polygon.points.push_back(ros_point); + } + return polygon; + } + + + bool ConvexPolygon::isProjectableInside(const Eigen::Vector3f& p) + { + Eigen::Vector3f foot_point; + Plane::project(p, foot_point); + return isInside(foot_point); + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/cube.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/cube.cpp new file mode 100644 index 00000000000..de1d6e483df --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/cube.cpp @@ -0,0 +1,310 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ +#define BOOST_PARAMETER_MAX_ARITY 7 + +#include "jsk_recognition_utils/geo/cube.h" +#include "jsk_recognition_utils/geo_util.h" + +namespace jsk_recognition_utils +{ + Cube::Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot): + pos_(pos), rot_(rot) + { + dimensions_.resize(3); + } + + Cube::Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot, + const std::vector& dimensions): + pos_(pos), rot_(rot), dimensions_(dimensions) + { + + } + Cube::Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot, + const Eigen::Vector3f& dimensions): + pos_(pos), rot_(rot) + { + dimensions_.resize(3); + dimensions_[0] = dimensions[0]; + dimensions_[1] = dimensions[1]; + dimensions_[2] = dimensions[2]; + } + + Cube::Cube(const Eigen::Vector3f& pos, + const Line& line_a, const Line& line_b, const Line& line_c) + { + double distance_a_b = line_a.distance(line_b); + double distance_a_c = line_a.distance(line_c); + double distance_b_c = line_b.distance(line_c); + Line::Ptr axis; + dimensions_.resize(3); + Eigen::Vector3f ex, ey, ez; + if (distance_a_b >= distance_a_c && + distance_a_b >= distance_b_c) { + axis = line_a.midLine(line_b); + line_a.parallelLineNormal(line_c, ex); + line_c.parallelLineNormal(line_b, ey); + + } + else if (distance_a_c >= distance_a_b && + distance_a_c >= distance_b_c) { + axis = line_a.midLine(line_c); + line_a.parallelLineNormal(line_b, ex); + line_b.parallelLineNormal(line_c, ey); + } + else { + // else if (distance_b_c >= distance_a_b && + // distance_b_c >= distance_a_c) { + axis = line_b.midLine(line_c); + line_b.parallelLineNormal(line_a, ex); + line_a.parallelLineNormal(line_c, ey); + } + dimensions_[0] = ex.norm(); + dimensions_[1] = ey.norm(); + axis->getDirection(ez); + ez.normalize(); + ex.normalize(); + ey.normalize(); + if (ex.cross(ey).dot(ez) < 0) { + ez = - ez; + } + rot_ = rotFrom3Axis(ex, ey, ez); + axis->foot(pos, pos_); // project + } + + Cube::Cube(const jsk_recognition_msgs::BoundingBox& box) + { + dimensions_.resize(3); + dimensions_[0] = box.dimensions.x; + dimensions_[1] = box.dimensions.y; + dimensions_[2] = box.dimensions.z; + Eigen::Affine3f pose; + tf::poseMsgToEigen(box.pose, pose); + pos_ = Eigen::Vector3f(pose.translation()); + rot_ = Eigen::Quaternionf(pose.rotation()); // slow + } + + Cube::~Cube() + { + + } + + std::vector Cube::edges() + { + std::vector ret; + Eigen::Vector3f A = pos_ + + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) + + (- dimensions_[1] * Eigen::Vector3f::UnitY()) + + (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0; + Eigen::Vector3f B = pos_ + + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) + + (+ dimensions_[1] * Eigen::Vector3f::UnitY()) + + (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0; + Eigen::Vector3f C = pos_ + + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) + + (+ dimensions_[1] * Eigen::Vector3f::UnitY()) + + (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0; + Eigen::Vector3f D = pos_ + + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) + + (- dimensions_[1] * Eigen::Vector3f::UnitY()) + + (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0; + Eigen::Vector3f E = pos_ + + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) + + (- dimensions_[1] * Eigen::Vector3f::UnitY()) + + (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0; + Eigen::Vector3f F = pos_ + + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) + + (+ dimensions_[1] * Eigen::Vector3f::UnitY()) + + (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0; + Eigen::Vector3f G = pos_ + + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) + + (+ dimensions_[1] * Eigen::Vector3f::UnitY()) + + (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0; + Eigen::Vector3f H = pos_ + + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) + + (- dimensions_[1] * Eigen::Vector3f::UnitY()) + + (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0; + + ret.push_back(Segment::Ptr(new Segment(A, B))); + ret.push_back(Segment::Ptr(new Segment(B, C))); + ret.push_back(Segment::Ptr(new Segment(C, D))); + ret.push_back(Segment::Ptr(new Segment(D, A))); + ret.push_back(Segment::Ptr(new Segment(E, F))); + ret.push_back(Segment::Ptr(new Segment(F, G))); + ret.push_back(Segment::Ptr(new Segment(G, H))); + ret.push_back(Segment::Ptr(new Segment(H, E))); + ret.push_back(Segment::Ptr(new Segment(A, E))); + ret.push_back(Segment::Ptr(new Segment(B, F))); + ret.push_back(Segment::Ptr(new Segment(C, G))); + ret.push_back(Segment::Ptr(new Segment(D, H))); + return ret; + } + + ConvexPolygon::Ptr Cube::intersectConvexPolygon(Plane& plane) + { + std::vector candidate_edges = edges(); + Vertices intersects; + for (size_t i = 0; i < candidate_edges.size(); i++) { + Segment::Ptr edge = candidate_edges[i]; + Eigen::Vector3f p; + if (edge->intersect(plane, p)) { + intersects.push_back(p); + } + } + //ROS_INFO("%lu intersects", intersects.size()); + // Compute convex hull + pcl::ConvexHull chull; + pcl::PointCloud::Ptr chull_input + = verticesToPointCloud(intersects); + pcl::PointCloud::Ptr chull_cloud + (new pcl::PointCloud); + chull.setDimension(2); + chull.setInputCloud(chull_input); + { + boost::mutex::scoped_lock lock(global_chull_mutex); + chull.reconstruct(*chull_cloud); + } + + return ConvexPolygon::Ptr( + new ConvexPolygon(pointCloudToVertices(*chull_cloud))); + } + + jsk_recognition_msgs::BoundingBox Cube::toROSMsg() + { + jsk_recognition_msgs::BoundingBox ret; + ret.pose.position.x = pos_[0]; + ret.pose.position.y = pos_[1]; + ret.pose.position.z = pos_[2]; + ret.pose.orientation.x = rot_.x(); + ret.pose.orientation.y = rot_.y(); + ret.pose.orientation.z = rot_.z(); + ret.pose.orientation.w = rot_.w(); + ret.dimensions.x = dimensions_[0]; + ret.dimensions.y = dimensions_[1]; + ret.dimensions.z = dimensions_[2]; + return ret; + } + + Vertices Cube::vertices() + { + Vertices vs; + vs.push_back(buildVertex(0.5, 0.5, 0.5)); + vs.push_back(buildVertex(-0.5, 0.5, 0.5)); + vs.push_back(buildVertex(-0.5, -0.5, 0.5)); + vs.push_back(buildVertex(0.5, -0.5, 0.5)); + vs.push_back(buildVertex(0.5, 0.5, -0.5)); + vs.push_back(buildVertex(-0.5, 0.5, -0.5)); + vs.push_back(buildVertex(-0.5, -0.5, -0.5)); + vs.push_back(buildVertex(0.5, -0.5, -0.5)); + return vs; + } + + Vertices Cube::transformVertices(const Eigen::Affine3f& pose_offset) + { + Vertices vs = vertices(); + Vertices transformed_vertices; + for (size_t i = 0; i < vs.size(); i++) { + transformed_vertices.push_back(pose_offset * vs[i]); + } + return transformed_vertices; + } + + Polygon::Ptr Cube::buildFace(const Eigen::Vector3f v0, + const Eigen::Vector3f v1, + const Eigen::Vector3f v2, + const Eigen::Vector3f v3) + { + Vertices vs; + vs.push_back(v0); + vs.push_back(v1); + vs.push_back(v2); + vs.push_back(v3); + Polygon::Ptr(new Polygon(vs)); + } + + std::vector Cube::faces() + { + std::vector fs(6); + Vertices vs = vertices(); + Eigen::Vector3f A = vs[0]; + Eigen::Vector3f B = vs[1]; + Eigen::Vector3f C = vs[2]; + Eigen::Vector3f D = vs[3]; + Eigen::Vector3f E = vs[4]; + Eigen::Vector3f F = vs[5]; + Eigen::Vector3f G = vs[6]; + Eigen::Vector3f H = vs[7]; + Vertices vs0, vs1, vs2, vs3, vs4, vs5, vs6; + vs0.push_back(A); vs0.push_back(E); vs0.push_back(F); vs0.push_back(B); + vs1.push_back(B); vs1.push_back(F); vs1.push_back(G); vs1.push_back(C); + vs2.push_back(C); vs2.push_back(G); vs2.push_back(H); vs2.push_back(D); + vs3.push_back(D); vs3.push_back(H); vs3.push_back(E); vs3.push_back(A); + vs4.push_back(A); vs4.push_back(B); vs4.push_back(C); vs4.push_back(D); + vs5.push_back(E); vs5.push_back(H); vs5.push_back(G); vs5.push_back(F); + fs[0].reset(new Polygon(vs0)); + fs[1].reset(new Polygon(vs1)); + fs[2].reset(new Polygon(vs2)); + fs[3].reset(new Polygon(vs3)); + fs[4].reset(new Polygon(vs4)); + fs[5].reset(new Polygon(vs5)); + return fs; + } + + Eigen::Vector3f Cube::buildVertex(double i, double j, double k) + { + Eigen::Vector3f local = (Eigen::Vector3f::UnitX() * i * dimensions_[0] + + Eigen::Vector3f::UnitY() * j * dimensions_[1] + + Eigen::Vector3f::UnitZ() * k * dimensions_[2]); + return Eigen::Translation3f(pos_) * rot_ * local; + } + + Eigen::Vector3f Cube::nearestPoint(const Eigen::Vector3f& p, + double& distance) + { + std::vector current_faces = faces(); + double min_distance = DBL_MAX; + Eigen::Vector3f min_point; + for (size_t i = 0; i < current_faces.size(); i++) { + Polygon::Ptr f = current_faces[i]; + double d; + Eigen::Vector3f q = f->nearestPoint(p, d); + if (min_distance > d) { + min_distance = d; + min_point = q; + } + } + distance = min_distance; + return min_point; + } +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/cylinder.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/cylinder.cpp new file mode 100644 index 00000000000..c17829c4cf2 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/cylinder.cpp @@ -0,0 +1,114 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#define BOOST_PARAMETER_MAX_ARITY 7 + +#include "jsk_recognition_utils/geo/cylinder.h" +#include "jsk_recognition_utils/geo_util.h" + +namespace jsk_recognition_utils +{ + Cylinder::Cylinder(Eigen::Vector3f point, Eigen::Vector3f direction, double radius): + point_(point), direction_(direction), radius_(radius) + { + + } + + void Cylinder::filterPointCloud(const pcl::PointCloud& cloud, + const double threshold, + pcl::PointIndices& output) + { + Line line(direction_, point_); + output.indices.clear(); + for (size_t i = 0; i < cloud.points.size(); i++) { + Eigen::Vector3f p = cloud.points[i].getVector3fMap(); + double d = line.distanceToPoint(p); + if (d < radius_ + threshold && d > radius_ - threshold) { + output.indices.push_back(i); + } + } + } + + void Cylinder::estimateCenterAndHeight(const pcl::PointCloud& cloud, + const pcl::PointIndices& indices, + Eigen::Vector3f& center, + double& height) + { + Line line(direction_, point_); + Vertices points; + for (size_t i = 0; i < indices.indices.size(); i++) { + int point_index = indices.indices[i]; + points.push_back(cloud.points[point_index].getVector3fMap()); + } + PointPair min_max = line.findEndPoints(points); + Eigen::Vector3f min_point = min_max.get<0>(); + Eigen::Vector3f max_point = min_max.get<1>(); + Eigen::Vector3f min_point_projected, max_point_projected; + line.foot(min_point, min_point_projected); + line.foot(max_point, max_point_projected); + height = (min_point_projected - max_point_projected).norm(); + center = (min_point_projected + max_point_projected) / 2.0; + } + + void Cylinder::toMarker(visualization_msgs::Marker& marker, + const Eigen::Vector3f& center, + const Eigen::Vector3f& uz, + const double height) + { + marker.type = visualization_msgs::Marker::CYLINDER; + marker.pose.position.x = center[0]; + marker.pose.position.y = center[1]; + marker.pose.position.z = center[2]; + Eigen::Vector3f orig_z(0, 0, 1); + Eigen::Quaternionf q; + q.setFromTwoVectors(orig_z, uz); + marker.pose.orientation.x = q.x(); + marker.pose.orientation.y = q.y(); + marker.pose.orientation.z = q.z(); + marker.pose.orientation.w = q.w(); + marker.scale.x = radius_ * 2; + marker.scale.y = radius_ * 2; + marker.scale.z = height; + marker.color.a = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + } + + Eigen::Vector3f Cylinder::getDirection() + { + return direction_; + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/grid_plane.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/grid_plane.cpp new file mode 100644 index 00000000000..c85850acec5 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/grid_plane.cpp @@ -0,0 +1,387 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#define BOOST_PARAMETER_MAX_ARITY 7 +#include "jsk_recognition_utils/geo/grid_plane.h" +#include "jsk_recognition_utils/geo_util.h" +#include + +namespace jsk_recognition_utils +{ + GridPlane::GridPlane(ConvexPolygon::Ptr plane, const double resolution): + convex_(plane), resolution_(resolution) + { + + } + + GridPlane::~GridPlane() + { + + } + + GridPlane::IndexPair GridPlane::projectLocalPointAsIndexPair( + const Eigen::Vector3f& p) + { + double offset_x = p[0] + 0.5 * resolution_; + double offset_y = p[1] + 0.5 * resolution_; + // return boost::make_tuple(std::floor(offset_x / resolution_), + // std::floor(offset_y / resolution_)); + return boost::make_tuple(boost::math::round(p[0] / resolution_), + boost::math::round(p[1] / resolution_)); + } + + void GridPlane::addIndexPair(IndexPair pair) + { + cells_.insert(pair); + } + + GridPlane::Ptr GridPlane::dilate(int num) + { + GridPlane::Ptr ret (new GridPlane(convex_, resolution_)); + for (std::set::iterator it = cells_.begin(); + it != cells_.end(); + ++it) { + IndexPair the_index = *it; + for (int xi = - num; xi <= num; xi++) { + for (int yi = - num; yi <= num; yi++) { + if (abs(xi) + abs(yi) <= num) { + IndexPair new_pair = boost::make_tuple( + the_index.get<0>() + xi, + the_index.get<1>() + yi); + ret->cells_.insert(new_pair); + } + } + } + } + return ret; + } + + GridPlane::Ptr GridPlane::erode(int num) + { + GridPlane::Ptr ret (new GridPlane(convex_, resolution_)); + for (std::set::iterator it = cells_.begin(); + it != cells_.end(); + ++it) { + IndexPair the_index = *it; + bool should_removed = false; + for (int xi = - num; xi <= num; xi++) { + for (int yi = - num; yi <= num; yi++) { + if (abs(xi) + abs(yi) <= num) { + IndexPair check_pair = boost::make_tuple( + the_index.get<0>() + xi, + the_index.get<1>() + yi); + if (!isOccupied(check_pair)) { + should_removed = true; + } + } + } + } + if (!should_removed) { + ret->cells_.insert(the_index); + } + } + return ret; + } + + bool GridPlane::isOccupied(const IndexPair& pair) + { + bool result = cells_.find(pair) != cells_.end(); + // Verbosing for debug + // ROS_INFO("Checking index pair (%d, %d)", pair.get<0>(), pair.get<1>()); + // ROS_INFO("Result: %d", result); + // ROS_INFO("cells are:"); + // for (IndexPairSet::iterator it = cells_.begin(); + // it != cells_.end(); + // ++it) { + // ROS_INFO(" (%d, %d)", it->get<0>(), it->get<1>()); + // } + return result; + } + + bool GridPlane::isOccupied(const Eigen::Vector3f& p) + { + IndexPair pair = projectLocalPointAsIndexPair(p); + return isOccupied(pair); + } + + bool GridPlane::isOccupiedGlobal(const Eigen::Vector3f& p) + { + return isOccupied(convex_->coordinates().inverse() * p); + } + + GridPlane::Ptr GridPlane::clone() + { + GridPlane::Ptr ret (new GridPlane(convex_, resolution_)); + ret->cells_ = cells_; + return ret; + } + + Eigen::Vector3f GridPlane::unprojectIndexPairAsLocalPoint( + const IndexPair& pair) + { + return Eigen::Vector3f(pair.get<0>() * resolution_, + pair.get<1>() * resolution_, + 0); + } + + Eigen::Vector3f GridPlane::unprojectIndexPairAsGlobalPoint( + const IndexPair& pair) + { + Eigen::Vector3f local_point = unprojectIndexPairAsLocalPoint(pair); + return convex_->coordinates() * local_point; + } + + void GridPlane::fillCellsFromCube(Cube& cube) + { + ConvexPolygon::Ptr intersect_polygon = cube.intersectConvexPolygon(*convex_); + // 1. transform vertices into local coordinates + // 2. compute min-max + // 3. compute candidates + // 4. filter candidates + + // 1. transform vertices into local coordinates + Vertices local_vertices; + Vertices global_vertices = intersect_polygon->getVertices(); + Eigen::Affine3f inv_coords = convex_->coordinates().inverse(); + for (size_t i = 0; i < global_vertices.size(); i++) { + local_vertices.push_back(inv_coords * global_vertices[i]); + } + + // 2. compute min-max + double min_x = DBL_MAX; + double min_y = DBL_MAX; + double max_x = - DBL_MAX; + double max_y = - DBL_MAX; + for (size_t i = 0; i < local_vertices.size(); i++) { + min_x = ::fmin(min_x, local_vertices[i][0]); + min_y = ::fmin(min_y, local_vertices[i][1]); + max_x = ::fmax(max_x, local_vertices[i][0]); + max_y = ::fmax(max_y, local_vertices[i][1]); + } + // ROS_INFO("x: [%f~%f]", min_x, max_x); + // ROS_INFO("y: [%f~%f]", min_y, max_y); + // 3. compute candidates + std::vector triangles + = intersect_polygon->decomposeToTriangles(); + for (double x = min_x; x <= max_x; x += resolution_) { + for (double y = min_y; y <= max_y; y += resolution_) { + Eigen::Vector3f local_p(x, y, 0); + Eigen::Vector3f p = convex_->coordinates() * local_p; + // 4. filter candidates + bool insidep = false; + for (size_t i = 0; i < triangles.size(); i++) { + if (triangles[i]->isInside(p)) { + insidep = true; + break; + } + } + if (insidep) { + IndexPair pair = projectLocalPointAsIndexPair(local_p); + addIndexPair(pair); + } + } + } + } + + size_t GridPlane::fillCellsFromPointCloud( + pcl::PointCloud::Ptr& cloud, + double distance_threshold, + std::set& non_plane_indices) + { + return fillCellsFromPointCloud( + cloud, distance_threshold, M_PI / 2.0, non_plane_indices); + } + + size_t GridPlane::fillCellsFromPointCloud( + pcl::PointCloud::Ptr& cloud, + double distance_threshold, + double normal_threshold, + std::set& non_plane_indices) + { + Eigen::Affine3f local_coordinates = convex_->coordinates(); + Eigen::Affine3f inv_local_coordinates = local_coordinates.inverse(); + //std::vector triangles = convex_->decomposeToTriangles(); + + pcl::ExtractPolygonalPrismData prism_extract; + pcl::PointCloud::Ptr + hull_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr + hull_output (new pcl::PointCloud); + pcl::PointCloud::Ptr + rehull_cloud (new pcl::PointCloud); + convex_->boundariesToPointCloud(*hull_cloud); + // pcl::ConvexHull chull; + // chull.setDimension(2); + // chull.setInputCloud (hull_cloud); + // chull.reconstruct(*hull_output); + + // it's important to make it sure to close the loop of + // convex hull + hull_cloud->points.push_back(hull_cloud->points[0]); + + prism_extract.setInputCloud(cloud); + prism_extract.setHeightLimits(-distance_threshold, distance_threshold); + prism_extract.setInputPlanarHull(hull_cloud); + //prism_extract.setInputPlanarHull(hull_output); + // output_indices is set of indices which are on plane + pcl::PointIndices output_indices; + prism_extract.segment(output_indices); + std::set output_set(output_indices.indices.begin(), + output_indices.indices.end()); + Eigen::Vector3f n = convex_->getNormal(); + for (size_t i = 0; i < cloud->points.size(); i++) { + if (output_set.find(i) != output_set.end()) { + // check normal + pcl::PointNormal p = cloud->points[i]; + Eigen::Vector3f n_p = p.getNormalVector3fMap(); + if (std::abs(n.dot(n_p)) > cos(normal_threshold)) { + non_plane_indices.insert(i); + } + } + } + + + for (size_t i = 0; i < output_indices.indices.size(); i++) { + //for (size_t i = 0; i < cloud->points.size(); i++) { + int point_index = output_indices.indices[i]; + pcl::PointNormal p = cloud->points[point_index]; + Eigen::Vector3f ep = p.getVector3fMap(); + Eigen::Vector3f local_ep = inv_local_coordinates * ep; + IndexPair pair = projectLocalPointAsIndexPair(local_ep); + addIndexPair(pair); + } + return output_indices.indices.size(); + } + + size_t GridPlane::fillCellsFromPointCloud( + pcl::PointCloud::Ptr& cloud, + double distance_threshold) + { + std::set dummy; + return fillCellsFromPointCloud(cloud, distance_threshold, dummy); + } + + jsk_recognition_msgs::SimpleOccupancyGrid GridPlane::toROSMsg() + { + jsk_recognition_msgs::SimpleOccupancyGrid ros_msg; + std::vector coeff; + convex_->toCoefficients(coeff); + //ROS_INFO("coef: [%f, %f, %f, %f]", coeff[0], coeff[1], coeff[2], coeff[3]); + ros_msg.coefficients[0] = coeff[0]; + ros_msg.coefficients[1] = coeff[1]; + ros_msg.coefficients[2] = coeff[2]; + ros_msg.coefficients[3] = coeff[3]; + ros_msg.resolution = resolution_; + for (std::set::iterator it = cells_.begin(); + it != cells_.end(); + ++it) { + IndexPair pair = *it; + Eigen::Vector3f c = unprojectIndexPairAsLocalPoint(pair); + geometry_msgs::Point p; + pointFromVectorToXYZ( + c, p); + ros_msg.cells.push_back(p); + } + return ros_msg; + } + + GridPlane GridPlane::fromROSMsg( + const jsk_recognition_msgs::SimpleOccupancyGrid& rosmsg, + const Eigen::Affine3f& offset = Eigen::Affine3f::Identity()) + { + boost::mutex::scoped_lock lock(global_chull_mutex); + Plane plane = Plane(rosmsg.coefficients).transform(offset); + // ROS_INFO("[GridPlane::fromROSMsg] c: [%f, %f, %f, %f]", + // rosmsg.coefficients[0], + // rosmsg.coefficients[1], + // rosmsg.coefficients[2], + // rosmsg.coefficients[3]); + // ROS_INFO("[GridPlane::fromROSMsg] transformed c: [%f, %f, %f, %f]", + // plane.toCoefficients()[0], + // plane.toCoefficients()[1], + // plane.toCoefficients()[2], + // plane.toCoefficients()[3]); + Eigen::Affine3f plane_coords = plane.coordinates(); + Eigen::Vector3f plane_origin(plane_coords.translation()); + // ROS_INFO_EIGEN_VECTOR3("[GridPlane::fromROSMsg] plane_origin", + // plane_origin); + pcl::PointCloud::Ptr + vertices (new pcl::PointCloud); + for (size_t i = 0; i < rosmsg.cells.size(); i++) { + Eigen::Vector3f local_p(rosmsg.cells[i].x, rosmsg.cells[i].y, 0); + Eigen::Vector3f global_p = plane.coordinates() * local_p; + pcl::PointNormal p; + p.x = global_p[0]; + p.y = global_p[1]; + p.z = global_p[2]; + // ROS_INFO("[%f, %f, %f] => [%f, %f, %f]", + // local_p[0], local_p[1], local_p[2], + // global_p[0], global_p[1], global_p[2]); + vertices->points.push_back(p); + } + pcl::ConvexHull chull; + //chull.setDimension(2); + chull.setInputCloud (vertices); + pcl::PointCloud::Ptr + convex_vertices_cloud (new pcl::PointCloud); + chull.reconstruct (*convex_vertices_cloud); + + Vertices convex_vertices + = pointCloudToVertices(*convex_vertices_cloud); + ConvexPolygon::Ptr convex(new ConvexPolygon(convex_vertices)); + // Check orientation + if (!convex->isSameDirection(plane)) { + // ROS_INFO("[GridPlane::fromROSMsg] flip convex"); + //convex = boost::make_shared(convex->flipConvex()); + Vertices reversed_convex_vertices; + std::reverse_copy(convex_vertices.begin(), convex_vertices.end(), + std::back_inserter(reversed_convex_vertices)); + convex.reset(new ConvexPolygon(reversed_convex_vertices)); + } + Eigen::Vector3f convex_origin(convex->coordinates().translation()); + Eigen::Vector3f convex_normal = convex->getNormal(); + // ROS_INFO_EIGEN_VECTOR3("[GridPlane::fromROSMsg] convex_origin", + // convex_origin); + // ROS_INFO_EIGEN_VECTOR3("[GridPlane::fromROSMsg] convex_normal", + // convex_normal); + GridPlane ret(convex, rosmsg.resolution); + //ROS_INFO("resolution: %f", ret.resolution_); + ret.fillCellsFromPointCloud(vertices, 1000.0); + // ROS_INFO("cell size: %lu", ret.cells_.size()); + // ROS_INFO("original cell size: %lu", rosmsg.cells.size()); + return ret; + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/line.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/line.cpp new file mode 100644 index 00000000000..4a9d128d48d --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/line.cpp @@ -0,0 +1,205 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#define BOOST_PARAMETER_MAX_ARITY 7 +#include "jsk_recognition_utils/geo/line.h" +#include +#include + +namespace jsk_recognition_utils +{ + Line::Line(const Eigen::Vector3f& direction, const Eigen::Vector3f& origin) + : direction_ (direction.normalized()), origin_(origin) + { + + } + + void Line::getDirection(Eigen::Vector3f& output) const + { + output = direction_; + } + + Eigen::Vector3f Line::getDirection() const + { + return direction_; + } + + void Line::getOrigin(Eigen::Vector3f& output) const + { + output = origin_; + } + + Eigen::Vector3f Line::getOrigin() const + { + return origin_; + } + + void Line::foot(const Eigen::Vector3f& point, Eigen::Vector3f& output) const + { + const double alpha = computeAlpha(point); + output = alpha * direction_ + origin_; + } + + double Line::distanceToPoint( + const Eigen::Vector3f& from, Eigen::Vector3f& foot_point) const + { + foot(from, foot_point); + return (from - foot_point).norm(); + } + + double Line::distanceToPoint(const Eigen::Vector3f& from) const + { + Eigen::Vector3f foot_point; + return distanceToPoint(from, foot_point); + } + + double Line::angle(const Line& other) const + { + double dot = fabs(direction_.dot(other.direction_)); + if (dot > 1.0) { + return M_PI / 2.0; + } + else { + double theta = acos(dot); + if (theta > M_PI / 2.0) { + return M_PI / 2.0 - theta; + } + else { + return theta; + } + } + } + + bool Line::isParallel(const Line& other, double angle_threshold) const + { + return angle(other) < angle_threshold; + } + + bool Line::isPerpendicular(const Line& other, double angle_threshold) const + { + return (M_PI / 2.0 - angle(other)) < angle_threshold; + } + + bool Line::isSameDirection(const Line& other) const + { + return direction_.dot(other.direction_) > 0; + } + + Line::Ptr Line::flip() + { + Line::Ptr ret (new Line(-direction_, origin_)); + return ret; + } + + Line::Ptr Line::midLine(const Line& other) const + { + Eigen::Vector3f new_directin = (direction_ + other.direction_).normalized(); + Eigen::Vector3f new_origin; + other.foot(origin_, new_origin); + Line::Ptr ret (new Line(new_directin, (new_origin + origin_) / 2.0)); + return ret; + } + + void Line::parallelLineNormal(const Line& other, Eigen::Vector3f& output) + const + { + Eigen::Vector3f foot_point; + other.foot(origin_, foot_point); + output = origin_ - foot_point; + } + + Line::Ptr Line::fromCoefficients(const std::vector& coefficients) + { + Eigen::Vector3f p(coefficients[0], + coefficients[1], + coefficients[2]); + Eigen::Vector3f d(coefficients[3], + coefficients[4], + coefficients[5]); + Line::Ptr ret(new Line(d, p)); + return ret; + } + + double Line::distance(const Line& other) const + { + Eigen::Vector3f v12 = (other.origin_ - origin_); + Eigen::Vector3f n = direction_.cross(other.direction_); + return fabs(n.dot(v12)) / n.norm(); + } + + Line::Ptr Line::parallelLineOnAPoint(const Eigen::Vector3f& p) const + { + Line::Ptr ret (new Line(direction_, p)); + return ret; + } + + double Line::computeAlpha(const Point& p) const + { + return p.dot(direction_) - origin_.dot(direction_); + } + + PointPair Line::findEndPoints(const Vertices& points) const + { + double min_alpha = DBL_MAX; + double max_alpha = - DBL_MAX; + Point min_alpha_point, max_alpha_point; + for (size_t i = 0; i < points.size(); i++) { + Point p = points[i]; + double alpha = computeAlpha(p); + if (alpha > max_alpha) { + max_alpha_point = p; + max_alpha = alpha; + } + if (alpha < min_alpha) { + min_alpha_point = p; + min_alpha = alpha; + } + } + // ROS_INFO("min: %f", min_alpha); + // ROS_INFO("max: %f", max_alpha); + return boost::make_tuple(min_alpha_point, max_alpha_point); + } + + void Line::print() + { + ROS_INFO("d: [%f, %f, %f], p: [%f, %f, %f]", direction_[0], direction_[1], direction_[2], + origin_[0], origin_[1], origin_[2]); + } + + void Line::point(double alpha, Eigen::Vector3f& output) + { + output = alpha * direction_ + origin_; + } +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/plane.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/plane.cpp new file mode 100644 index 00000000000..8660cfd0ac5 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/plane.cpp @@ -0,0 +1,287 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#define BOOST_PARAMETER_MAX_ARITY 7 + +#include "jsk_recognition_utils/geo/plane.h" +#include "jsk_recognition_utils/geo_util.h" +#include "jsk_recognition_utils/pcl_conversion_util.h" + +namespace jsk_recognition_utils +{ + Plane::Plane(const std::vector& coefficients) + { + normal_ = Eigen::Vector3f(coefficients[0], coefficients[1], coefficients[2]); + d_ = coefficients[3] / normal_.norm(); + normal_.normalize(); + initializeCoordinates(); + } + + Plane::Plane(const boost::array& coefficients) + { + normal_ = Eigen::Vector3f(coefficients[0], coefficients[1], coefficients[2]); + d_ = coefficients[3] / normal_.norm(); + normal_.normalize(); + initializeCoordinates(); + } + + Plane::Plane(Eigen::Vector3f normal, double d) : + normal_(normal.normalized()), d_(d / normal.norm()) + { + initializeCoordinates(); + } + + Plane::Plane(Eigen::Vector3f normal, Eigen::Vector3f p) : + normal_(normal.normalized()), d_(- normal.dot(p) / normal.norm()) + { + initializeCoordinates(); + } + + + Plane::~Plane() + { + + } + + Eigen::Vector3f Plane::getPointOnPlane() + { + Eigen::Vector3f x = normal_ / (normal_.norm() * normal_.norm()) * (- d_); + return x; + } + + Plane Plane::flip() + { + return Plane(- normal_, - d_); + } + + Plane::Ptr Plane::faceToOrigin() + { + Eigen::Vector3f p = getPointOnPlane(); + Eigen::Vector3f n = getNormal(); + + if (p.dot(n) < 0) { + return Plane::Ptr (new Plane(normal_, d_)); + } + else { + return Plane::Ptr (new Plane(- normal_, - d_)); + } + } + + bool Plane::isSameDirection(const Plane& another) + { + return isSameDirection(another.normal_); + } + + bool Plane::isSameDirection(const Eigen::Vector3f& another_normal) + { + return normal_.dot(another_normal) > 0; + } + + double Plane::signedDistanceToPoint(const Eigen::Vector3f p) + { + return (normal_.dot(p) + d_); + } + + double Plane::signedDistanceToPoint(const Eigen::Vector4f p) + { + return signedDistanceToPoint(Eigen::Vector3f(p[0], p[1], p[2])); + } + + double Plane::distanceToPoint(const Eigen::Vector4f p) + { + return fabs(signedDistanceToPoint(p)); + } + + double Plane::distanceToPoint(const Eigen::Vector3f p) + { + return fabs(signedDistanceToPoint(p)); + } + + double Plane::distance(const Plane& another) + { + return fabs(fabs(d_) - fabs(another.d_)); + } + + double Plane::angle(const Eigen::Vector3f& vector) + { + double dot = normal_.dot(vector); + if (dot > 1.0) { + dot = 1.0; + } + else if (dot < -1.0) { + dot = -1.0; + } + double theta = acos(dot); + if (theta > M_PI / 2.0) { + return M_PI - theta; + } + + return acos(dot); + } + + double Plane::angle(const Plane& another) + { + double dot = normal_.dot(another.normal_); + if (dot > 1.0) { + dot = 1.0; + } + else if (dot < -1.0) { + dot = -1.0; + } + double theta = acos(dot); + if (theta > M_PI / 2.0) { + return M_PI - theta; + } + + return acos(dot); + } + + void Plane::project(const Eigen::Vector3f& p, Eigen::Vector3f& output) + { + // double alpha = - p.dot(normal_); + // output = p + alpha * normal_; + double alpha = p.dot(normal_) + d_; + //double alpha = p.dot(normal_) - d_; + output = p - alpha * normal_; + } + + void Plane::project(const Eigen::Vector3d& p, Eigen::Vector3d& output) + { + Eigen::Vector3f output_f; + project(Eigen::Vector3f(p[0], p[1], p[2]), output_f); + pointFromVectorToVector(output_f, output); + } + + void Plane::project(const Eigen::Vector3d& p, Eigen::Vector3f& output) + { + project(Eigen::Vector3f(p[0], p[1], p[2]), output); + } + + void Plane::project(const Eigen::Vector3f& p, Eigen::Vector3d& output) + { + Eigen::Vector3f output_f; + project(p, output); + pointFromVectorToVector(output_f, output); + } + + void Plane::project(const Eigen::Affine3d& pose, Eigen::Affine3d& output) + { + Eigen::Affine3f pose_f, output_f; + convertEigenAffine3(pose, pose_f); + project(pose_f, output_f); + convertEigenAffine3(output_f, output); + } + + void Plane::project(const Eigen::Affine3f& pose, Eigen::Affine3f& output) + { + Eigen::Vector3f p(pose.translation()); + Eigen::Vector3f output_p; + project(p, output_p); + Eigen::Quaternionf rot; + rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(), + coordinates().rotation() * Eigen::Vector3f::UnitZ()); + output = Eigen::Affine3f::Identity() * Eigen::Translation3f(output_p) * rot; + } + + Plane Plane::transform(const Eigen::Affine3f& transform) + { + Eigen::Affine3d transform_d; + convertEigenAffine3(transform, transform_d); + return this->transform(transform_d); + } + + Plane Plane::transform(const Eigen::Affine3d& transform) + { + Eigen::Vector4d n; + n[0] = normal_[0]; + n[1] = normal_[1]; + n[2] = normal_[2]; + n[3] = d_; + Eigen::Matrix4d m = transform.matrix(); + Eigen::Vector4d n_d = m.transpose() * n; + //Eigen::Vector4d n_dd = n_d.normalized(); + Eigen::Vector4d n_dd = n_d / sqrt(n_d[0] * n_d[0] + n_d[1] * n_d[1] + n_d[2] * n_d[2]); + return Plane(Eigen::Vector3f(n_dd[0], n_dd[1], n_dd[2]), n_dd[3]); + } + + std::vector Plane::toCoefficients() + { + std::vector ret; + toCoefficients(ret); + return ret; + } + + void Plane::toCoefficients(std::vector& output) + { + output.push_back(normal_[0]); + output.push_back(normal_[1]); + output.push_back(normal_[2]); + output.push_back(d_); + } + + Eigen::Vector3f Plane::getNormal() + { + return normal_; + } + + double Plane::getD() + { + return d_; + } + + void Plane::initializeCoordinates() + { + Eigen::Quaternionf rot; + rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), normal_); + double c = normal_[2]; + double z = 0.0; + // ax + by + cz + d = 0 + // z = - d / c (when x = y = 0) + if (c == 0.0) { // its not good + z = 0.0; + } + else { + z = - d_ / c; + } + plane_coordinates_ + = Eigen::Affine3f::Identity() * Eigen::Translation3f(0, 0, z) * rot; + } + + Eigen::Affine3f Plane::coordinates() + { + return plane_coordinates_; + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/polygon.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/polygon.cpp new file mode 100644 index 00000000000..801a6ecdabf --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/polygon.cpp @@ -0,0 +1,633 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ + +#define BOOST_PARAMETER_MAX_ARITY 7 + +#include "jsk_recognition_utils/geo/polygon.h" +#include "jsk_recognition_utils/geo_util.h" +#include +#include "jsk_recognition_utils/pcl/ear_clipping_patched.h" +#include "jsk_recognition_utils/sensor_model_utils.h" + +namespace jsk_recognition_utils +{ + Polygon Polygon::createPolygonWithSkip(const Vertices& vertices) + { + const double thr = 0.01; + Polygon not_skipped_polygon(vertices); + Vertices skipped_vertices; + for (size_t i = 0; i < vertices.size(); i++) { + size_t next_i = not_skipped_polygon.nextIndex(i); + Eigen::Vector3f v0 = vertices[i]; + Eigen::Vector3f v1 = vertices[next_i]; + if ((v1 - v0).norm() > thr) { + skipped_vertices.push_back(vertices[i]); + } + } + return Polygon(skipped_vertices); + } + + Eigen::Vector3f Polygon::centroid() + { + Eigen::Vector3f c(0, 0, 0); + if (vertices_.size() == 0) { + return c; + } + else { + for (size_t i = 0; i < vertices_.size(); i++) { + c = c + vertices_[i]; + } + return c / vertices_.size(); + } + } + + std::vector convertToPlanes( + std::vector coefficients) + { + std::vector ret; + for (size_t i = 0; i < coefficients.size(); i++) { + ret.push_back(Plane::Ptr (new Plane(coefficients[i]->values))); + } + return ret; + } + + + Polygon::Polygon(const Vertices& vertices): + Plane((vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]).normalized(), vertices[0]), + vertices_(vertices) + { + + } + + Polygon::Polygon(const Vertices& vertices, + const std::vector& coefficients): + Plane(coefficients), vertices_(vertices) + { + + } + + Polygon::~Polygon() + { + + } + + size_t Polygon::getFarestPointIndex(const Eigen::Vector3f& O) + { + double max_distance = - DBL_MAX; + size_t max_index = 0; + for (size_t i = 0; i < vertices_.size(); i++) { + Eigen::Vector3f v = vertices_[i]; + double d = (O - v).norm(); + if (max_distance < d) { + max_distance = d; + max_index = i; + } + } + return max_index; + } + + PointIndexPair Polygon::getNeighborIndex(size_t index) + { + return boost::make_tuple( + previousIndex(index), nextIndex(index)); + } + + double Polygon::area() + { + if (isTriangle()) { + return (vertices_[1] - vertices_[0]).cross(vertices_[2] - vertices_[0]).norm() / 2.0; + } + else { + std::vector triangles = decomposeToTriangles(); + double sum = 0; + for (size_t i = 0; i < triangles.size(); i++) { + sum += triangles[i]->area(); + } + return sum; + } + } + + Eigen::Vector3f Polygon::directionAtPoint(size_t i) + { + Eigen::Vector3f O = vertices_[i]; + Eigen::Vector3f A = vertices_[previousIndex(i)]; + Eigen::Vector3f B = vertices_[nextIndex(i)]; + Eigen::Vector3f OA = A - O; + Eigen::Vector3f OB = B - O; + Eigen::Vector3f n = (OA.normalized()).cross(OB.normalized()); + if (n.norm() == 0) { + // ROS_ERROR("normal is 0"); + // ROS_ERROR("O: [%f, %f, %f]", O[0], O[1], O[2]); + // ROS_ERROR("A: [%f, %f, %f]", A[0], A[1], A[2]); + // ROS_ERROR("B: [%f, %f, %f]", B[0], B[1], B[2]); + // ROS_ERROR("OA: [%f, %f, %f]", OA[0], OA[1], OA[2]); + // ROS_ERROR("OB: [%f, %f, %f]", OB[0], OB[1], OB[2]); + //exit(1); + } + return n.normalized(); + } + + bool Polygon::isTriangle() { + return vertices_.size() == 3; + } + + void Polygon::getLocalMinMax(double& min_x, double& min_y, + double& max_x, double& max_y) + { + min_x = DBL_MAX; + min_y = DBL_MAX; + max_x = - DBL_MAX; + max_y = - DBL_MAX; + + Eigen::Affine3f inv_coords = coordinates().inverse(); + for (size_t i = 0; i < vertices_.size(); i++) { + // Convert vertices into local coordinates + Eigen::Vector3f local_point = inv_coords * vertices_[i]; + min_x = ::fmin(local_point[0], min_x); + min_y = ::fmin(local_point[1], min_y); + max_x = ::fmax(local_point[0], max_x); + max_y = ::fmax(local_point[1], max_y); + } + } + + Eigen::Vector3f Polygon::randomSampleLocalPoint(boost::mt19937& random_generator) + { + // Compute min/max point + double min_x, min_y, max_x, max_y; + getLocalMinMax(min_x, min_y, max_x, max_y); + std::vector triangles = decomposeToTriangles(); + while (true) { + double x = randomUniform(min_x, max_x, random_generator); + double y = randomUniform(min_y, max_y, random_generator); + Eigen::Vector3f local_v = Eigen::Vector3f(x, y, 0); + Eigen::Vector3f v = coordinates() * local_v; + // ROS_INFO_THROTTLE(1.0, "x: %f -- %f", min_x, max_x); + // ROS_INFO_THROTTLE(1.0, "y: %f -- %f", min_y, max_y); + // ROS_INFO_THROTTLE(1.0, "sampled point: [%f, %f]", x, y); + // for (size_t i = 0; i < vertices_.size(); i++) { + // Eigen::Vector3f v = coordinates().inverse() * vertices_[i]; + // ROS_INFO("v: [%f, %f, %f]", v[0], v[1], v[2]); + // } + if (isInside(v)) { + return local_v; + } + else { + // ROS_INFO_THROTTLE(1.0, "outside"); + } + } + } + + std::vector Polygon::edges() const + { + std::vector ret; + ret.reserve(vertices_.size()); + for (size_t i = 0; i < vertices_.size() - 1; i++) { + // edge between i and i+1 + ret.push_back(Segment::Ptr(new Segment(vertices_[i], vertices_[i+1]))); + } + // edge between [-1] and [0] + ret.push_back(Segment::Ptr(new Segment(vertices_[vertices_.size() - 1], vertices_[0]))); + return ret; + } + + double Polygon::distance(const Eigen::Vector3f& point) + { + Eigen::Vector3f nearest_point; + return Polygon::distance(point, nearest_point); + } + + double Polygon::distance(const Eigen::Vector3f& point, + Eigen::Vector3f& nearest_point) + { + double distance; + nearest_point = Polygon::nearestPoint(point, distance); + return distance; + } + + Eigen::Vector3f Polygon::nearestPoint(const Eigen::Vector3f& p, + double& distance) + { + Eigen::Vector3f projected_p; + Plane::project(p, projected_p); + if (isInside(projected_p)) { + distance = (p - projected_p).norm(); + return projected_p; + } + else { + std::vector boundary_edges = edges(); + double min_distnace = DBL_MAX; + Eigen::Vector3f nearest_point; + // brute-force searching + for (size_t i = 0; i < boundary_edges.size(); i++) { + Segment::Ptr edge = boundary_edges[i]; + Eigen::Vector3f foot; + double d = edge->distance(p, foot); + if (min_distnace > d) { + nearest_point = foot; + min_distnace = d; + } + } + distance = min_distnace; + return nearest_point; + } + } + + size_t Polygon::getNumVertices() { + return vertices_.size(); + } + + Eigen::Vector3f Polygon::getVertex(size_t i) { + return vertices_[i]; + } + + Polygon::PtrPair Polygon::separatePolygon(size_t index) + { + PointIndexPair neighbor_index = getNeighborIndex(index); + Vertices triangle_vertices; + triangle_vertices.push_back(vertices_[index]); + triangle_vertices.push_back(vertices_[neighbor_index.get<1>()]); + triangle_vertices.push_back(vertices_[neighbor_index.get<0>()]); + Polygon::Ptr triangle(new Polygon(triangle_vertices)); + Vertices rest_vertices; + // do not add the points on the line + for (size_t i = neighbor_index.get<1>(); i != index;) { + // check the points on the line + if (i == neighbor_index.get<1>()) { + rest_vertices.push_back(vertices_[i]); + } + else { + if (directionAtPoint(i).norm() != 0.0) { + rest_vertices.push_back(vertices_[i]); + } + else { + ROS_ERROR("removed: %lu", i); + } + } + i = nextIndex(i); + } + Polygon::Ptr rest(new Polygon(rest_vertices)); + return boost::make_tuple( + triangle, rest); + } + + bool Polygon::isPossibleToRemoveTriangleAtIndex( + size_t index, + const Eigen::Vector3f& direction) + { + Polygon::PtrPair candidate = separatePolygon(index); + Polygon::Ptr triangle_candidate = candidate.get<0>(); + Polygon::Ptr rest_candidate = candidate.get<1>(); + // first check direction + Eigen::Vector3f the_direction = directionAtPoint(index); + //ROS_INFO("direction: [%f, %f, %f]", the_direction[0], the_direction[1], the_direction[2]); + if (the_direction.norm() == 0.0) { + ROS_ERROR("malformed polygon"); + exit(1); + } + if (direction.dot(the_direction) < 0) { +#ifdef DEBUG_GEO_UTIL + ROS_INFO("triangle is not same direction"); + ROS_INFO("direction: [%f, %f, %f]", direction[0], direction[1], direction[2]); + ROS_INFO("the_direction: [%f, %f, %f]", + the_direction[0], + the_direction[1], + the_direction[2]); + for (size_t i = 0; i < vertices_.size(); i++) { + Eigen::Vector3f v = directionAtPoint(i); + ROS_INFO("the_direction[%lu]: [%f, %f, %f]", + i, v[0], v[1], v[2]); + // other direction + } +#endif + return false; + } + else { + //return true; + // second, check the triangle includes the rest of points or not + for (size_t i = 0; i < rest_candidate->vertices_.size(); i++) { + if (i == 0 || i == rest_candidate->vertices_.size() - 1) { + continue; // do not check the first and the last point + } + else { + Eigen::Vector3f P = rest_candidate->getVertex(i); + Eigen::Vector3f A = triangle_candidate->getVertex(0); + Eigen::Vector3f B = triangle_candidate->getVertex(1); + Eigen::Vector3f C = triangle_candidate->getVertex(2); + Eigen::Vector3f CA = A - C; + Eigen::Vector3f BC = C - B; + Eigen::Vector3f AB = B - A; + Eigen::Vector3f AP = P - A; + Eigen::Vector3f BP = P - B; + Eigen::Vector3f CP = P - C; + Eigen::Vector3f Across = CA.normalized().cross(AP.normalized()).normalized(); + Eigen::Vector3f Bcross = AB.normalized().cross(BP.normalized()).normalized(); + Eigen::Vector3f Ccross = BC.normalized().cross(CP.normalized()).normalized(); +#ifdef DEBUG_GEO_UTIL + ROS_INFO("P: [%f, %f, %f]", P[0], P[1], P[2]); + ROS_INFO("A: [%f, %f, %f]", A[0], A[1], A[2]); + ROS_INFO("B: [%f, %f, %f]", B[0], B[1], B[2]); + ROS_INFO("C: [%f, %f, %f]", C[0], C[1], C[2]); + ROS_INFO("Across: [%f, %f, %f]", Across[0], Across[1], Across[2]); + ROS_INFO("Bcross: [%f, %f, %f]", Bcross[0], Bcross[1], Bcross[2]); + ROS_INFO("Ccross: [%f, %f, %f]", Ccross[0], Ccross[1], Ccross[2]); + ROS_INFO("Across-Bcross: %f", Across.dot(Bcross)); + ROS_INFO("Bcross-Ccross: %f", Bcross.dot(Ccross)); + ROS_INFO("Ccross-Across: %f", Ccross.dot(Across)); +#endif + if (((Across.dot(Bcross) > 0 && + Bcross.dot(Ccross) > 0 && + Ccross.dot(Across) > 0) || + (Across.dot(Bcross) < 0 && + Bcross.dot(Ccross) < 0 && + Ccross.dot(Across) < 0))) { + // ROS_ERROR("%lu -- %lu is inside", index, i); + return false; + } + // ConvexPolygon convex_triangle(triangle_candidate->vertices_); + // if (convex_triangle.isInside(v)) { + // //ROS_INFO("vertices is inside of the polygon"); + // return false; + // } + } + } + return true; + } + } + + void Polygon::transformBy(const Eigen::Affine3d& transform) + { + Eigen::Affine3f float_affine; + convertEigenAffine3(transform, float_affine); + transformBy(float_affine); + } + + void Polygon::transformBy(const Eigen::Affine3f& transform) + { + // 1. clear cached_triangles_ + // 2. transform vertices_ + // 3. update normal_ and d_ + // 3. update plane_coordinates_ + cached_triangles_.clear(); + for (size_t i = 0; i < vertices_.size(); i++) { + vertices_[i] = transform * vertices_[i]; + } + // compute normal and d + normal_ = (vertices_[1] - vertices_[0]).cross(vertices_[2] - vertices_[0]).normalized(); + d_ = - normal_.dot(vertices_[0]) / normal_.norm(); + initializeCoordinates(); + } + + bool Polygon::maskImage(const jsk_recognition_utils::CameraDepthSensor& model, + cv::Mat& image) const + { + std::vector projected_vertices + = project3DPointstoPixel(model.getPinholeCameraModel(), vertices_); + bool all_outside = true; + // check some of vertices is inside of FoV + for (size_t i = 0; i < projected_vertices.size(); i++) { + if (model.isInside(projected_vertices[i])) { + all_outside = false; + } + } + image = model.image(CV_8UC1); + // check all the v is positive + for (size_t i = 0; i < vertices_.size(); i++) { + if (vertices_[i][2] < 0) { + return false; + } + } + const cv::Point* element_points[1] = {&projected_vertices[0]}; + int number_of_points = (int)projected_vertices.size(); + // Is it should be cv::fillPoly? + cv::fillPoly(image, + element_points, + &number_of_points, + 1, + cv::Scalar(255)); + return !all_outside; + } + + void Polygon::drawLineToImage(const jsk_recognition_utils::CameraDepthSensor& model, + cv::Mat& image, + const cv::Scalar& color, + const int line_width) const + { + std::vector projected_vertices + = project3DPointstoPixel(model.getPinholeCameraModel(), vertices_); + + for (size_t i = 0; i < projected_vertices.size() - 1; i++) { + cv::Point from = projected_vertices[i]; + cv::Point to = projected_vertices[i+1]; + if (model.isInside(from) || model.isInside(to)) { + cv::line(image, from, to, color, line_width); + } + } + cv::Point from = projected_vertices[projected_vertices.size() - 1]; + cv::Point to = projected_vertices[0]; + if (model.isInside(from) || model.isInside(to)) { + cv::line(image, from, to, color, line_width); + } + } + + bool Polygon::isConvex() + { +#ifdef DEBUG_GEO_UTIL + for (size_t i = 0; i < getNumVertices(); i++) { + Eigen::Vector3f n = directionAtPoint(i); + ROS_INFO("n[%lu] [%f, %f, %f]", i, n[0], n[1], n[2]); + } +#endif + Eigen::Vector3f n0 = directionAtPoint(0); + for (size_t i = 1; i < getNumVertices(); i++) { + Eigen::Vector3f n = directionAtPoint(i); + if (n0.dot(n) < 0) { + return false; + } + } + return true; + } + + std::vector Polygon::decomposeToTriangles() + { + if (cached_triangles_.size() != 0) { + return cached_triangles_; + } + std::vector ret; + + // if this polygon is triangle, return immediately + if (isTriangle()) { + ret.push_back(Polygon::Ptr( new Polygon(*this))); + return ret; + } + + pcl::EarClippingPatched clip; + // convert + pcl::PolygonMesh::Ptr input_mesh (new pcl::PolygonMesh); + pcl::PCLPointCloud2 mesh_cloud; + pcl::PointCloud mesh_pcl_cloud; + boundariesToPointCloud(mesh_pcl_cloud); + std::vector mesh_vertices(1); + for (size_t i = 0; i < vertices_.size(); i++) { + mesh_vertices[0].vertices.push_back(i); + } + //mesh_vertices[0].vertices.push_back(0); // close + mesh_pcl_cloud.height = 1; + mesh_pcl_cloud.width = mesh_pcl_cloud.points.size(); + pcl::toPCLPointCloud2(mesh_pcl_cloud, mesh_cloud); + + input_mesh->polygons = mesh_vertices; + input_mesh->cloud = mesh_cloud; + clip.setInputMesh(input_mesh); + pcl::PolygonMesh output; + clip.process(output); + assert(output.polygons.size() != 0); + // convert to Polygon instances + for (size_t i = 0; i < output.polygons.size(); i++) { + pcl::Vertices output_polygon_vertices = output.polygons[i]; + Vertices vs(output_polygon_vertices.vertices.size()); + for (size_t j = 0; j < output_polygon_vertices.vertices.size(); j++) { + pcl::PointXYZ p + = mesh_pcl_cloud.points[output_polygon_vertices.vertices[j]]; + Eigen::Vector3f v; + pointFromXYZToVector(p, v); + vs[j] = v; + } + ret.push_back(Polygon::Ptr(new Polygon(vs, toCoefficients()))); + } + cached_triangles_ = ret; + return ret; + } + + Eigen::Vector3f Polygon::getNormalFromVertices() + { + if (vertices_.size() >= 3) { + return (vertices_[1] - vertices_[0]).cross(vertices_[2] - vertices_[0]).normalized(); + } + else { + ROS_ERROR("the number of vertices is not enough"); + return Eigen::Vector3f(0, 0, 0); + } + } + + size_t Polygon::previousIndex(size_t i) + { + if (i == 0) { + return vertices_.size() - 1; + } + else { + return i - 1; + } + } + + size_t Polygon::nextIndex(size_t i) + { + if (i == vertices_.size() - 1) { + return 0; + } + else { + return i + 1; + } + } + + Polygon Polygon::fromROSMsg(const geometry_msgs::Polygon& polygon) + { + Vertices vertices; + for (size_t i = 0; i < polygon.points.size(); i++) { + Eigen::Vector3f v; + pointFromXYZToVector( + polygon.points[i], v); + vertices.push_back(v); + } + return Polygon(vertices); + } + + Polygon::Ptr Polygon::fromROSMsgPtr(const geometry_msgs::Polygon& polygon) + { + Vertices vertices; + for (size_t i = 0; i < polygon.points.size(); i++) { + Eigen::Vector3f v; + pointFromXYZToVector( + polygon.points[i], v); + vertices.push_back(v); + } + return Polygon::Ptr(new Polygon(vertices)); + } + + std::vector Polygon::fromROSMsg(const jsk_recognition_msgs::PolygonArray& msg, + const Eigen::Affine3f& offset) + { + std::vector ret; + for (size_t i = 0; i < msg.polygons.size(); i++) { + Polygon::Ptr polygon = Polygon::fromROSMsgPtr(msg.polygons[i].polygon); + polygon->transformBy(offset); + ret.push_back(polygon); + } + return ret; + } + + bool Polygon::isInside(const Eigen::Vector3f& p) + { + if (isTriangle()) { + Eigen::Vector3f A = vertices_[0]; + Eigen::Vector3f B = vertices_[1]; + Eigen::Vector3f C = vertices_[2]; + // Eigen::Vector3f cross0 = (A - C).cross(p - A); + // Eigen::Vector3f cross1 = (B - A).cross(p - B); + // Eigen::Vector3f cross2 = (C - B).cross(p - C); + + Eigen::Vector3f cross0 = (B - A).cross(p - A); + Eigen::Vector3f cross1 = (C - B).cross(p - B); + Eigen::Vector3f cross2 = (A - C).cross(p - C); + if (cross0.dot(cross1) >= 0 && + cross1.dot(cross2) >= 0) { + return true; + } + else { + return false; + } + } + else { + std::vector triangles = decomposeToTriangles(); + for (size_t i = 0; i < triangles.size(); i++) { + if (triangles[i]->isInside(p)) { + return true; + } + } + return false; + } + } + + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/polyline.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/polyline.cpp new file mode 100644 index 00000000000..e3fa192f2dd --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/polyline.cpp @@ -0,0 +1,181 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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/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. + *********************************************************************/ + +#define BOOST_PARAMETER_MAX_ARITY 7 +#include "jsk_recognition_utils/geo/polyline.h" +#include +#include + +namespace jsk_recognition_utils +{ + PolyLine::PolyLine(const std::vector < Eigen::Vector3f > &points) : Line(points[points.size()-1] - points[0], points[0]) + { + int n = points.size(); + segments.resize(n-1); + for (int i = 0; i < n-1; i++) { + Segment::Ptr ln(new Segment(points[i], points[i+1])); + segments[i] = ln; + } + } + + Segment::Ptr PolyLine::at(int index) const + { + return segments.at(index); + } + + double PolyLine::distanceWithInfo(const Eigen::Vector3f& from, + Eigen::Vector3f& foot_point, + double& distance_to_goal, + int& foot_index, + double& foot_alpha) const + { + double min_len = DBL_MAX; + Eigen::Vector3f point; + double from_start_to_foot = 0; + double distance_from_start = 0; + + for(int i = 0; i < segments.size(); i++) { + double to_goal; + double dist = segments[i]->distanceWithInfo(from, point, to_goal); + if (dist < min_len) { + min_len = dist; + foot_point = point; + foot_index = i; + foot_alpha = (segments[i]->length() - to_goal); + from_start_to_foot = distance_from_start + foot_alpha; + } + distance_from_start += segments[i]->length(); + } + distance_to_goal = distance_from_start - from_start_to_foot; + return min_len; + } + + double PolyLine::distance(const Eigen::Vector3f& from) const + { + double gl, alp; + int idx; + Eigen::Vector3f p; + distanceWithInfo(from, p, gl, idx, alp); + } + + double PolyLine::distance(const Eigen::Vector3f& from, + Eigen::Vector3f& foot_point) const + { + double gl, alp; + int idx; + distanceWithInfo(from, foot_point, gl, idx, alp); + } + + void PolyLine::getDirection(int index, Eigen::Vector3f& output) const + { + segments[index]->getDirection(output); + } + Eigen::Vector3f PolyLine::getDirection(int index) const + { + Eigen::Vector3f dir; + getDirection(index, dir); + return dir; + } + + double PolyLine::length() const + { + double distance_from_start = 0; + for(int i = 0; i < segments.size(); i++) { + distance_from_start += segments[i]->length(); + } + return distance_from_start; + } + + PolyLine::Ptr PolyLine::flipPolyLine() const + { + PolyLine::Ptr ret; + return ret; + } + + void PolyLine::toMarker(visualization_msgs::Marker& marker) const + { + marker.type = visualization_msgs::Marker::LINE_STRIP; + + marker.scale.x = 0.02; // line width + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0; + marker.pose.orientation.y = 0; + marker.pose.orientation.z = 0; + marker.pose.orientation.w = 1; + + marker.points.resize(0); + for(int i = 0; i < segments.size(); i++) { + Eigen::Vector3f p; + segments[i]->getOrigin(p); + geometry_msgs::Point pt; + pt.x = p[0]; + pt.y = p[1]; + pt.z = p[2]; + marker.points.push_back(pt); + } + { + Eigen::Vector3f p; + segments[segments.size() - 1]->getEnd(p); + geometry_msgs::Point pt; + pt.x = p[0]; + pt.y = p[1]; + pt.z = p[2]; + marker.points.push_back(pt); + } + } + + std::ostream& operator<<(std::ostream& os, const PolyLine& pl) + { + os << "[" << pl.origin_[0]; + os << ", " << pl.origin_[1]; + os << ", " << pl.origin_[2] << "]"; + + for (int i = 0; i < pl.segments.size(); i++) { + Eigen::Vector3f p; + pl.segments[i]->getEnd(p); + os << " -- [" << p[0]; + os << ", " << p[1]; + os << ", " << p[2] << "]"; + } + return os; + } +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/segment.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/segment.cpp new file mode 100644 index 00000000000..f8559ac50ac --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo/segment.cpp @@ -0,0 +1,225 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ + +#define BOOST_PARAMETER_MAX_ARITY 7 + +#include "jsk_recognition_utils/geo/segment.h" +#include "jsk_recognition_utils/geo_util.h" +namespace jsk_recognition_utils +{ + Segment::Segment(const Eigen::Vector3f& from, const Eigen::Vector3f to): + Line(to - from, from), to_(to), length_((to - from).norm()) + { + } + + double Segment::dividingRatio(const Eigen::Vector3f& point) const + { + if (to_[0] != origin_[0]) { + return (point[0] - origin_[0]) / (to_[0] - origin_[0]); + } + else if (to_[1] != origin_[1]) { + return (point[1] - origin_[1]) / (to_[1] - origin_[1]); + } + else { + return (point[2] - origin_[2]) / (to_[2] - origin_[2]); + } + } + + void Segment::foot(const Eigen::Vector3f& from, Eigen::Vector3f& output) const + { + Eigen::Vector3f foot_point; + Line::foot(from, foot_point); + double r = dividingRatio(foot_point); + if (r < 0.0) { + output = origin_; + } + else if (r > 1.0) { + output = to_; + } + else { + output = foot_point; + } + } + + double Segment::distance(const Eigen::Vector3f& point) const + { + Eigen::Vector3f foot_point; + return distance(point, foot_point); + } + + double Segment::distance(const Eigen::Vector3f& point, + Eigen::Vector3f& foot_point) const + { + foot(point, foot_point); + return (foot_point - point).norm(); + } + + bool Segment::intersect(Plane& plane, Eigen::Vector3f& point) const + { + double x = - (plane.getNormal().dot(origin_) + plane.getD()) / (plane.getNormal().dot(direction_)); + point = direction_ * x + origin_; + double r = dividingRatio(point); + return 0 <= r && r <= 1.0; + } + + void Segment::midpoint(Eigen::Vector3f& midpoint) const + { + midpoint = (origin_ + to_) * 0.5; + } + + std::ostream& operator<<(std::ostream& os, const Segment& seg) + { + os << "[" << seg.origin_[0] << ", " << seg.origin_[1] << ", " << seg.origin_[2] << "] -- " + << "[" << seg.to_[0] << ", " << seg.to_[1] << ", " << seg.to_[2] << "]"; + } + + void Segment::getEnd(Eigen::Vector3f& output) const + { + output = to_; + } + + Eigen::Vector3f Segment::getEnd() const + { + return to_; + } + + double Segment::distanceWithInfo(const Eigen::Vector3f& from, + Eigen::Vector3f& foot_point, + double &distance_to_goal) const + { + const double alpha = computeAlpha(from); + + if (alpha >= 0 && alpha <= length_) { + // foot on the line + foot_point = alpha * direction_ + origin_; + distance_to_goal = length_ - alpha; + } else if (alpha < 0) { + // foot out of the line + foot_point = origin_; + distance_to_goal = length_; + } else { + foot_point = to_; + distance_to_goal = 0; + } + return (from - foot_point).norm(); + } + + Segment::Ptr Segment::flipSegment() const + { + Segment::Ptr ret (new Segment(to_, origin_)); + return ret; + } + + double Segment::length() const + { + return length_; + } + + void Segment::toMarker(visualization_msgs::Marker& marker) const + { + marker.type = visualization_msgs::Marker::ARROW;// + + geometry_msgs::Point st; + geometry_msgs::Point ed; + st.x = origin_[0]; + st.y = origin_[1]; + st.z = origin_[2]; + ed.x = to_[0]; + ed.y = to_[1]; + ed.z = to_[2]; + + marker.points.push_back(st); + marker.points.push_back(ed); + + marker.scale.x = 0.012; + marker.scale.y = 0.02; + marker.color.a = 1; + marker.color.r = 1; + marker.color.g = 1; + marker.color.b = 0; + } + + bool Segment::isCross (const Line &ln, double distance_threshold) const + { + Eigen::Vector3f ln_origin = ln.getOrigin(); + Eigen::Vector3f ln_direction = ln.getDirection(); + Eigen::Vector3f v12 = (ln_origin - origin_); + double n1n2 = ln_direction.dot(direction_); + if (fabs(n1n2) < 1e-20) { // parallel + return false; + } + double alp1 = (ln_direction.dot(v12) - (n1n2 * direction_.dot(v12))) / (1 - n1n2 * n1n2); + double alp2 = ((n1n2 * ln_direction.dot(v12)) - direction_.dot(v12)) / (1 - n1n2 * n1n2); + + if (// alp1 >= 0 && alp1 <= ln.length() && + alp2 >= 0 && alp2 <= length_) { + Eigen::Vector3f p1 = alp1 * ln_direction + ln_origin; + Eigen::Vector3f p2 = alp2 * direction_ + origin_; + if ((p1 - p2).norm() < distance_threshold) { + return true; + } else { + return false; + } + } + + return false; + } + + bool Segment::isCross (const Segment &ln, double distance_threshold) const + { + Eigen::Vector3f ln_origin = ln.getOrigin(); + Eigen::Vector3f ln_direction = ln.getDirection(); + Eigen::Vector3f v12 = (ln_origin - origin_); + double n1n2 = ln_direction.dot(direction_); + if (fabs(n1n2) < 1e-20) { // parallel + return false; + } + double alp1 = (ln_direction.dot(v12) - (n1n2 * direction_.dot(v12))) / (1 - n1n2 * n1n2); + double alp2 = ((n1n2 * ln_direction.dot(v12)) - direction_.dot(v12)) / (1 - n1n2 * n1n2); + + if (alp1 >= 0 && alp1 <= ln.length() && + alp2 >= 0 && alp2 <= length_) { + Eigen::Vector3f p1 = alp1 * ln_direction + ln_origin; + Eigen::Vector3f p2 = alp2 * direction_ + origin_; + if ((p1 - p2).norm() < distance_threshold) { + return true; + } else { + return false; + } + } + + return false; + } +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo_util.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo_util.cpp new file mode 100644 index 00000000000..eebeb7bfba9 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/geo_util.cpp @@ -0,0 +1,66 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ +#define BOOST_PARAMETER_MAX_ARITY 7 +#include "jsk_recognition_utils/geo_util.h" +#include "jsk_recognition_utils/pcl_conversion_util.h" +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include "jsk_recognition_utils/sensor_model_utils.h" + +// #define DEBUG_GEO_UTIL +namespace jsk_recognition_utils +{ + Eigen::Quaternionf rotFrom3Axis(const Eigen::Vector3f& ex, + const Eigen::Vector3f& ey, + const Eigen::Vector3f& ez) + { + Eigen::Matrix3f rot; + rot.col(0) = ex.normalized(); + rot.col(1) = ey.normalized(); + rot.col(2) = ez.normalized(); + return Eigen::Quaternionf(rot); + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/grid_index.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/grid_index.cpp new file mode 100644 index 00000000000..affbf9613d8 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/grid_index.cpp @@ -0,0 +1,55 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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 "jsk_recognition_utils/grid_index.h" + +namespace jsk_recognition_utils +{ + GridIndex::GridIndex(): x(0), y(0) + { + + } + + GridIndex::GridIndex(int _x, int _y): x(_x), y(_y) + { + + } + + GridIndex::~GridIndex() + { + + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/grid_line.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/grid_line.cpp new file mode 100644 index 00000000000..89a5b9da06e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/grid_line.cpp @@ -0,0 +1,81 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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 "jsk_recognition_utils/grid_line.h" + +namespace jsk_recognition_utils +{ + GridLine::GridLine(const pcl::PointXYZRGB a, const pcl::PointXYZRGB b) + : from(a.getVector3fMap()), to(b.getVector3fMap()), d_(from - to) + { + d_.normalized(); + } + + GridLine::~GridLine() + { + + } + + // bool GridLine::penetrateGrid(const Eigen::Vector3f A, + // const Eigen::Vector3f B, + // const Eigen::Vector3f C, + // const Eigen::Vector3f D) + // { + // Eigen::Vector3f Across = (A - from).cross(d_); + // Eigen::Vector3f Bcross = (B - from).cross(d_); + // Eigen::Vector3f Ccross = (C - from).cross(d_); + // Eigen::Vector3f Dcross = (D - from).cross(d_); + // bool ab_direction = Across.dot(Bcross) < 0; + // bool ac_direction = Across.dot(Ccross) < 0; + // bool ad_direction = Across.dot(Dcross) < 0; + // if ((ab_direction == ac_direction) && (ab_direction == ad_direction)) { + // return false; + // } + // else { + // return true; + // } + // } + + bool GridLine::penetrateGrid(const pcl::PointXYZRGB& A, + const pcl::PointXYZRGB& B, + const pcl::PointXYZRGB& C, + const pcl::PointXYZRGB& D) + { + return penetrateGrid(A.getVector3fMap(), + B.getVector3fMap(), + C.getVector3fMap(), + D.getVector3fMap()); + } +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/grid_map.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/grid_map.cpp new file mode 100644 index 00000000000..dd8c20c4ae9 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/grid_map.cpp @@ -0,0 +1,672 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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. + *********************************************************************/ +#define BOOST_PARAMETER_MAX_ARITY 7 +#include "jsk_recognition_utils/grid_map.h" +#include +#include +#include "jsk_recognition_utils/geo_util.h" +#include +#include +#include "jsk_recognition_utils/pcl_conversion_util.h" +#include +#include +//#define DEBUG_GRID_MAP + +namespace jsk_recognition_utils +{ + GridMap::GridMap(double resolution, const std::vector& coefficients): + resolution_(resolution), vote_(0) + { + normal_[0] = -coefficients[0]; + normal_[1] = -coefficients[1]; + normal_[2] = -coefficients[2]; + d_ = -coefficients[3]; + if (normal_.norm() != 1.0) { + d_ = d_ / normal_.norm(); + normal_.normalize(); + } + O_ = - d_ * normal_; + // decide ex_ and ey_ + Eigen::Vector3f u(1, 0, 0); + if (normal_ == u) { + u[0] = 0; u[1] = 1; u[2] = 0; + } + ey_ = normal_.cross(u).normalized(); + ex_ = ey_.cross(normal_).normalized(); + } + + GridMap::~GridMap() + { + + } + + GridIndex::Ptr GridMap::registerIndex(const int x, const int y) + { + ColumnIterator it = data_.find(x); + if (it != data_.end()) { + (it->second).insert(y); + } + else { + RowIndices new_row; + new_row.insert(y); + data_[x] = new_row; + } + GridIndex::Ptr ret(new GridIndex(x, y)); + return ret; + } + + GridIndex::Ptr GridMap::registerIndex(const GridIndex::Ptr& index) + { + return registerIndex(index->x, index->y); + } + + void GridMap::registerPoint(const pcl::PointXYZRGB& point) + { + GridIndex::Ptr index (new GridIndex()); + pointToIndex(point, index); + // check duplication + registerIndex(index); + } + + std::vector GridMap::registerLine(const pcl::PointXYZRGB& from, + const pcl::PointXYZRGB& to) + { +#ifdef DEBUG_GRID_MAP + std::cout << "newline" << std::endl; +#endif + std::vector added_indices; + //GridLine::Ptr new_line (new GridLine(from, to)); + //lines_.push_back(new_line); + // count up all the grids which the line penetrates + + // 1. convert to y = ax + b style equation. + // 2. move x from the start index to the end index and count up the y range + // if it cannot be convert to y = ax + b style, it means the equation + // is represented as x = c style. + double from_x = from.getVector3fMap().dot(ex_) / resolution_; + double from_y = from.getVector3fMap().dot(ey_) / resolution_; + double to_x = to.getVector3fMap().dot(ex_) / resolution_; + double to_y = to.getVector3fMap().dot(ey_) / resolution_; +#ifdef DEBUG_GRID_MAP + std::cout << "registering (" << (int)from_x << ", " << (int)from_y << ")" << std::endl; + std::cout << "registering (" << (int)to_x << ", " << (int)to_y << ")" << std::endl; +#endif + added_indices.push_back(registerIndex(from_x, from_y)); + added_indices.push_back(registerIndex(to_x, to_y)); + if (from_x != to_x) { + double a = (to_y - from_y) / (to_x - from_x); + double b = - a * from_x + from_y; +#ifdef DEBUG_GRID_MAP + std::cout << "a: " << a << std::endl; +#endif + if (a == 0.0) { +#ifdef DEBUG_GRID_MAP + std::cout << "parallel to x" << std::endl; +#endif + int from_int_x = (int)from_x; + int to_int_x = (int)to_x; + int int_y = (int)from_y; + if (from_int_x > to_int_x) { + std::swap(from_int_x, to_int_x); + } + for (int ix = from_int_x; ix < to_int_x; ++ix) { + added_indices.push_back(registerIndex(ix, int_y)); +#ifdef DEBUG_GRID_MAP + std::cout << "registering (" << ix << ", " << int_y << ")" << std::endl; +#endif + } + } + else if (fabs(a) < 1.0) { +#ifdef DEBUG_GRID_MAP + std::cout << "based on x" << std::endl; +#endif + int from_int_x = (int)from_x; + int to_int_x = (int)to_x; + if (from_int_x > to_int_x) { + std::swap(from_int_x, to_int_x); + } + + for (int ix = from_int_x; ix < to_int_x; ++ix) { + double y = a * ix + b; + added_indices.push_back(registerIndex(ix, (int)y)); +#ifdef DEBUG_GRID_MAP + std::cout << "registering (" << ix << ", " << (int)y << ")" << std::endl; +#endif + } + } + else { +#ifdef DEBUG_GRID_MAP + std::cout << "based on y" << std::endl; +#endif + int from_int_y = (int)from_y; + int to_int_y = (int)to_y; + if (from_int_y > to_int_y) { + std::swap(from_int_y, to_int_y); + } + + for (int iy = from_int_y; iy < to_int_y; ++iy) { + double x = iy / a - b / a; + added_indices.push_back(registerIndex((int)x, iy)); +#ifdef DEBUG_GRID_MAP + std::cout << "registering (" << (int)x << ", " << iy << ")" << std::endl; +#endif + } + } + + } + else { +#ifdef DEBUG_GRID_MAP + std::cout << "parallel to y" << std::endl; +#endif + // the line is parallel to y + int from_int_y = (int)from_y; + int to_int_y = (int)to_y; + int int_x = (int)from_x; + if (from_int_y > to_int_y) { + std::swap(from_int_y, to_int_y); + } + for (int iy = from_int_y; iy < to_int_y; ++iy) { + added_indices.push_back(registerIndex(int_x, iy)); +#ifdef DEBUG_GRID_MAP + std::cout << "registering (" << int_x << ", " << (int)iy << ")" << std::endl; +#endif + } + } + return added_indices; + } + + void GridMap::registerPointCloud(pcl::PointCloud::Ptr cloud) + { + for (size_t i = 0; i < cloud->points.size(); i++) { + registerPoint(cloud->points[i]); + //ROS_INFO("registered point: [%f, %f, %f]", cloud->points[i].x, cloud->points[i].y, cloud->points[i].z); + } + } + + void GridMap::pointToIndex(const pcl::PointXYZRGB& point, GridIndex::Ptr index) + { + pointToIndex(point.getVector3fMap(), index); + } + + void GridMap::pointToIndex(const Eigen::Vector3f& p, GridIndex::Ptr index) + { + index->x = (p - O_).dot(ex_) / resolution_; + index->y = (p - O_).dot(ey_) / resolution_; + } + + void GridMap::gridToPoint(GridIndex::Ptr index, Eigen::Vector3f& pos) + { + gridToPoint(*index, pos); + } + + void GridMap::gridToPoint(const GridIndex& index, Eigen::Vector3f& pos) + { + //pos = resolution_ * (index.x * ex_ + index.y * ey_) + O_; + pos = resolution_ * ((index.x + 0.5) * ex_ + (index.y + 0.5) * ey_) + O_; + } + + void GridMap::gridToPoint2(const GridIndex& index, Eigen::Vector3f& pos) + { + //pos = resolution_ * ((index.x - 0.5) * ex_ + (index.y - 0.5) * ey_) + O_; + pos = resolution_ * ((index.x - 0.0) * ex_ + (index.y - 0.0) * ey_) + O_; + } + + + bool GridMap::getValue(const int x, const int y) + { + // check line + // for (size_t i = 0; i < lines_.size(); i++) { + // GridLine::Ptr line = lines_[i]; + // Eigen::Vector3f A, B, C, D; + // gridToPoint2(GridIndex(x, y), A); + // gridToPoint2(GridIndex(x + 1, y), B); + // gridToPoint2(GridIndex(x + 1, y + 1), C); + // gridToPoint2(GridIndex(x, y + 1), D); + // bool penetrate = line->penetrateGrid(A, B, C, D); + // if (penetrate) { + // // // printf("(%lf, %lf, %lf) - (%lf, %lf, %lf) penetrate (%d, %d)\n", + // // // line->from[0],line->from[1],line->from[2], + // // // line->to[0],line->to[1],line->to[2], + // // // x, y); + // // //std::cout << "penetrate" + // return true; + // } + // } + + ColumnIterator it = data_.find(x); + if (it == data_.end()) { + return false; + } + else { + RowIndices c = it->second; + if (c.find(y) == c.end()) { + return false; + } + else { + return true; + } + } + } + + bool GridMap::getValue(const GridIndex& index) + { + return getValue(index.x, index.y); + } + + bool GridMap::getValue(const GridIndex::Ptr& index) + { + return getValue(*index); + } + + void GridMap::fillRegion(const GridIndex::Ptr start, std::vector& output) + { +#ifdef DEBUG_GRID_MAP + std::cout << "filling " << start->x << ", " << start->y << std::endl; +#endif + output.push_back(start); + registerIndex(start); +#ifdef DEBUG_GRID_MAP + if (abs(start->x) > 100 || abs(start->y) > 100) { + //exit(1); + std::cout << "force to quit" << std::endl; + for (size_t i = 0; i < lines_.size(); i++) { + GridLine::Ptr line = lines_[i]; + Eigen::Vector3f from = line->from; + Eigen::Vector3f to = line->to; +#ifdef DEBUG_GRID_MAP + std::cout << "line[" << i << "]: " + << "[" << from[0] << ", " << from[1] << ", " << from[2] << "] -- " + << "[" << to[0] << ", " << to[1] << ", " << to[2] << std::endl; +#endif + } + return; // force to quit + } +#endif + GridIndex U(start->x, start->y + 1), + D(start->x, start->y - 1), + R(start->x + 1, start->y), + L(start->x - 1, start->y); + + if (!getValue(U)) { + fillRegion(boost::make_shared(U), output); + } + if (!getValue(L)) { + fillRegion(boost::make_shared(L), output); + } + if (!getValue(R)) { + fillRegion(boost::make_shared(R), output); + } + if (!getValue(D)) { + fillRegion(boost::make_shared(D), output); + } + + } + + void GridMap::fillRegion(const Eigen::Vector3f& start, std::vector& output) + { + GridIndex::Ptr start_index (new GridIndex); + pointToIndex(start, start_index); + fillRegion(start_index, output); + } + + void GridMap::indicesToPointCloud(const std::vector& indices, + pcl::PointCloud::Ptr cloud) + { + for (size_t i = 0; i < indices.size(); i++) { + GridIndex::Ptr index = indices[i]; + Eigen::Vector3f point; + pcl::PointXYZRGB new_point; + gridToPoint(index, point); + new_point.x = point[0]; + new_point.y = point[1]; + new_point.z = point[2]; + cloud->points.push_back(new_point); + } + } + + void GridMap::originPose(Eigen::Affine3f& output) + { + Eigen::Matrix3f rot_mat; + rot_mat.col(0) = Eigen::Vector3f(ex_[0], ex_[1], ex_[2]); + rot_mat.col(1) = Eigen::Vector3f(ey_[0], ey_[1], ey_[2]); + rot_mat.col(2) = Eigen::Vector3f(normal_[0], normal_[1], normal_[2]); + ROS_DEBUG("O: [%f, %f, %f]", O_[0], O_[1], O_[2]); + ROS_DEBUG("ex: [%f, %f, %f]", ex_[0], ex_[1], ex_[2]); + ROS_DEBUG("ey: [%f, %f, %f]", ey_[0], ey_[1], ey_[2]); + ROS_DEBUG("normal: [%f, %f, %f]", normal_[0], normal_[1], normal_[2]); + output = Eigen::Translation3f(O_) * Eigen::Quaternionf(rot_mat); + } + + void GridMap::originPose(Eigen::Affine3d& output) + { + Eigen::Affine3f float_affine; + originPose(float_affine); + convertEigenAffine3(float_affine, output); + } + + void GridMap::toMsg(jsk_recognition_msgs::SparseOccupancyGrid& grid) + { + grid.resolution = resolution_; + // compute origin POSE from O and normal_, d_ + Eigen::Affine3d plane_pose; + originPose(plane_pose); + tf::poseEigenToMsg(plane_pose, grid.origin_pose); + for (ColumnIterator it = data_.begin(); + it != data_.end(); + it++) { + int column_index = it->first; + RowIndices row_indices = it->second; + jsk_recognition_msgs::SparseOccupancyGridColumn ros_column; + ros_column.column_index = column_index; + for (RowIterator rit = row_indices.begin(); + rit != row_indices.end(); + rit++) { + jsk_recognition_msgs::SparseOccupancyGridCell cell; + cell.row_index = *rit; + cell.value = 1.0; + ros_column.cells.push_back(cell); + } + grid.columns.push_back(ros_column); + } + } + + Plane GridMap::toPlane() + { + return Plane(normal_, d_); + } + + Plane::Ptr GridMap::toPlanePtr() + { + Plane::Ptr ret (new Plane(normal_, d_)); + return ret; + } + + + std::vector GridMap::getCoefficients() + { + std::vector output; + output.push_back(normal_[0]); + output.push_back(normal_[1]); + output.push_back(normal_[2]); + output.push_back(d_); + return output; + } + + void GridMap::vote() + { + ++vote_; + } + + unsigned int GridMap::getVoteNum() + { + return vote_; + } + + void GridMap::setGeneration(unsigned int generation) { + generation_ = generation; + } + + unsigned int GridMap::getGeneration() + { + return generation_; + } + + void GridMap::removeIndex(const GridIndex::Ptr& index) + { + int x = index->x; + int y = index->y; + ColumnIterator it = data_.find(x); + if (it != data_.end()) { + RowIterator rit = (it->second).find(y); + if (rit != it->second.end()) { + it->second.erase(rit); + } + } + } + bool GridMap::isBinsOccupied(const Eigen::Vector3f& p) + { + GridIndex::Ptr ret (new GridIndex()); + pointToIndex(p, ret); + //ROS_INFO("checking (%d, %d)", ret->x, ret->y); + return getValue(ret); + } + + boost::tuple GridMap::minMaxX() + { + int min_x = INT_MAX; + int max_x = - INT_MAX; + for (ColumnIterator it = data_.begin(); + it != data_.end(); + ++it) { + int x = it->first; + if (min_x > x) { + min_x = x; + } + if (max_x < x) { + max_x = x; + } + } + return boost::make_tuple(min_x, max_x); + } + + boost::tuple GridMap::minMaxY() + { + int min_y = INT_MAX; + int max_y = - INT_MAX; + for (ColumnIterator it = data_.begin(); + it != data_.end(); + ++it) { + RowIndices row_indices = it->second; + for (RowIterator rit = row_indices.begin(); + rit != row_indices.end(); + rit++) { + int y = *rit; + if (min_y > y) { + min_y = y; + } + if (max_y < y) { + max_y = y; + } + } + } + return boost::make_tuple(min_y, max_y); + } + + int GridMap::normalizedWidth() + { + boost::tuple min_max_x = minMaxX(); + return min_max_x.get<1>() - min_max_x.get<0>(); + } + + int GridMap::normalizedHeight() + { + boost::tuple min_max_y = minMaxY(); + return min_max_y.get<1>() - min_max_y.get<0>(); + } + + int GridMap::widthOffset() + { + boost::tuple min_max_x = minMaxX(); + int min_x = min_max_x.get<0>(); + return - min_x; + } + + int GridMap::heightOffset() + { + boost::tuple min_max_y = minMaxY(); + int min_y = min_max_y.get<0>(); + return - min_y; + } + + int GridMap::normalizedIndex(int width_offset, int height_offset, + int step, + int elem_size, + int original_x, int original_y) + { + int x = original_x + width_offset; + int y = original_y + height_offset; + return y * step + x * elem_size; + } + + + cv::Mat GridMap::toImage() + { + // initialize with black + int width = normalizedWidth(); + int height = normalizedHeight(); + int width_offset = widthOffset(); + int height_offset = heightOffset(); + cv::Mat m = cv::Mat(width, height, CV_8UC1) * 0; + // for all index + for (ColumnIterator it = data_.begin(); + it != data_.end(); + ++it) { + for (RowIterator rit = it->second.begin(); + rit != it->second.end(); + ++rit) { + m.data[normalizedIndex(width_offset, height_offset, + m.step, m.elemSize(), + it->first, *rit)] = 255; + } + } + + return m; + } + + bool GridMap::check4Neighbor(int x, int y) { + if (getValue(x + 1, y) && + getValue(x + 1, y + 1) && + getValue(x - 1, y) && + getValue(x - 1, y - 1)) { + return true; + } + else { + return false; + } + } + + void GridMap::decreaseOne() + { + //Columns new_data; + GridMap::Ptr new_map (new GridMap(resolution_, getCoefficients())); + for (ColumnIterator it = data_.begin(); + it != data_.end(); + it++) { + RowIndices row_indices = it->second; + int x = it->first; + for (RowIterator rit = row_indices.begin(); + rit != row_indices.end(); + rit++) { + int y = *rit; + if (check4Neighbor(x, y)) { + new_map->registerIndex(x, y); + } + } + } + data_ = new_map->data_; + } + + void GridMap::decrease(int i) + { + for (int ii = 0; ii < i; ii++) { + decreaseOne(); + } + } + + void GridMap::add(GridMap& other) + { + for (ColumnIterator it = other.data_.begin(); + it != other.data_.end(); + it++) { + RowIndices row_indices = it->second; + int x = it->first; + for (RowIterator rit = row_indices.begin(); + rit != row_indices.end(); + rit++) { + int y = *rit; + Eigen::Vector3f pos; + GridIndex index(x, y); + other.gridToPoint(index, pos); + pcl::PointXYZRGB p; + pointFromVectorToXYZ(pos, p); + registerPoint(p); + } + } + } + + pcl::PointCloud::Ptr GridMap::toPointCloud() + { + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + for (ColumnIterator it = data_.begin(); + it != data_.end(); + it++) { + RowIndices row_indices = it->second; + int x = it->first; + for (RowIterator rit = row_indices.begin(); + rit != row_indices.end(); + rit++) { + int y = *rit; + Eigen::Vector3f pos; + GridIndex index(x, y); + gridToPoint(index, pos); + pcl::PointXYZ p; + pointFromVectorToXYZ(pos, p); + cloud->points.push_back(p); + } + } + return cloud; + } + + ConvexPolygon::Ptr GridMap::toConvexPolygon() + { + // 1. build pointcloud + // 2. compute convex hull + // 3. return it as ConvexPolygon + pcl::PointCloud::Ptr cloud = toPointCloud(); + pcl::ConvexHull chull; + chull.setInputCloud(cloud); + chull.setDimension(2); + pcl::PointCloud chull_output; + chull.reconstruct(chull_output); + // convex chull_output to Vertices + Vertices vs; + for (size_t i = 0; i < chull_output.points.size(); i++) { + Eigen::Vector3f v = chull_output.points[i].getVector3fMap(); + vs.push_back(v); + } + return ConvexPolygon::Ptr(new ConvexPolygon(vs)); + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl/ear_clipping_patched.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl/ear_clipping_patched.cpp new file mode 100644 index 00000000000..5efdd83759e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl/ear_clipping_patched.cpp @@ -0,0 +1,209 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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 "jsk_recognition_utils/pcl/ear_clipping_patched.h" +#include +#include +#include + +///////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::EarClippingPatched::initCompute () +{ + points_.reset (new pcl::PointCloud); + + if (!MeshProcessing::initCompute ()) + return (false); + fromPCLPointCloud2 (input_mesh_->cloud, *points_); + + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::EarClippingPatched::performProcessing (PolygonMesh& output) +{ + output.polygons.clear (); + output.cloud = input_mesh_->cloud; + for (int i = 0; i < static_cast (input_mesh_->polygons.size ()); ++i) + triangulate (input_mesh_->polygons[i], output); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::EarClippingPatched::triangulate (const Vertices& vertices, PolygonMesh& output) +{ + const int n_vertices = static_cast (vertices.vertices.size ()); + + if (n_vertices < 3) + return; + else if (n_vertices == 3) + { + output.polygons.push_back( vertices ); + return; + } + + std::vector remaining_vertices (n_vertices); + if (area (vertices.vertices) > 0) // clockwise? + remaining_vertices = vertices.vertices; + else + for (int v = 0; v < n_vertices; v++) + remaining_vertices[v] = vertices.vertices[n_vertices - 1 - v]; + + // Avoid closed loops. + if (remaining_vertices.front () == remaining_vertices.back ()) + remaining_vertices.erase (remaining_vertices.end () - 1); + + // null_iterations avoids infinite loops if the polygon is not simple. + for (int u = static_cast (remaining_vertices.size ()) - 1, null_iterations = 0; + remaining_vertices.size () > 2 && null_iterations < static_cast(remaining_vertices.size () * 2); + ++null_iterations, u = (u+1) % static_cast (remaining_vertices.size ())) + { + int v = (u + 1) % static_cast (remaining_vertices.size ()); + int w = (u + 2) % static_cast (remaining_vertices.size ()); + + if (isEar (u, v, w, remaining_vertices)) + { + Vertices triangle; + triangle.vertices.resize (3); + triangle.vertices[0] = remaining_vertices[u]; + triangle.vertices[1] = remaining_vertices[v]; + triangle.vertices[2] = remaining_vertices[w]; + output.polygons.push_back (triangle); + remaining_vertices.erase (remaining_vertices.begin () + v); + null_iterations = 0; + } + } +} + + +///////////////////////////////////////////////////////////////////////////////////////////// +float +pcl::EarClippingPatched::area (const std::vector& vertices) +{ + //if the polygon is projected onto the xy-plane, the area of the polygon is determined + //by the trapeze formula of Gauss. However this fails, if the projection is one 'line'. + //Therefore the following implementation determines the area of the flat polygon in 3D-space + //using Stoke's law: http://code.activestate.com/recipes/578276-3d-polygon-area/ + + int n = static_cast (vertices.size ()); + float area = 0.0f; + Eigen::Vector3f prev_p, cur_p; + Eigen::Vector3f total (0,0,0); + Eigen::Vector3f unit_normal; + + if (n > 3) + { + for (int prev = n - 1, cur = 0; cur < n; prev = cur++) + { + prev_p = points_->points[vertices[prev]].getVector3fMap(); + cur_p = points_->points[vertices[cur]].getVector3fMap(); + + total += prev_p.cross( cur_p ); + } + + //unit_normal is unit normal vector of plane defined by the first three points + prev_p = points_->points[vertices[1]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap(); + cur_p = points_->points[vertices[2]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap(); + unit_normal = (prev_p.cross(cur_p)).normalized(); + + area = total.dot( unit_normal ); + } + + return area * 0.5f; +} + + +///////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::EarClippingPatched::isEar (int u, int v, int w, const std::vector& vertices) +{ + Eigen::Vector3f p_u, p_v, p_w; + p_u = points_->points[vertices[u]].getVector3fMap(); + p_v = points_->points[vertices[v]].getVector3fMap(); + p_w = points_->points[vertices[w]].getVector3fMap(); + + const float eps = 1e-15f; + Eigen::Vector3f p_uv, p_uw; + p_uv = p_v - p_u; + p_uw = p_w - p_u; + + // Avoid flat triangles. + if ((p_uv.cross(p_uw)).norm() < eps) + return (false); + + Eigen::Vector3f p; + // Check if any other vertex is inside the triangle. + for (int k = 0; k < static_cast (vertices.size ()); k++) + { + if ((k == u) || (k == v) || (k == w)) + continue; + p = points_->points[vertices[k]].getVector3fMap(); + + if (isInsideTriangle (p_u, p_v, p_w, p)) + return (false); + } + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::EarClippingPatched::isInsideTriangle (const Eigen::Vector3f& u, + const Eigen::Vector3f& v, + const Eigen::Vector3f& w, + const Eigen::Vector3f& p) +{ + // see http://www.blackpawn.com/texts/pointinpoly/default.html + // Barycentric Coordinates + Eigen::Vector3f v0 = w - u; + Eigen::Vector3f v1 = v - u; + Eigen::Vector3f v2 = p - u; + + // Compute dot products + float dot00 = v0.dot(v0); + float dot01 = v0.dot(v1); + float dot02 = v0.dot(v2); + float dot11 = v1.dot(v1); + float dot12 = v1.dot(v2); + + // Compute barycentric coordinates + float invDenom = 1 / (dot00 * dot11 - dot01 * dot01); + float a = (dot11 * dot02 - dot01 * dot12) * invDenom; + float b = (dot00 * dot12 - dot01 * dot02) * invDenom; + + // Check if point is in triangle + return (a >= 0) && (b >= 0) && (a + b < 1); +} + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl_conversion_util.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl_conversion_util.cpp new file mode 100644 index 00000000000..58cb8e44a1d --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl_conversion_util.cpp @@ -0,0 +1,225 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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 "jsk_recognition_utils/pcl_conversion_util.h" +#include + +namespace jsk_recognition_utils +{ + + void rangeImageToCvMat(const pcl::RangeImage& range_image, + cv::Mat& image) + { + float min_range, max_range; + range_image.getMinMaxRanges(min_range, max_range); + float min_max_range = max_range - min_range; + + image = cv::Mat(range_image.height, range_image.width, CV_8UC3); + unsigned char r,g,b; + for (int y=0; y < range_image.height; y++) { + for (int x=0; x(y,x)[0] = b; + image.at(y,x)[1] = g; + image.at(y,x)[2] = r; + } + } + return; + } + + void convertEigenAffine3(const Eigen::Affine3d& from, + Eigen::Affine3f& to) + { + Eigen::Matrix4d from_mat = from.matrix(); + Eigen::Matrix4f to_mat; + convertMatrix4(from_mat, to_mat); + to = Eigen::Affine3f(to_mat); + } + + void convertEigenAffine3(const Eigen::Affine3f& from, + Eigen::Affine3d& to) + { + Eigen::Matrix4f from_mat = from.matrix(); + Eigen::Matrix4d to_mat; + convertMatrix4(from_mat, to_mat); + to = Eigen::Affine3d(to_mat); + } +} + +namespace pcl_conversions +{ + std::vector + convertToPCLPointIndices( + const std::vector& cluster_indices) + { + std::vector ret; + for (size_t i = 0; i < cluster_indices.size(); i++) { + std::vector indices = cluster_indices[i].indices; + pcl::PointIndices::Ptr pcl_indices (new pcl::PointIndices); + pcl_indices->indices = indices; + ret.push_back(pcl_indices); + } + return ret; + } + + std::vector + convertToPCLModelCoefficients( + const std::vector& coefficients) + { + std::vector ret; + for (size_t i = 0; i < coefficients.size(); i++) { + pcl::ModelCoefficients::Ptr pcl_coefficients (new pcl::ModelCoefficients); + pcl_coefficients->values = coefficients[i].values; + ret.push_back(pcl_coefficients); + } + return ret; + } + + std::vector + convertToROSPointIndices( + const std::vector cluster_indices, + const std_msgs::Header& header) + { + std::vector ret; + for (size_t i = 0; i < cluster_indices.size(); i++) { + PCLIndicesMsg ros_msg; + ros_msg.header = header; + ros_msg.indices = cluster_indices[i]->indices; + ret.push_back(ros_msg); + } + return ret; + } + + std::vector + convertToROSPointIndices( + const std::vector cluster_indices, + const std_msgs::Header& header) + { + std::vector ret; + for (size_t i = 0; i < cluster_indices.size(); i++) { + PCLIndicesMsg ros_msg; + ros_msg.header = header; + ros_msg.indices = cluster_indices[i].indices; + ret.push_back(ros_msg); + } + return ret; + } + + std::vector + convertToROSModelCoefficients( + const std::vector& coefficients, + const std_msgs::Header& header) + { + std::vector ret; + for (size_t i = 0; i < coefficients.size(); i++) { + PCLModelCoefficientMsg ros_msg; + ros_msg.header = header; + ros_msg.values = coefficients[i]->values; + ret.push_back(ros_msg); + } + return ret; + } + +} + + +namespace tf +{ + void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Affine3f& eigen) + { + Eigen::Affine3d eigen_d; + poseMsgToEigen(msg, eigen_d); + jsk_recognition_utils::convertEigenAffine3(eigen_d, eigen); + } + + void poseEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Pose& msg) + { + Eigen::Affine3d eigen_d; + jsk_recognition_utils::convertEigenAffine3(eigen, eigen_d); + poseEigenToMsg(eigen_d, msg); + } + + void transformMsgToEigen(const geometry_msgs::Transform& msg, Eigen::Affine3f& eigen) + { + Eigen::Affine3d eigen_d; + transformMsgToEigen(msg, eigen_d); + jsk_recognition_utils::convertEigenAffine3(eigen_d, eigen); + } + + void transformEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Transform& msg) + { + Eigen::Affine3d eigen_d; + jsk_recognition_utils::convertEigenAffine3(eigen_d, eigen); + transformEigenToMsg(eigen_d, msg); + } + + void transformTFToEigen(const tf::Transform& t, Eigen::Affine3f& eigen) + { + Eigen::Affine3d eigen_d; + transformTFToEigen(t, eigen_d); + jsk_recognition_utils::convertEigenAffine3(eigen_d, eigen); + } + + void transformEigenToTF(Eigen::Affine3f& eigen , tf::Transform& t) + { + Eigen::Affine3d eigen_d; + jsk_recognition_utils::convertEigenAffine3(eigen, eigen_d); + transformEigenToTF(eigen_d, t); + } + + void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3f& e) + { + Eigen::Vector3d d; + tf::vectorTFToEigen(t, d); + e[0] = d[0]; + e[1] = d[1]; + e[2] = d[2]; + } + + void vectorEigenToTF(const Eigen::Vector3f& e, tf::Vector3& t) + { + Eigen::Vector3d d(e[0], e[1], e[2]); + tf::vectorEigenToTF(d, t); + } +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl_ros_util.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl_ros_util.cpp new file mode 100644 index 00000000000..8d7e001515c --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl_ros_util.cpp @@ -0,0 +1,87 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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 "jsk_recognition_utils/pcl_ros_util.h" +#include + +namespace jsk_recognition_utils +{ + void publishPointIndices( + ros::Publisher& pub, const pcl::PointIndices& indices, const std_msgs::Header& header) + { + pcl_msgs::PointIndices msg; + //pcl_conversions::moveFromPCL does not support const indices + msg.indices = indices.indices; + msg.header = header; + pub.publish(msg); + } + + bool isSameFrameId(const std::string& a, const std::string& b) + { + // we can ignore the first / + std::string aa; + if (a.length() > 0 && a[0] == '/') { + aa = a.substr(1, a.length() - 1); + } + else { + aa = a; + } + std::string bb; + if (b.length() > 0 && b[0] == '/') { + bb = b.substr(1, b.length() - 1); + } + else { + bb = b; + } + return aa == bb; + } + + bool isSameFrameId(const std_msgs::Header& a, const std_msgs::Header& b) + { + return isSameFrameId(a.frame_id, b.frame_id); + } + + bool hasField(const std::string& field_name, const sensor_msgs::PointCloud2& msg) + { + for (size_t i = 0; i < msg.fields.size(); i++) { + sensor_msgs::PointField field = msg.fields[i]; + if (field.name == field_name) { + return true; + } + } + return false; + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl_util.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl_util.cpp new file mode 100644 index 00000000000..6234ad29918 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/pcl_util.cpp @@ -0,0 +1,245 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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 "jsk_recognition_utils/pcl_util.h" +#include +#include + +namespace jsk_recognition_utils +{ + //static boost::mutex global_chull_mutex; + boost::mutex global_chull_mutex; + + Eigen::Affine3f affineFromYAMLNode(const YAML::Node& pose) + { + float x, y, z, rx, ry, rz, rw; +#ifdef USE_OLD_YAML + pose[0] >> x; pose[1] >> y; pose[2] >> z; + pose[3] >> rx; pose[4] >> ry; pose[5] >> rz; pose[6] >> rw; +#else + x = pose[0].as(); y = pose[1].as(); z = pose[2].as(); + rx= pose[3].as(); ry= pose[4].as(); rz= pose[5].as(); rw = pose[6].as(); +#endif + Eigen::Vector3f p(x, y, z); + Eigen::Quaternionf q(rw, rx, ry, rz); + Eigen::Affine3f trans = Eigen::Translation3f(p) * Eigen::AngleAxisf(q); + return trans; + } + + + std::vector addIndices(const std::vector& a, + const std::vector& b) + { + std::set all(b.begin(), b.end()); + for (size_t i = 0; i < a.size(); i++) { + all.insert(a[i]); + } + return std::vector(all.begin(), all.end()); + } + + pcl::PointIndices::Ptr addIndices(const pcl::PointIndices& a, + const pcl::PointIndices& b) + { + std::vector indices = addIndices(a.indices, b.indices); + pcl::PointIndices::Ptr ret(new pcl::PointIndices); + ret->indices = indices; + return ret; + } + + std::vector subIndices(const std::vector& a, + const std::vector& b) + { + std::set all(a.begin(), a.end()); + for (size_t i = 0; i < b.size(); i++) { + std::set::iterator it = all.find(b[i]); + if (it != all.end()) { + all.erase(it); + } + } + return std::vector(all.begin(), all.end()); + } + + pcl::PointIndices::Ptr subIndices(const pcl::PointIndices& a, + const pcl::PointIndices& b) + { + std::vector indices = subIndices(a.indices, b.indices); + pcl::PointIndices::Ptr ret(new pcl::PointIndices); + ret->indices = indices; + return ret; + } + + void Counter::add(double v) + { + acc_(v); + } + + double Counter::mean() + { + return boost::accumulators::mean(acc_); + } + + double Counter::min() + { + return boost::accumulators::min(acc_); + } + + double Counter::max() + { + return boost::accumulators::max(acc_); + } + + int Counter::count() + { + return boost::accumulators::count(acc_); + } + + double Counter::variance() + { + return boost::accumulators::variance(acc_); + } + + void buildGroupFromGraphMap(IntegerGraphMap graph_map, + const int from_index_arg, + std::vector& to_indices_arg, + std::set& output_set) + { + // convert graph_map into one-directional representation + IntegerGraphMap onedirectional_map(graph_map); + for (IntegerGraphMap::iterator it = onedirectional_map.begin(); + it != onedirectional_map.end(); + ++it) { + int from_index = it->first; + std::vector to_indices = it->second; + for (size_t i = 0; i < to_indices.size(); i++) { + int to_index = to_indices[i]; + if (onedirectional_map.find(to_index) == onedirectional_map.end()) { + // not yet initialized + onedirectional_map[to_index] = std::vector(); + } + if (std::find(onedirectional_map[to_index].begin(), + onedirectional_map[to_index].end(), + from_index) == onedirectional_map[to_index].end()) { + onedirectional_map[to_index].push_back(from_index); + } + } + } + _buildGroupFromGraphMap(onedirectional_map, + from_index_arg, + to_indices_arg, + output_set); + } + + void _buildGroupFromGraphMap(IntegerGraphMap graph_map, + const int from_index, + std::vector& to_indices, + std::set& output_set) + { + output_set.insert(from_index); + for (size_t i = 0; i < to_indices.size(); i++) { + int to_index = to_indices[i]; + if (output_set.find(to_index) == output_set.end()) { + output_set.insert(to_index); + //std::cout << "__connection__: " << from_index << " --> " << to_index << std::endl; + std::vector next_indices = graph_map[to_index]; + _buildGroupFromGraphMap(graph_map, + to_index, + next_indices, + output_set); + } + } + } + + void buildAllGroupsSetFromGraphMap(IntegerGraphMap graph_map, + std::vector >& output_sets) + { + std::set duplication_check_set; + for (IntegerGraphMap::iterator it = graph_map.begin(); + it != graph_map.end(); + ++it) { + int from_index = it->first; + if (duplication_check_set.find(from_index) + == duplication_check_set.end()) { + std::set new_graph_set; + buildGroupFromGraphMap(graph_map, from_index, it->second, + new_graph_set); + output_sets.push_back(new_graph_set); + // update duplication_check_set + addSet(duplication_check_set, new_graph_set); + } + } + } + + SeriesedBoolean::SeriesedBoolean(const int buf_len): + buf_(buf_len), buf_len_(buf_len) + { + } + + SeriesedBoolean::~SeriesedBoolean() + { + } + + void SeriesedBoolean::addValue(bool val) + { + buf_.push_front(val); + } + + bool SeriesedBoolean::isAllTrueFilled() + { + return (buf_.size() == buf_len_ && getValue()); + } + + bool SeriesedBoolean::getValue() + { + if (buf_.size() == 0) { + return false; + } + else { + for (boost::circular_buffer::iterator it = buf_.begin(); + it != buf_.end(); + ++it) { + if (!*it) { + return false; + } + } + return true; + } + } + + void SeriesedBoolean::clear() + { + buf_.clear(); + } +} + diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/random_util.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/random_util.cpp new file mode 100644 index 00000000000..54fcf2d144f --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/random_util.cpp @@ -0,0 +1,66 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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 "jsk_recognition_utils/random_util.h" +#include + +namespace jsk_recognition_utils +{ + double randomGaussian(double mean, double var, boost::mt19937& gen) + { + if (var == 0.0) { + return mean; + } + else { + boost::normal_distribution<> dst(mean, sqrt(var)); + boost::variate_generator< + boost::mt19937&, + boost::normal_distribution<> > rand(gen, dst); + return rand(); + } + } + + double randomUniform(double min, double max, boost::mt19937& gen) + { + // Ensure min < max + double amin = std::min(min, max); + double amax = std::max(min, max); + boost::uniform_real<> dst(amin, amax); + boost::variate_generator< + boost::mt19937&, + boost::uniform_real<> > rand(gen, dst); + return rand(); + } +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/rgb_colors.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/rgb_colors.cpp new file mode 100644 index 00000000000..940387799fc --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/rgb_colors.cpp @@ -0,0 +1,197 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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 "jsk_recognition_utils/rgb_colors.h" +#include + + +namespace jsk_recognition_utils +{ + + cv::Vec3d getRGBColor(const int color) + { + cv::Vec3d c; + switch (color % 146) { + case ALICEBLUE: c = cv::Vec3d(0.941, 0.973, 1); break; + case ANTIQUEWHITE: c = cv::Vec3d(0.98, 0.922, 0.843); break; + case AQUA: c = cv::Vec3d(0, 1, 1); break; + case AQUAMARINE: c = cv::Vec3d(0.498, 1, 0.831); break; + case AZURE: c = cv::Vec3d(0.941, 1, 1); break; + case BEIGE: c = cv::Vec3d(0.961, 0.961, 0.863); break; + case BISQUE: c = cv::Vec3d(1, 0.894, 0.769); break; + case BLACK: c = cv::Vec3d(0, 0, 0); break; + case BLANCHEDALMOND: c = cv::Vec3d(1, 0.922, 0.804); break; + case BLUE: c = cv::Vec3d(0, 0, 1); break; + case BLUEVIOLET: c = cv::Vec3d(0.541, 0.169, 0.886); break; + case BROWN: c = cv::Vec3d(0.647, 0.165, 0.165); break; + case BURLYWOOD: c = cv::Vec3d(0.871, 0.722, 0.529); break; + case CADETBLUE: c = cv::Vec3d(0.373, 0.62, 0.627); break; + case CHARTREUSE: c = cv::Vec3d(0.498, 1, 0); break; + case CHOCOLATE: c = cv::Vec3d(0.824, 0.412, 0.118); break; + case CORAL: c = cv::Vec3d(1, 0.498, 0.314); break; + case CORNFLOWERBLUE: c = cv::Vec3d(0.392, 0.584, 0.929); break; + case CORNSILK: c = cv::Vec3d(1, 0.973, 0.863); break; + case CRIMSON: c = cv::Vec3d(0.863, 0.0784, 0.235); break; + case CYAN: c = cv::Vec3d(0, 1, 1); break; + case DARKBLUE: c = cv::Vec3d(0, 0, 0.545); break; + case DARKCYAN: c = cv::Vec3d(0, 0.545, 0.545); break; + case DARKGOLDENROD: c = cv::Vec3d(0.722, 0.525, 0.0431); break; + case DARKGRAY: c = cv::Vec3d(0.663, 0.663, 0.663); break; + case DARKGREEN: c = cv::Vec3d(0, 0.392, 0); break; + case DARKGREY: c = cv::Vec3d(0.663, 0.663, 0.663); break; + case DARKKHAKI: c = cv::Vec3d(0.741, 0.718, 0.42); break; + case DARKMAGENTA: c = cv::Vec3d(0.545, 0, 0.545); break; + case DARKOLIVEGREEN: c = cv::Vec3d(0.333, 0.42, 0.184); break; + case DARKORANGE: c = cv::Vec3d(1, 0.549, 0); break; + case DARKORCHID: c = cv::Vec3d(0.6, 0.196, 0.8); break; + case DARKRED: c = cv::Vec3d(0.545, 0, 0); break; + case DARKSALMON: c = cv::Vec3d(0.914, 0.588, 0.478); break; + case DARKSEAGREEN: c = cv::Vec3d(0.561, 0.737, 0.561); break; + case DARKSLATEBLUE: c = cv::Vec3d(0.282, 0.239, 0.545); break; + case DARKSLATEGRAY: c = cv::Vec3d(0.184, 0.31, 0.31); break; + case DARKSLATEGREY: c = cv::Vec3d(0.184, 0.31, 0.31); break; + case DARKTURQUOISE: c = cv::Vec3d(0, 0.808, 0.82); break; + case DARKVIOLET: c = cv::Vec3d(0.58, 0, 0.827); break; + case DEEPPINK: c = cv::Vec3d(1, 0.0784, 0.576); break; + case DEEPSKYBLUE: c = cv::Vec3d(0, 0.749, 1); break; + case DIMGRAY: c = cv::Vec3d(0.412, 0.412, 0.412); break; + case DIMGREY: c = cv::Vec3d(0.412, 0.412, 0.412); break; + case DODGERBLUE: c = cv::Vec3d(0.118, 0.565, 1); break; + case FIREBRICK: c = cv::Vec3d(0.698, 0.133, 0.133); break; + case FLORALWHITE: c = cv::Vec3d(1, 0.98, 0.941); break; + case FORESTGREEN: c = cv::Vec3d(0.133, 0.545, 0.133); break; + case FUCHSIA: c = cv::Vec3d(1, 0, 1); break; + case GAINSBORO: c = cv::Vec3d(0.863, 0.863, 0.863); break; + case GHOSTWHITE: c = cv::Vec3d(0.973, 0.973, 1); break; + case GOLD: c = cv::Vec3d(1, 0.843, 0); break; + case GOLDENROD: c = cv::Vec3d(0.855, 0.647, 0.125); break; + case GRAY: c = cv::Vec3d(0.502, 0.502, 0.502); break; + case GREEN: c = cv::Vec3d(0, 0.502, 0); break; + case GREENYELLOW: c = cv::Vec3d(0.678, 1, 0.184); break; + case GREY: c = cv::Vec3d(0.502, 0.502, 0.502); break; + case HONEYDEW: c = cv::Vec3d(0.941, 1, 0.941); break; + case HOTPINK: c = cv::Vec3d(1, 0.412, 0.706); break; + case INDIANRED: c = cv::Vec3d(0.804, 0.361, 0.361); break; + case INDIGO: c = cv::Vec3d(0.294, 0, 0.51); break; + case IVORY: c = cv::Vec3d(1, 1, 0.941); break; + case KHAKI: c = cv::Vec3d(0.941, 0.902, 0.549); break; + case LAVENDER: c = cv::Vec3d(0.902, 0.902, 0.98); break; + case LAVENDERBLUSH: c = cv::Vec3d(1, 0.941, 0.961); break; + case LAWNGREEN: c = cv::Vec3d(0.486, 0.988, 0); break; + case LEMONCHIFFON: c = cv::Vec3d(1, 0.98, 0.804); break; + case LIGHTBLUE: c = cv::Vec3d(0.678, 0.847, 0.902); break; + case LIGHTCORAL: c = cv::Vec3d(0.941, 0.502, 0.502); break; + case LIGHTCYAN: c = cv::Vec3d(0.878, 1, 1); break; + case LIGHTGOLDENRODYELLOW: c = cv::Vec3d(0.98, 0.98, 0.824); break; + case LIGHTGRAY: c = cv::Vec3d(0.827, 0.827, 0.827); break; + case LIGHTGREEN: c = cv::Vec3d(0.565, 0.933, 0.565); break; + case LIGHTGREY: c = cv::Vec3d(0.827, 0.827, 0.827); break; + case LIGHTPINK: c = cv::Vec3d(1, 0.714, 0.757); break; + case LIGHTSALMON: c = cv::Vec3d(1, 0.627, 0.478); break; + case LIGHTSEAGREEN: c = cv::Vec3d(0.125, 0.698, 0.667); break; + case LIGHTSKYBLUE: c = cv::Vec3d(0.529, 0.808, 0.98); break; + case LIGHTSLATEGRAY: c = cv::Vec3d(0.467, 0.533, 0.6); break; + case LIGHTSLATEGREY: c = cv::Vec3d(0.467, 0.533, 0.6); break; + case LIGHTSTEELBLUE: c = cv::Vec3d(0.69, 0.769, 0.871); break; + case LIGHTYELLOW: c = cv::Vec3d(1, 1, 0.878); break; + case LIME: c = cv::Vec3d(0, 1, 0); break; + case LIMEGREEN: c = cv::Vec3d(0.196, 0.804, 0.196); break; + case LINEN: c = cv::Vec3d(0.98, 0.941, 0.902); break; + case MAGENTA: c = cv::Vec3d(1, 0, 1); break; + case MAROON: c = cv::Vec3d(0.502, 0, 0); break; + case MEDIUMAQUAMARINE: c = cv::Vec3d(0.4, 0.804, 0.667); break; + case MEDIUMBLUE: c = cv::Vec3d(0, 0, 0.804); break; + case MEDIUMORCHID: c = cv::Vec3d(0.729, 0.333, 0.827); break; + case MEDIUMPURPLE: c = cv::Vec3d(0.576, 0.439, 0.859); break; + case MEDIUMSEAGREEN: c = cv::Vec3d(0.235, 0.702, 0.443); break; + case MEDIUMSLATEBLUE: c = cv::Vec3d(0.482, 0.408, 0.933); break; + case MEDIUMSPRINGGREEN: c = cv::Vec3d(0, 0.98, 0.604); break; + case MEDIUMTURQUOISE: c = cv::Vec3d(0.282, 0.82, 0.8); break; + case MEDIUMVIOLETRED: c = cv::Vec3d(0.78, 0.0824, 0.522); break; + case MIDNIGHTBLUE: c = cv::Vec3d(0.098, 0.098, 0.439); break; + case MINTCREAM: c = cv::Vec3d(0.961, 1, 0.98); break; + case MISTYROSE: c = cv::Vec3d(1, 0.894, 0.882); break; + case MOCCASIN: c = cv::Vec3d(1, 0.894, 0.71); break; + case NAVAJOWHITE: c = cv::Vec3d(1, 0.871, 0.678); break; + case NAVY: c = cv::Vec3d(0, 0, 0.502); break; + case OLDLACE: c = cv::Vec3d(0.992, 0.961, 0.902); break; + case OLIVE: c = cv::Vec3d(0.502, 0.502, 0); break; + case OLIVEDRAB: c = cv::Vec3d(0.42, 0.557, 0.137); break; + case ORANGE: c = cv::Vec3d(1, 0.647, 0); break; + case ORANGERED: c = cv::Vec3d(1, 0.271, 0); break; + case ORCHID: c = cv::Vec3d(0.855, 0.439, 0.839); break; + case PALEGOLDENROD: c = cv::Vec3d(0.933, 0.91, 0.667); break; + case PALEGREEN: c = cv::Vec3d(0.596, 0.984, 0.596); break; + case PALEVIOLETRED: c = cv::Vec3d(0.686, 0.933, 0.933); break; + case PAPAYAWHIP: c = cv::Vec3d(1, 0.937, 0.835); break; + case PEACHPUFF: c = cv::Vec3d(1, 0.855, 0.725); break; + case PERU: c = cv::Vec3d(0.804, 0.522, 0.247); break; + case PINK: c = cv::Vec3d(1, 0.753, 0.796); break; + case PLUM: c = cv::Vec3d(0.867, 0.627, 0.867); break; + case POWDERBLUE: c = cv::Vec3d(0.69, 0.878, 0.902); break; + case PURPLE: c = cv::Vec3d(0.502, 0, 0.502); break; + case RED: c = cv::Vec3d(1, 0, 0); break; + case ROSYBROWN: c = cv::Vec3d(0.737, 0.561, 0.561); break; + case ROYALBLUE: c = cv::Vec3d(0.255, 0.412, 0.882); break; + case SADDLEBROWN: c = cv::Vec3d(0.545, 0.271, 0.0745); break; + case SALMON: c = cv::Vec3d(0.98, 0.502, 0.447); break; + case SANDYBROWN: c = cv::Vec3d(0.98, 0.643, 0.376); break; + case SEAGREEN: c = cv::Vec3d(0.18, 0.545, 0.341); break; + case SEASHELL: c = cv::Vec3d(1, 0.961, 0.933); break; + case SIENNA: c = cv::Vec3d(0.627, 0.322, 0.176); break; + case SILVER: c = cv::Vec3d(0.753, 0.753, 0.753); break; + case SKYBLUE: c = cv::Vec3d(0.529, 0.808, 0.922); break; + case SLATEBLUE: c = cv::Vec3d(0.416, 0.353, 0.804); break; + case SLATEGRAY: c = cv::Vec3d(0.439, 0.502, 0.565); break; + case SLATEGREY: c = cv::Vec3d(0.439, 0.502, 0.565); break; + case SNOW: c = cv::Vec3d(1, 0.98, 0.98); break; + case SPRINGGREEN: c = cv::Vec3d(0, 1, 0.498); break; + case STEELBLUE: c = cv::Vec3d(0.275, 0.51, 0.706); break; + case TAN: c = cv::Vec3d(0.824, 0.706, 0.549); break; + case TEAL: c = cv::Vec3d(0, 0.502, 0.502); break; + case THISTLE: c = cv::Vec3d(0.847, 0.749, 0.847); break; + case TOMATO: c = cv::Vec3d(1, 0.388, 0.278); break; + case TURQUOISE: c = cv::Vec3d(0.251, 0.878, 0.816); break; + case VIOLET: c = cv::Vec3d(0.933, 0.51, 0.933); break; + case WHEAT: c = cv::Vec3d(0.961, 0.871, 0.702); break; + case WHITE: c = cv::Vec3d(1, 1, 1); break; + case WHITESMOKE: c = cv::Vec3d(0.961, 0.961, 0.961); break; + case YELLOW: c = cv::Vec3d(1, 1, 0); break; + case YELLOWGREEN: c = cv::Vec3d(0.604, 0.804, 0.196); break; + } // switch + return c; + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/sensor_model_utils.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/sensor_model_utils.cpp new file mode 100644 index 00000000000..7b85af47136 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/sensor_model_utils.cpp @@ -0,0 +1,52 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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. + *********************************************************************/ +#define BOOST_PARAMETER_MAX_ARITY 7 +#include "jsk_recognition_utils/sensor_model_utils.h" + +namespace jsk_recognition_utils +{ + std::vector + project3DPointstoPixel(const image_geometry::PinholeCameraModel& model, + const Vertices& vertices) + { + std::vector ret; + for (size_t i = 0; i < vertices.size(); i++) { + cv::Point p = project3DPointToPixel(model, vertices[i]); + ret.push_back(p); + } + return ret; + } + +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tests/test_cv_utils.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tests/test_cv_utils.cpp new file mode 100644 index 00000000000..6ec5768ea3e --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tests/test_cv_utils.cpp @@ -0,0 +1,20 @@ +#include "jsk_recognition_utils/cv_utils.h" +#include +#include + + +TEST(CvUtils, testLabelToRGB){ + cv::Mat label_image = cv::Mat::zeros(1, 2, CV_32SC1); + label_image.at(0, 1) = 1; + cv::Mat rgb_image; + jsk_recognition_utils::labelToRGB(label_image, rgb_image); + // background label 0 -> [0, 0, 0] + EXPECT_EQ(0, cv::norm(rgb_image.at(0, 0))); + // label 1 -> not [0, 0, 0] + EXPECT_NE(0, cv::norm(rgb_image.at(0, 1))); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tests/test_rgb_colors.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tests/test_rgb_colors.cpp new file mode 100644 index 00000000000..e2145a2625c --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tests/test_rgb_colors.cpp @@ -0,0 +1,23 @@ +#include "jsk_recognition_utils/rgb_colors.h" +#include +#include + + +TEST(RGBColors, testGetRGBColor){ + cv::Vec3d color; + // red + color = jsk_recognition_utils::getRGBColor(jsk_recognition_utils::RED); + EXPECT_EQ(1, color[0]); + EXPECT_EQ(0, color[1]); + EXPECT_EQ(0, color[2]); + // gray + color = jsk_recognition_utils::getRGBColor(jsk_recognition_utils::GRAY); + EXPECT_EQ(0.502, color[0]); + EXPECT_EQ(0.502, color[1]); + EXPECT_EQ(0.502, color[2]); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tests/test_tf_listener_singleton.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tests/test_tf_listener_singleton.cpp new file mode 100644 index 00000000000..4e00cf86155 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tests/test_tf_listener_singleton.cpp @@ -0,0 +1,38 @@ +#include "jsk_recognition_utils/tf_listener_singleton.h" +#include +#include + + +TEST(TfListenerSingleton, testLookupTransformWithDuration){ + boost::mutex mutex; + tf::TransformListener* tf_listener; + + mutex.lock(); + tf_listener = new tf::TransformListener(ros::Duration(30.0)); + mutex.unlock(); + + std::string from_frame("base"); + std::string to_frame("head"); + tf::StampedTransform transform; + transform = jsk_recognition_utils::lookupTransformWithDuration( + /*listener=*/tf_listener, + /*to_frame=*/to_frame, + /*from_frame=*/from_frame, + /*time=*/ros::Time(), + /*duration=*/ros::Duration(1.0)); + ASSERT_STREQ("base", transform.frame_id_.c_str()); + ASSERT_STREQ("head", transform.child_frame_id_.c_str()); + ASSERT_EQ(0, transform.getOrigin().getX()); + ASSERT_EQ(0, transform.getOrigin().getY()); + ASSERT_EQ(1, transform.getOrigin().getZ()); + ASSERT_EQ(0, transform.getRotation().getX()); + ASSERT_EQ(0, transform.getRotation().getY()); + ASSERT_EQ(0, transform.getRotation().getZ()); + ASSERT_EQ(1, transform.getRotation().getW()); +} + +int main(int argc, char **argv){ + ros::init(argc, argv, "simple_lookup_transform"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tf_listener_singleton.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tf_listener_singleton.cpp new file mode 100644 index 00000000000..45169762a94 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/tf_listener_singleton.cpp @@ -0,0 +1,81 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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/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 +#include "jsk_recognition_utils/tf_listener_singleton.h" +#include + +namespace jsk_recognition_utils +{ + tf::TransformListener* TfListenerSingleton::getInstance() + { + boost::mutex::scoped_lock lock(mutex_); + if (!instance_) { + ROS_INFO("instantiating tf::TransformListener"); + instance_ = new tf::TransformListener(ros::Duration(30.0)); + } + return instance_; + } + + void TfListenerSingleton::destroy() + { + boost::mutex::scoped_lock lock(mutex_); + if (instance_) { + delete instance_; + } + } + + tf::StampedTransform lookupTransformWithDuration( + tf::TransformListener* listener, + const std::string& to_frame, + const std::string& from_frame, + const ros::Time& stamp, + ros::Duration duration) + { + if (listener->waitForTransform(from_frame, to_frame, stamp, duration)) { + tf::StampedTransform transform; + listener->lookupTransform( + from_frame, to_frame, stamp, transform); + return transform; + } + throw tf2::TransformException( + (boost::format("Failed to lookup transformation from %s to %s") + % from_frame.c_str() % to_frame.c_str()).str().c_str()); + + } + + tf::TransformListener* TfListenerSingleton::instance_; + boost::mutex TfListenerSingleton::mutex_; +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/time_util.cpp b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/time_util.cpp new file mode 100644 index 00000000000..ab3d8d531df --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/src/time_util.cpp @@ -0,0 +1,131 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, 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/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 "jsk_recognition_utils/time_util.h" +#include + +namespace jsk_recognition_utils +{ + ScopedWallDurationReporter::ScopedWallDurationReporter(WallDurationTimer* parent): + parent_(parent), start_time_(ros::WallTime::now()), + is_publish_(false), is_enabled_(true) + { + + } + + ScopedWallDurationReporter::ScopedWallDurationReporter( + WallDurationTimer* parent, + ros::Publisher& pub_latest, + ros::Publisher& pub_average): + parent_(parent), start_time_(ros::WallTime::now()), + pub_latest_(pub_latest), pub_average_(pub_average), + is_publish_(true), is_enabled_(true) + { + + } + + ScopedWallDurationReporter::~ScopedWallDurationReporter() + { + ros::WallTime end_time = ros::WallTime::now(); + ros::WallDuration d = end_time - start_time_; + if (is_enabled_) { + parent_->report(d); + if (is_publish_) { + std_msgs::Float32 ros_latest; + ros_latest.data = parent_->latestSec(); + pub_latest_.publish(ros_latest); + std_msgs::Float32 ros_average; + ros_average.data = parent_->meanSec(); + pub_average_.publish(ros_average); + } + } + } + + void ScopedWallDurationReporter::setIsPublish(bool v) + { + is_publish_ = v; + } + + void ScopedWallDurationReporter::setIsEnabled(bool v) + { + is_enabled_ = v; + } + + WallDurationTimer::WallDurationTimer(const int max_num): + max_num_(max_num), buffer_(max_num) + { + } + + void WallDurationTimer::report(ros::WallDuration& duration) + { + buffer_.push_back(duration); + } + + ScopedWallDurationReporter WallDurationTimer::reporter() + { + return ScopedWallDurationReporter(this); + } + + ScopedWallDurationReporter WallDurationTimer::reporter( + ros::Publisher& pub_latest, + ros::Publisher& pub_average) + { + return ScopedWallDurationReporter(this, pub_latest, pub_average); + } + + double WallDurationTimer::latestSec() + { + return buffer_[buffer_.size() - 1].toSec(); + } + + void WallDurationTimer::clearBuffer() + { + buffer_.clear(); + } + + double WallDurationTimer::meanSec() + { + double secs = 0.0; + for (size_t i = 0; i < buffer_.size(); i++) { + secs += buffer_[i].toSec(); + } + return secs / buffer_.size(); + } + + size_t WallDurationTimer::sampleNum() + { + return buffer_.size(); + } +} diff --git a/ros/src/vendor/jsk_recognition/jsk_recognition_utils/test/tf_listener_singleton.test b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/test/tf_listener_singleton.test new file mode 100644 index 00000000000..297bbbda533 --- /dev/null +++ b/ros/src/vendor/jsk_recognition/jsk_recognition_utils/test/tf_listener_singleton.test @@ -0,0 +1,10 @@ + + + + + + +