├── people_msgs ├── msg │ ├── People.msg │ ├── PersonStamped.msg │ ├── Person.msg │ ├── PositionMeasurementArray.msg │ └── PositionMeasurement.msg ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── face_detector ├── param │ └── classifier.yaml ├── action │ └── FaceDetector.action ├── test │ ├── wide-stereo_true_rtest.xml │ ├── action-rgbd_true_rtest.xml │ ├── narrow-stereo_true_rtest.xml │ ├── wide-stereo_false_rtest.xml │ ├── action-rgbd_false_rtest.xml │ ├── action-wide_false_rtest.xml │ ├── action-wide_true_rtest.xml │ ├── narrow-stereo_false_rtest.xml │ ├── rgbd_false_rtest.xml │ ├── rgbd_true_rtest.xml │ └── common_rtest.xml ├── launch │ ├── face_detector.narrow.launch │ ├── face_detector_action.rgbd.launch │ ├── face_detector_action.wide.launch │ ├── face_detector.wide.launch │ ├── face_detector.rgbd.launch │ └── face_detector_common.launch ├── scripts │ └── face_detector_action_client.py ├── package.xml ├── CHANGELOG.rst ├── mainpage.dox ├── CMakeLists.txt └── include │ └── face_detector │ └── faces.h ├── people ├── CMakeLists.txt ├── CHANGELOG.rst └── package.xml ├── leg_detector ├── launch │ ├── leg_detector.launch │ ├── filtered_leg_detector.launch │ └── shadows.launch ├── package.xml ├── cfg │ └── LegDetector.cfg ├── CMakeLists.txt ├── include │ └── leg_detector │ │ ├── calc_leg_features.h │ │ └── laser_processor.h ├── CHANGELOG.rst └── src │ ├── laser_processor.cpp │ ├── train_leg_detector.cpp │ └── calc_leg_features.cpp ├── people_velocity_tracker ├── launch │ ├── tracked_detector.launch │ └── filtered_tracked_detector.launch ├── package.xml ├── CHANGELOG.rst ├── CMakeLists.txt └── scripts │ ├── static.py │ └── tracker.py ├── people_tracking_filter ├── package.xml ├── launch │ └── filter.launch ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── people_tracking_filter │ │ ├── mcpdf_vector.h │ │ ├── state_pos_vel.h │ │ ├── uniform_vector.h │ │ ├── gaussian_vector.h │ │ ├── mcpdf_pos_vel.h │ │ ├── gaussian_pos_vel.h │ │ ├── measmodel_vector.h │ │ ├── measmodel_pos.h │ │ ├── sysmodel_vector.h │ │ ├── tracker.h │ │ ├── sysmodel_pos_vel.h │ │ ├── people_tracking_node.h │ │ ├── detector_particle.h │ │ ├── tracker_kalman.h │ │ └── tracker_particle.h └── src │ ├── measmodel_pos.cpp │ ├── sysmodel_vector.cpp │ ├── measmodel_vector.cpp │ ├── sysmodel_pos_vel.cpp │ ├── uniform_vector.cpp │ ├── gaussian_pos_vel.cpp │ ├── gaussian_vector.cpp │ ├── detector_particle.cpp │ ├── tracker_particle.cpp │ ├── mcpdf_vector.cpp │ ├── mcpdf_pos_vel.cpp │ └── tracker_kalman.cpp └── README.md /people_msgs/msg/People.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | people_msgs/Person[] people 3 | -------------------------------------------------------------------------------- /people_msgs/msg/PersonStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | people_msgs/Person person 3 | 4 | -------------------------------------------------------------------------------- /face_detector/param/classifier.yaml: -------------------------------------------------------------------------------- 1 | classifier_filename: //usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml 2 | -------------------------------------------------------------------------------- /people/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(people) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /face_detector/action/FaceDetector.action: -------------------------------------------------------------------------------- 1 | #goal 2 | --- 3 | #result 4 | people_msgs/PositionMeasurement[] face_positions 5 | --- 6 | #feedback 7 | -------------------------------------------------------------------------------- /people_msgs/msg/Person.msg: -------------------------------------------------------------------------------- 1 | string name 2 | geometry_msgs/Point position 3 | geometry_msgs/Point velocity 4 | float64 reliability 5 | string[] tagnames 6 | string[] tags 7 | 8 | -------------------------------------------------------------------------------- /people_msgs/msg/PositionMeasurementArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # All of the people found 4 | people_msgs/PositionMeasurement[] people 5 | 6 | # The co-occurrence matrix between people 7 | float32[] cooccurrence 8 | 9 | -------------------------------------------------------------------------------- /leg_detector/launch/leg_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /leg_detector/launch/filtered_leg_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | -------------------------------------------------------------------------------- /people_msgs/msg/PositionMeasurement.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | # The name of the detector that detected the person (i.e frontalface, profileface) 3 | string name 4 | string object_id 5 | geometry_msgs/Point pos 6 | float64 reliability 7 | float64[9] covariance 8 | byte initialization 9 | 10 | -------------------------------------------------------------------------------- /people_velocity_tracker/launch/tracked_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /people_velocity_tracker/launch/filtered_tracked_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /face_detector/test/wide-stereo_true_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /face_detector/test/action-rgbd_true_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /face_detector/test/narrow-stereo_true_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /leg_detector/launch/shadows.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 9 | 10 | -------------------------------------------------------------------------------- /face_detector/test/wide-stereo_false_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /face_detector/test/action-rgbd_false_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /face_detector/test/action-wide_false_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /face_detector/test/action-wide_true_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /face_detector/test/narrow-stereo_false_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /people/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package people 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.9 (2015-09-01) 6 | ------------------ 7 | 8 | 1.0.8 (2014-12-10) 9 | ------------------ 10 | * moved social_navigation_layers from metapackage 11 | * Contributors: Dan Lazewatsky 12 | 13 | 1.0.3 (2014-03-01) 14 | ------------------ 15 | 16 | 1.0.2 (2014-02-28) 17 | ------------------ 18 | 19 | 1.0.1 (2014-02-27) 20 | ------------------ 21 | * catkinizing 22 | * Contributors: Dan Lazewatsky 23 | -------------------------------------------------------------------------------- /face_detector/launch/face_detector.narrow.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /people_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(people_msgs) 3 | find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs) 4 | 5 | add_message_files( 6 | DIRECTORY msg 7 | FILES 8 | People.msg 9 | Person.msg 10 | PersonStamped.msg 11 | PositionMeasurement.msg 12 | PositionMeasurementArray.msg 13 | ) 14 | 15 | generate_messages( 16 | DEPENDENCIES geometry_msgs std_msgs 17 | ) 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS geometry_msgs message_runtime std_msgs 21 | ) 22 | -------------------------------------------------------------------------------- /people_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | people_msgs 3 | 1.4.0 4 | Messages used by nodes in the people stack. 5 | Dan Lazewatsky 6 | David V. Lu!! 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/people_msgs 11 | Caroline Pantofaru 12 | catkin 13 | geometry_msgs 14 | std_msgs 15 | message_generation 16 | message_runtime 17 | message_runtime 18 | 19 | -------------------------------------------------------------------------------- /face_detector/test/rgbd_false_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /face_detector/launch/face_detector_action.rgbd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /people_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package people_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.2.0 (2019-08-19) 6 | ------------------ 7 | * Document name field in PositionMeasurement (`#75 `_) 8 | * Cleanup (`#73 `_) 9 | * Fill in implied packages 10 | * Misc Whitespace cleanup 11 | * Contributors: David V. Lu!!, Shane Loretz 12 | 13 | 1.0.9 (2015-09-01) 14 | ------------------ 15 | 16 | 1.0.8 (2014-12-10) 17 | ------------------ 18 | 19 | 1.0.3 (2014-03-01) 20 | ------------------ 21 | 22 | 1.0.2 (2014-02-28) 23 | ------------------ 24 | 25 | 1.0.1 (2014-02-27) 26 | ------------------ 27 | * catkinizing 28 | * Contributors: Dan Lazewatsky 29 | -------------------------------------------------------------------------------- /face_detector/test/rgbd_true_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /face_detector/launch/face_detector_action.wide.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /people_tracking_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | people_tracking_filter 3 | 1.4.0 4 | 5 | A collection of filtering tools for tracking people's locations 6 | 7 | Caroline Pantofaru 8 | David V. Lu!! 9 | BSD 10 | http://ros.org/wiki/people_tracking_filter 11 | 12 | catkin 13 | 14 | bfl 15 | geometry_msgs 16 | message_filters 17 | people_msgs 18 | roscpp 19 | sensor_msgs 20 | std_msgs 21 | tf 22 | roslaunch 23 | roslint 24 | 25 | -------------------------------------------------------------------------------- /people_velocity_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | people_velocity_tracker 3 | 1.4.0 4 | Track the output of the leg_detector to indicate the velocity of person. 5 | David V. Lu!! 6 | David V. Lu!! 7 | BSD 8 | http://ros.org/wiki/people_velocity_tracker 9 | 10 | catkin 11 | easy_markers 12 | geometry_msgs 13 | kalman_filter 14 | people_msgs 15 | roslib 16 | rospy 17 | leg_detector 18 | roslaunch 19 | roslint 20 | 21 | -------------------------------------------------------------------------------- /people/package.xml: -------------------------------------------------------------------------------- 1 | 2 | people 3 | 1.4.0 4 | The people stack holds algorithms for perceiving people from a number of sensors. 5 | Dan Lazewatsky 6 | David V. Lu!! 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/people 11 | Caroline Pantofaru 12 | catkin 13 | face_detector 14 | leg_detector 15 | people_msgs 16 | people_tracking_filter 17 | people_velocity_tracker 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /people_velocity_tracker/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package people_velocity_tracker 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.2.0 (2019-08-19) 6 | ------------------ 7 | * Whitespace cleanup (`#73 `_) 8 | * Contributors: David V. Lu!! 9 | 10 | 1.0.9 (2015-09-01) 11 | ------------------ 12 | * change queue sizes to 10 13 | * Update for Indigo and add unfiltered people velocity launcher 14 | * PEP8 fixes 15 | * Contributors: Aaron Blasdel 16 | 17 | 1.0.8 (2014-12-10) 18 | ------------------ 19 | 20 | 1.0.4 (2014-07-09) 21 | ------------------ 22 | * Merging people_velocity_tracker into people 23 | * Contributors: David Lu!! 24 | 25 | 1.0.3 (2014-03-01) 26 | ------------------ 27 | 28 | 1.0.2 (2014-02-28) 29 | ------------------ 30 | 31 | 1.0.1 (2014-02-27) 32 | ------------------ 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | People 2 | ====== 3 | Algorithms related to detecting and tracking people using various robot sensors. 4 | 5 | ## Documentation 6 | [http://ros.org/wiki/people](http://ros.org/wiki/people) 7 | 8 | ## Status 9 | | Distro | Status | 10 | | ------ | ------ | 11 | | **Kinetic** | [![Kinetic Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__people__ubuntu_xenial_amd64__binary)](http://build.ros.org/job/Kbin_uX64__people__ubuntu_xenial_amd64__binary/) | 12 | | **Melodic** | [![Melodic Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__people__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__people__ubuntu_bionic_amd64__binary/) | 13 | | **Dashing** | None | 14 | 15 | ## Current Branches 16 | * `kinetic` is used by the ROS `kinetic` distro. 17 | 18 | * `melodic` is used by the ROS `melodic` distro. 19 | 20 | * `ros2` is used by the ROS2 `dashing` distro. 21 | -------------------------------------------------------------------------------- /people_velocity_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(people_velocity_tracker) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | easy_markers 6 | geometry_msgs 7 | kalman_filter 8 | people_msgs 9 | roslib 10 | rospy 11 | ) 12 | 13 | catkin_package( 14 | CATKIN_DEPENDS 15 | easy_markers geometry_msgs kalman_filter people_msgs roslib rospy 16 | ) 17 | 18 | if(CATKIN_ENABLE_TESTING) 19 | find_package(catkin REQUIRED COMPONENTS roslaunch roslint) 20 | roslaunch_add_file_check(launch) 21 | roslint_python() 22 | roslint_add_test() 23 | endif() 24 | 25 | install(FILES launch/filtered_tracked_detector.launch launch/tracked_detector.launch 26 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 27 | ) 28 | 29 | catkin_install_python(PROGRAMS scripts/static.py scripts/tracker.py 30 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 31 | ) 32 | -------------------------------------------------------------------------------- /face_detector/launch/face_detector.wide.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /leg_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | leg_detector 3 | 1.4.0 4 | Leg Detector using a machine learning approach to find leg-like patterns of laser scanner readings. 5 | Caroline Pantofaru 6 | David V. Lu!! 7 | BSD 8 | http://ros.org/wiki/leg_detector 9 | 10 | catkin 11 | bfl 12 | dynamic_reconfigure 13 | geometry_msgs 14 | image_geometry 15 | laser_geometry 16 | message_filters 17 | people_msgs 18 | people_tracking_filter 19 | roscpp 20 | sensor_msgs 21 | std_msgs 22 | std_srvs 23 | tf 24 | visualization_msgs 25 | laser_filters 26 | map_laser 27 | roslaunch 28 | roslint 29 | 30 | 31 | -------------------------------------------------------------------------------- /people_tracking_filter/launch/filter.launch: -------------------------------------------------------------------------------- 1 | \n\n 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /face_detector/scripts/face_detector_action_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import actionlib 4 | 5 | from face_detector.msg import FaceDetectorAction, FaceDetectorGoal 6 | 7 | import rospy 8 | 9 | 10 | def face_detector_client(): 11 | # Creates the SimpleActionClient, passing the type of the action to the constructor. 12 | client = actionlib.SimpleActionClient('face_detector_action', FaceDetectorAction) 13 | 14 | # Waits until the action server has started up and started 15 | # listening for goals. 16 | client.wait_for_server() 17 | 18 | # Creates a goal to send to the action server. (no parameters) 19 | goal = FaceDetectorGoal() 20 | 21 | # Sends the goal to the action server. 22 | client.send_goal(goal) 23 | 24 | # Waits for the server to finish performing the action. 25 | client.wait_for_result() 26 | 27 | return client.get_result() # people_msgs/PositionMeasurement[] face_positions 28 | 29 | 30 | if __name__ == '__main__': 31 | try: 32 | # Initializes a rospy node so that the SimpleActionClient can 33 | # publish and subscribe over ROS. 34 | rospy.init_node('face_detector_action_client') 35 | result = face_detector_client() 36 | print('Done action') 37 | except rospy.ROSInterruptException: 38 | print('Program interrupted before completion') 39 | -------------------------------------------------------------------------------- /face_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | face_detector 3 | 1.4.0 4 | Face detection in images. 5 | Dan Lazewatsky 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/face_detector 10 | Caroline Pantofaru 11 | 12 | catkin 13 | 14 | actionlib 15 | actionlib_msgs 16 | cv_bridge 17 | geometry_msgs 18 | image_geometry 19 | image_transport 20 | message_filters 21 | people_msgs 22 | rosbag 23 | roscpp 24 | roslib 25 | rospy 26 | sensor_msgs 27 | std_msgs 28 | std_srvs 29 | stereo_image_proc 30 | stereo_msgs 31 | tf 32 | 33 | message_generation 34 | message_runtime 35 | 36 | dynamic_reconfigure 37 | message_runtime 38 | 39 | roslaunch 40 | roslint 41 | rostest 42 | stereo_image_proc 43 | 44 | -------------------------------------------------------------------------------- /face_detector/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package face_detector 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.2.0 (2019-08-19) 6 | ------------------ 7 | * Whitespace cleanu (`#73 `_) 8 | * Remove and move unused includes (`#70 `_) 9 | * Contributors: David V. Lu!!, Shane Loretz 10 | 11 | 1.0.9 (2015-09-01) 12 | ------------------ 13 | * Install missing param directory: face_detector.rgbd.launch fails due to the `param` folder. 14 | * Contributors: Isaac I.Y. Saito 15 | 16 | 1.0.8 (2014-12-10) 17 | ------------------ 18 | * cleanup formatting with astyle (supersedes `#18 `_) 19 | * centrally reference cascade from standalone opencv (addresses `#15 `_) 20 | * Contributors: Dan Lazewatsky 21 | 22 | 1.0.3 (2014-03-01) 23 | ------------------ 24 | * fix message generation 25 | * Contributors: Dan Lazewatsky 26 | 27 | 1.0.2 (2014-02-28) 28 | ------------------ 29 | * update to properly generate messages 30 | * Contributors: Dan Lazewatsky 31 | 32 | 1.0.1 (2014-02-27) 33 | ------------------ 34 | * update some remappings to deal with the changed openni message api 35 | * convert 16FC1 depth images to 32FC1 and scale to get meters 36 | * switch constants to defines to get rid of linker issues for catkin switch 37 | * catkinizing 38 | * Contributors: Dan Lazewatsky 39 | -------------------------------------------------------------------------------- /leg_detector/cfg/LegDetector.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t, str_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add('connection_threshold', double_t, 0, '[m]', 0.06, 0, .25) 8 | gen.add('min_points_per_group', int_t, 0, '', 5, 1, 20) 9 | 10 | 11 | gen.add('leg_reliability_limit', double_t, 0, '', 0.7, 0, 1) 12 | gen.add('publish_legs', bool_t, 0, '', True) 13 | gen.add('publish_people', bool_t, 0, '', True) 14 | gen.add('publish_leg_markers', bool_t, 0, '', True) 15 | gen.add('publish_people_markers', bool_t, 0, '', True) 16 | 17 | gen.add('no_observation_timeout', double_t, 0, 'Timeout tolerance for no observations [s]', 0.5, 0, 5) 18 | gen.add('max_second_leg_age', double_t, 0, '[s]', 2.0, 0, 5) 19 | gen.add('max_track_jump', double_t, 0, '[m]', 1.0, 0, 5) 20 | gen.add('max_meas_jump', double_t, 0, '[m]', 0.75, 0, 5) 21 | gen.add('leg_pair_separation', double_t, 0, '[m]', 1.0, 0, 2) 22 | gen.add('fixed_frame', str_t, 0, 'Fixed Frame', 'odom_combined') 23 | 24 | gen.add('kalman_p', double_t, 0, '', 4, 0, 10) 25 | gen.add('kalman_q', double_t, 0, '', .002, 0, 10) 26 | gen.add('kalman_r', double_t, 0, '', 10, 0, 20) 27 | gen.add('kalman_on', int_t, 0, '', 1, 0, 1) 28 | 29 | exit(gen.generate('leg_detector', 'leg_detector', 'LegDetector')) 30 | -------------------------------------------------------------------------------- /people_velocity_tracker/scripts/static.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import argparse 4 | 5 | from people_msgs.msg import People, Person 6 | 7 | import rospy 8 | 9 | 10 | class VelocityTracker(object): 11 | def __init__(self, x, y, vx, vy): 12 | self.ppub = rospy.Publisher('/people', People, queue_size=10) 13 | self.person = Person() 14 | self.person.position.x = x 15 | self.person.position.y = y 16 | self.person.position.z = 0.5 17 | self.person.velocity.x = vx 18 | self.person.velocity.y = vy 19 | self.person.name = 'static_test_person' 20 | self.person.reliability = 0.90 21 | 22 | def spin(self): 23 | rate = rospy.Rate(10) 24 | while not rospy.is_shutdown(): 25 | pl = People() 26 | pl.header.stamp = rospy.Time.now() 27 | pl.header.frame_id = '/base_link' 28 | pl.people.append(self.person) 29 | self.ppub.publish(pl) 30 | rate.sleep() 31 | 32 | 33 | if __name__ == '__main__': 34 | rospy.init_node('people_velocity_tracker') 35 | parser = argparse.ArgumentParser() 36 | parser.add_argument('x', type=float, help='x coordinate') 37 | parser.add_argument('y', type=float, help='y coordinate') 38 | parser.add_argument('vx', type=float, nargs='?', default=0.0, help='x velocity') 39 | parser.add_argument('vy', type=float, nargs='?', default=0.0, help='y velocity') 40 | args = parser.parse_args() 41 | 42 | vt = VelocityTracker(args.x, args.y, args.vx, args.vy) 43 | vt.spin() 44 | -------------------------------------------------------------------------------- /face_detector/test/common_rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | topics: 28 | - name: $(arg topicname_hztest) 29 | timeout: 5 30 | negative: False 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /face_detector/launch/face_detector.rgbd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /face_detector/launch/face_detector_common.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /people_tracking_filter/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package people_tracking_filter 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.2.0 (2019-08-19) 6 | ------------------ 7 | * Fix bfl include directory (`#76 `_) 8 | * Whitespace cleanup (`#73 `_) 9 | * Contributors: David V. Lu!!, Lucas Chiesa 10 | 11 | 1.0.9 (2015-09-01) 12 | ------------------ 13 | * Update CMakeLists.txt 14 | * Contributors: David Lu!! 15 | 16 | 1.0.8 (2014-12-10) 17 | ------------------ 18 | * cleanup formatting with astyle (supersedes `#18 `_) 19 | * Contributors: Dan Lazewatsky 20 | 21 | 1.0.4 (2014-07-09) 22 | ------------------ 23 | * merging people_tracking_filter into people 24 | * Contributors: David Lu!! 25 | 26 | 1.0.3 (2014-03-01) 27 | ------------------ 28 | 29 | 1.0.2 (2014-02-28) 30 | ------------------ 31 | 32 | 1.0.1 (2014-02-27) 33 | ------------------ 34 | * people_tracking_filter --> people_experimental 35 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@40197 7275ad9f-c29b-430a-bdc5-66f4b3af1622 36 | * Added platform tags for Ubuntu 9.04, 9.10, and 10.04. 37 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@36945 7275ad9f-c29b-430a-bdc5-66f4b3af1622 38 | * Unblacklisting 39 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@33042 7275ad9f-c29b-430a-bdc5-66f4b3af1622 40 | * temporarily removed dep on message_sequencing 41 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@33041 7275ad9f-c29b-430a-bdc5-66f4b3af1622 42 | * more dependencies removed 43 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@32912 7275ad9f-c29b-430a-bdc5-66f4b3af1622 44 | * git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@32909 7275ad9f-c29b-430a-bdc5-66f4b3af1622 45 | * filter-->people_tracking_filter, follower-->person_follower 46 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@32896 7275ad9f-c29b-430a-bdc5-66f4b3af1622 47 | * Contributors: Brian Gerkey, Caroline Pantofaru 48 | -------------------------------------------------------------------------------- /leg_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(leg_detector) 3 | 4 | # look for bfl (Bayesian Filtering Library) 5 | find_package(PkgConfig REQUIRED) 6 | pkg_check_modules(BFL REQUIRED orocos-bfl) 7 | link_directories(${BFL_LIBRARY_DIRS}) 8 | 9 | # Look for Bullet 10 | pkg_check_modules(BULLET bullet) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | dynamic_reconfigure 14 | geometry_msgs 15 | image_geometry 16 | laser_geometry 17 | message_filters 18 | people_msgs 19 | people_tracking_filter 20 | roscpp 21 | sensor_msgs 22 | std_msgs 23 | std_srvs 24 | tf 25 | visualization_msgs 26 | ) 27 | 28 | ## dynamic reconfigure config 29 | generate_dynamic_reconfigure_options( 30 | cfg/LegDetector.cfg 31 | ) 32 | 33 | catkin_package( 34 | INCLUDE_DIRS include 35 | CATKIN_DEPENDS 36 | dynamic_reconfigure 37 | geometry_msgs 38 | message_filters 39 | people_msgs 40 | people_tracking_filter 41 | roscpp 42 | sensor_msgs 43 | std_msgs 44 | tf 45 | visualization_msgs 46 | ) 47 | 48 | include_directories( 49 | include ${catkin_INCLUDE_DIRS} ${BFL_INCLUDE_DIRS} ${BULLET_INCLUDE_DIRS} 50 | ) 51 | 52 | add_executable(leg_detector 53 | src/laser_processor.cpp 54 | src/leg_detector.cpp 55 | src/calc_leg_features.cpp) 56 | 57 | add_dependencies(leg_detector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 58 | 59 | target_link_libraries(leg_detector 60 | ${catkin_LIBRARIES} ${BFL_LIBRARIES} ${BULLET_LIBRARIES} 61 | ) 62 | 63 | if(CATKIN_ENABLE_TESTING) 64 | find_package(catkin REQUIRED COMPONENTS roslaunch roslint) 65 | roslaunch_add_file_check(launch) 66 | roslint_cpp() 67 | roslint_add_test() 68 | endif() 69 | 70 | install(TARGETS leg_detector 71 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 72 | ) 73 | 74 | install(FILES config/trained_leg_detector.yaml 75 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 76 | ) 77 | 78 | install(DIRECTORY launch/ 79 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 80 | ) 81 | 82 | install(DIRECTORY include/${PROJECT_NAME}/ 83 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 84 | ) 85 | -------------------------------------------------------------------------------- /leg_detector/include/leg_detector/calc_leg_features.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef LEG_DETECTOR_CALC_LEG_FEATURES_H 36 | #define LEG_DETECTOR_CALC_LEG_FEATURES_H 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | // TODO(dlu): Should remove scan dependency from here. 43 | // Only used for jump distance 44 | std::vector calcLegFeatures(laser_processor::SampleSet* cluster, const sensor_msgs::LaserScan& scan); 45 | 46 | #endif // LEG_DETECTOR_CALC_LEG_FEATURES_H 47 | -------------------------------------------------------------------------------- /face_detector/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b face_detector is a package containing algorithms and nodes related to the detection of faces. 6 | 7 | @section usage Usage 8 | See the launch files for examples. launch/face_dector..launch launches in continuous detection mode, and 9 | launch/face_detector_action..launch launches as an action. 10 | 11 | @param classifier_name A readable string name for the classifier. Will be published with the result. 12 | @param classifier_filename Full path to the trained haar cascade. Currently useful cascades are haar_frontalface_alt.xml and haar_profileface.xml in opencv2 13 | @param classifier_reliability Double 0-1. Some notion of the classifier's reliability for use in a larger system. 14 | @param do_continuous true = Run continuously. false = Wait for action call. 15 | @param do_publish_faces_of_unknown_size true = If depth info is not available, publish the resulting face with a position of (0,0,0). false = Don't publish faces if stereo information isn't available. 16 | @param do_display false = Don't display anything. true = Display detections in an OpenCV highgui window. 17 | @param face_size_min_m Double. The minimum width of a face, in meters. Defaults to 0.1m. 18 | @param face_size_max_m Double. The maximum width of a face, in meters. Defaults to 0.5m. 19 | @param max_face_z_m Double. The maximum distance of a face from the camera, in meters. (In the camera frame, depth is along the z-axis.) Defaults to 8.0m. 20 | @param face_separation_dist_m Double. Only used for tracking. The maximum distance between two face detections before they are considered different faces. Defaults to 1.0m. 21 | @param use_rgbd Double. true = use data from an RGBD camera (like the Kinect), false = use stereo data 22 | @param approximate_sync. true = use data from unsynchronized image-depth pairs (Kinect), false = use data from synchronized image-depth pairs (stereo) 23 | 24 | 25 | 26 | \section codeapi Code API 27 | 28 | 38 | 39 | 40 | */ 41 | -------------------------------------------------------------------------------- /leg_detector/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package leg_detector 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.2.0 (2019-08-19) 6 | ------------------ 7 | * Cleanup (`#73 `_) 8 | * General code cleanup (standard headers, whitespace, linting) 9 | * Contributors: David V. Lu!! 10 | 11 | 1.0.9 (2015-09-01) 12 | ------------------ 13 | * Fix handling of scans with negative angle increment 14 | For laser sensors which are mounted upside down. Old code did not generate any clusters in that case. 15 | * Contributors: Timm Linder 16 | 17 | 1.0.8 (2014-12-10) 18 | ------------------ 19 | * cleanup formatting with astyle (supersedes `#18 `_) 20 | * Contributors: Dan Lazewatsky 21 | 22 | 1.0.4 (2014-07-09) 23 | ------------------ 24 | * Merging leg_detector into people 25 | * Contributors: David Lu!! 26 | 27 | 1.0.3 (2014-03-01) 28 | ------------------ 29 | 30 | 1.0.2 (2014-02-28) 31 | ------------------ 32 | 33 | 1.0.1 (2014-02-27) 34 | ------------------ 35 | * height_tracker --> people_experimental 36 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@40194 7275ad9f-c29b-430a-bdc5-66f4b3af1622 37 | * Added platform tags for Ubuntu 9.04, 9.10, and 10.04. 38 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@36945 7275ad9f-c29b-430a-bdc5-66f4b3af1622 39 | * Unblacklisting 40 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@33043 7275ad9f-c29b-430a-bdc5-66f4b3af1622 41 | * removed a number of dependencies 42 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@32910 7275ad9f-c29b-430a-bdc5-66f4b3af1622 43 | * git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@32909 7275ad9f-c29b-430a-bdc5-66f4b3af1622 44 | * people: blacklisting packages due to deprecated deps 45 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@32439 7275ad9f-c29b-430a-bdc5-66f4b3af1622 46 | * leg detector from people_package --> leg_detector 47 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@32067 7275ad9f-c29b-430a-bdc5-66f4b3af1622 48 | * moved the filter into the filter package 49 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@31998 7275ad9f-c29b-430a-bdc5-66f4b3af1622 50 | * empty packages that will eventually hold the stuff in people_package 51 | git-svn-id: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/people@31910 7275ad9f-c29b-430a-bdc5-66f4b3af1622 52 | * Contributors: Brian Gerkey, Caroline Pantofaru, Ken Conley 53 | -------------------------------------------------------------------------------- /people_tracking_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(people_tracking_filter) 3 | 4 | # Look for bfl (Bayesian Filtering Library) 5 | find_package(PkgConfig) 6 | pkg_check_modules(BFL REQUIRED orocos-bfl) 7 | link_directories(${BFL_LIBRARY_DIRS}) 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | geometry_msgs 11 | message_filters 12 | people_msgs 13 | roscpp 14 | sensor_msgs 15 | std_msgs 16 | tf 17 | ) 18 | 19 | find_package(Boost REQUIRED COMPONENTS thread) 20 | 21 | catkin_package( 22 | INCLUDE_DIRS include 23 | CATKIN_DEPENDS 24 | geometry_msgs 25 | message_filters 26 | people_msgs 27 | roscpp 28 | sensor_msgs 29 | std_msgs 30 | tf 31 | LIBRARIES people_tracking_filter 32 | ) 33 | 34 | include_directories( 35 | include ${catkin_INCLUDE_DIRS} ${BFL_INCLUDE_DIRS}/bfl 36 | ) 37 | 38 | add_library(people_tracking_filter 39 | src/uniform_vector.cpp 40 | src/gaussian_vector.cpp 41 | src/gaussian_pos_vel.cpp 42 | src/mcpdf_pos_vel.cpp 43 | src/mcpdf_vector.cpp 44 | src/sysmodel_pos_vel.cpp 45 | src/sysmodel_vector.cpp 46 | src/measmodel_pos.cpp 47 | src/measmodel_vector.cpp 48 | src/tracker_particle.cpp 49 | src/tracker_kalman.cpp 50 | src/detector_particle.cpp 51 | ) 52 | add_dependencies(people_tracking_filter ${catkin_EXPORTED_TARGETS}) 53 | target_link_libraries(people_tracking_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BFL_LIBRARIES}) 54 | 55 | add_executable(people_tracker src/people_tracking_node.cpp) 56 | add_dependencies(people_tracker people_tracking_filter ${catkin_EXPORTED_TARGETS}) 57 | target_link_libraries(people_tracker 58 | people_tracking_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BFL_LIBRARIES} 59 | ) 60 | 61 | if(CATKIN_ENABLE_TESTING) 62 | find_package(catkin REQUIRED COMPONENTS roslaunch roslint) 63 | roslaunch_add_file_check(launch) 64 | roslint_cpp() 65 | roslint_add_test() 66 | endif() 67 | 68 | install(TARGETS people_tracking_filter 69 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 70 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 72 | ) 73 | install(TARGETS people_tracker 74 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 75 | ) 76 | install(DIRECTORY include/${PROJECT_NAME}/ 77 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 78 | ) 79 | install(DIRECTORY launch 80 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 81 | ) 82 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/mcpdf_vector.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_MCPDF_VECTOR_H 38 | #define PEOPLE_TRACKING_FILTER_MCPDF_VECTOR_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | namespace BFL 45 | { 46 | /// Class representing a vector mcpdf 47 | class MCPdfVector: public MCPdf 48 | { 49 | public: 50 | /// Constructor 51 | explicit MCPdfVector(unsigned int num_samples); 52 | 53 | /// Destructor 54 | virtual ~MCPdfVector(); 55 | 56 | /// Get evenly distributed particle cloud 57 | void getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const; 58 | 59 | /// Get pos histogram from certain area 60 | MatrixWrapper::Matrix getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const; 61 | 62 | virtual tf::Vector3 ExpectedValueGet() const; 63 | virtual WeightedSample SampleGet(unsigned int particle) const; 64 | virtual unsigned int numParticlesGet() const; 65 | }; 66 | } // end namespace BFL 67 | #endif // PEOPLE_TRACKING_FILTER_MCPDF_VECTOR_H 68 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/state_pos_vel.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | 38 | #ifndef PEOPLE_TRACKING_FILTER_STATE_POS_VEL_H 39 | #define PEOPLE_TRACKING_FILTER_STATE_POS_VEL_H 40 | 41 | #include 42 | 43 | namespace BFL 44 | { 45 | /// Class representing state with pos and vel 46 | class StatePosVel 47 | { 48 | public: 49 | tf::Vector3 pos_, vel_; 50 | 51 | /// Constructor 52 | StatePosVel(const tf::Vector3& pos = tf::Vector3(0, 0, 0), 53 | const tf::Vector3& vel = tf::Vector3(0, 0, 0)): pos_(pos), vel_(vel) {} 54 | 55 | /// Destructor 56 | ~StatePosVel() {} 57 | 58 | /// operator += 59 | StatePosVel& operator += (const StatePosVel& s) 60 | { 61 | this->pos_ += s.pos_; 62 | this->vel_ += s.vel_; 63 | return *this; 64 | } 65 | 66 | /// operator + 67 | StatePosVel operator + (const StatePosVel& s) 68 | { 69 | StatePosVel res; 70 | 71 | res.pos_ = this->pos_ + s.pos_; 72 | res.vel_ = this->vel_ + s.vel_; 73 | return res; 74 | } 75 | 76 | /// output stream for StatePosVel 77 | friend std::ostream& operator<< (std::ostream& os, const StatePosVel& s) 78 | { 79 | os << "(" << s.pos_[0] << ", " << s.pos_[1] << ", " << s.pos_[2] << ")--(" 80 | << "(" << s.vel_[0] << ", " << s.vel_[1] << ", " << s.vel_[2] << ") "; 81 | return os; 82 | }; 83 | }; 84 | } // end namespace BFL 85 | #endif // PEOPLE_TRACKING_FILTER_STATE_POS_VEL_H 86 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/uniform_vector.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_UNIFORM_VECTOR_H 38 | #define PEOPLE_TRACKING_FILTER_UNIFORM_VECTOR_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | namespace BFL 45 | { 46 | /// Class representing uniform vector 47 | class UniformVector: public Pdf 48 | { 49 | private: 50 | tf::Vector3 mu_, size_; 51 | double probability_; 52 | 53 | public: 54 | /// Constructor 55 | UniformVector(const tf::Vector3& mu, const tf::Vector3& size); 56 | 57 | /// Destructor 58 | virtual ~UniformVector(); 59 | 60 | /// output stream for UniformVector 61 | friend std::ostream& operator<< (std::ostream& os, const UniformVector& g); 62 | 63 | // Redefinition of pure virtuals 64 | virtual UniformVector* Clone() const; 65 | 66 | // Redefinition of pure virtuals 67 | virtual Probability ProbabilityGet(const tf::Vector3& input) const; 68 | bool SampleFrom(vector >& list_samples, const int num_samples, int method = DEFAULT, 69 | void * args = NULL) const; 70 | virtual bool SampleFrom(Sample& one_sample, int method = DEFAULT, void * args = NULL) const; 71 | 72 | virtual tf::Vector3 ExpectedValueGet() const; 73 | virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; 74 | }; 75 | } // namespace BFL 76 | #endif // PEOPLE_TRACKING_FILTER_UNIFORM_VECTOR_H 77 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/gaussian_vector.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_GAUSSIAN_VECTOR_H 38 | #define PEOPLE_TRACKING_FILTER_GAUSSIAN_VECTOR_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | namespace BFL 45 | { 46 | /// Class representing gaussian vector 47 | class GaussianVector: public Pdf 48 | { 49 | private: 50 | tf::Vector3 mu_, sigma_; 51 | mutable double sqrt_; 52 | mutable tf::Vector3 sigma_sq_; 53 | mutable bool sigma_changed_; 54 | 55 | public: 56 | /// Constructor 57 | GaussianVector(const tf::Vector3& mu, const tf::Vector3& sigma); 58 | 59 | /// Destructor 60 | virtual ~GaussianVector(); 61 | 62 | /// output stream for GaussianVector 63 | friend std::ostream& operator<< (std::ostream& os, const GaussianVector& g); 64 | 65 | void sigmaSet(const tf::Vector3& sigma); 66 | 67 | // Redefinition of pure virtuals 68 | virtual Probability ProbabilityGet(const tf::Vector3& input) const; 69 | bool SampleFrom(vector >& list_samples, const int num_samples, int method = DEFAULT, 70 | void * args = NULL) const; 71 | virtual bool SampleFrom(Sample& one_sample, int method = DEFAULT, void * args = NULL) const; 72 | 73 | virtual tf::Vector3 ExpectedValueGet() const; 74 | virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; 75 | 76 | virtual GaussianVector* Clone() const; 77 | }; 78 | } // end namespace BFL 79 | #endif // PEOPLE_TRACKING_FILTER_GAUSSIAN_VECTOR_H 80 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/mcpdf_pos_vel.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_MCPDF_POS_VEL_H 38 | #define PEOPLE_TRACKING_FILTER_MCPDF_POS_VEL_H 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace BFL 46 | { 47 | /// Class representing a posvel mcpdf 48 | class MCPdfPosVel: public MCPdf 49 | { 50 | public: 51 | /// Constructor 52 | explicit MCPdfPosVel(unsigned int num_samples); 53 | 54 | /// Destructor 55 | virtual ~MCPdfPosVel(); 56 | 57 | /// Get evenly distributed particle cloud 58 | void getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const; 59 | 60 | /// Get pos histogram from certain area 61 | MatrixWrapper::Matrix getHistogramPos(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const; 62 | 63 | /// Get vel histogram from certain area 64 | MatrixWrapper::Matrix getHistogramVel(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const; 65 | 66 | virtual StatePosVel ExpectedValueGet() const; 67 | virtual WeightedSample SampleGet(unsigned int particle) const; 68 | virtual unsigned int numParticlesGet() const; 69 | 70 | private: 71 | /// Get histogram from certain area 72 | MatrixWrapper::Matrix getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step, 73 | bool pos_hist) const; 74 | }; 75 | } // end namespace BFL 76 | #endif // PEOPLE_TRACKING_FILTER_MCPDF_POS_VEL_H 77 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/gaussian_pos_vel.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_GAUSSIAN_POS_VEL_H 38 | #define PEOPLE_TRACKING_FILTER_GAUSSIAN_POS_VEL_H 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace BFL 46 | { 47 | /// Class representing gaussian pos_vel 48 | class GaussianPosVel: public Pdf 49 | { 50 | private: 51 | StatePosVel mu_, sigma_; 52 | GaussianVector gauss_pos_, gauss_vel_; 53 | mutable double dt_; 54 | 55 | public: 56 | /// Constructor 57 | GaussianPosVel(const StatePosVel& mu, const StatePosVel& sigma); 58 | 59 | /// Destructor 60 | virtual ~GaussianPosVel(); 61 | 62 | /// clone function 63 | virtual GaussianPosVel* Clone() const; 64 | 65 | /// output stream for GaussianPosVel 66 | friend std::ostream& operator<< (std::ostream& os, const GaussianPosVel& g); 67 | 68 | // set time 69 | void SetDt(double dt) const 70 | { 71 | dt_ = dt; 72 | }; 73 | 74 | // Redefinition of pure virtuals 75 | virtual Probability ProbabilityGet(const StatePosVel& input) const; 76 | bool SampleFrom(vector >& list_samples, const int num_samples, int method = DEFAULT, 77 | void * args = NULL) const; 78 | virtual bool SampleFrom(Sample& one_sample, int method = DEFAULT, void * args = NULL) const; 79 | 80 | virtual StatePosVel ExpectedValueGet() const; 81 | virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; 82 | }; 83 | } // end namespace BFL 84 | #endif // PEOPLE_TRACKING_FILTER_GAUSSIAN_POS_VEL_H 85 | -------------------------------------------------------------------------------- /people_tracking_filter/src/measmodel_pos.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | 39 | namespace BFL 40 | { 41 | static const unsigned int NUM_MEASMODEL_POS_COND_ARGS = 1; 42 | static const unsigned int DIM_MEASMODEL_POS = 13; 43 | 44 | // Constructor 45 | MeasPdfPos::MeasPdfPos(const tf::Vector3& sigma) 46 | : ConditionalPdf(DIM_MEASMODEL_POS, NUM_MEASMODEL_POS_COND_ARGS), 47 | meas_noise_(tf::Vector3(0, 0, 0), sigma) 48 | {} 49 | 50 | // Destructor 51 | MeasPdfPos::~MeasPdfPos() 52 | {} 53 | 54 | Probability 55 | MeasPdfPos::ProbabilityGet(const tf::Vector3& measurement) const 56 | { 57 | return meas_noise_.ProbabilityGet(measurement - ConditionalArgumentGet(0).pos_); 58 | } 59 | 60 | bool 61 | MeasPdfPos::SampleFrom(Sample& one_sample, int method, void *args) const 62 | { 63 | std::cerr << "MeasPdfPos::SampleFrom Method not applicable" << std::endl; 64 | assert(0); 65 | return false; 66 | } 67 | 68 | tf::Vector3 69 | MeasPdfPos::ExpectedValueGet() const 70 | { 71 | std::cerr << "MeasPdfPos::ExpectedValueGet Method not applicable" << std::endl; 72 | tf::Vector3 result; 73 | assert(0); 74 | return result; 75 | } 76 | 77 | SymmetricMatrix 78 | MeasPdfPos::CovarianceGet() const 79 | { 80 | std::cerr << "MeasPdfPos::CovarianceGet Method not applicable" << std::endl; 81 | SymmetricMatrix Covar(DIM_MEASMODEL_POS); 82 | assert(0); 83 | return Covar; 84 | } 85 | 86 | void 87 | MeasPdfPos::CovarianceSet(const MatrixWrapper::SymmetricMatrix& cov) 88 | { 89 | tf::Vector3 cov_vec(sqrt(cov(1, 1)), sqrt(cov(2, 2)), sqrt(cov(3, 3))); 90 | meas_noise_.sigmaSet(cov_vec); 91 | } 92 | } // namespace BFL 93 | -------------------------------------------------------------------------------- /people_tracking_filter/src/sysmodel_vector.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | 39 | static const unsigned int NUM_SYS_VECTOR_COND_ARGS = 1; 40 | static const unsigned int DIM_SYS_VECTOR = 3; 41 | 42 | namespace BFL 43 | { 44 | // Constructor 45 | SysPdfVector::SysPdfVector(const tf::Vector3& sigma) 46 | : ConditionalPdf(DIM_SYS_VECTOR, NUM_SYS_VECTOR_COND_ARGS), 47 | noise_(tf::Vector3(0, 0, 0), sigma) 48 | {} 49 | 50 | // Destructor 51 | SysPdfVector::~SysPdfVector() 52 | {} 53 | 54 | Probability 55 | SysPdfVector::ProbabilityGet(const tf::Vector3& state) const 56 | { 57 | std::cerr << "SysPdfVector::ProbabilityGet Method not applicable" << std::endl; 58 | assert(0); 59 | return 0; 60 | } 61 | 62 | bool 63 | SysPdfVector::SampleFrom(Sample& one_sample, int method, void *args) const 64 | { 65 | tf::Vector3& res = one_sample.ValueGet(); 66 | 67 | // get conditional argument: state 68 | res = this->ConditionalArgumentGet(0); 69 | 70 | // add noise 71 | Sample noise_sample; 72 | noise_.SampleFrom(noise_sample, method, args); 73 | res += noise_sample.ValueGet(); 74 | 75 | return true; 76 | } 77 | 78 | tf::Vector3 79 | SysPdfVector::ExpectedValueGet() const 80 | { 81 | std::cerr << "SysPdfVector::ExpectedValueGet Method not applicable" << std::endl; 82 | assert(0); 83 | return tf::Vector3(); 84 | } 85 | 86 | SymmetricMatrix 87 | SysPdfVector::CovarianceGet() const 88 | { 89 | std::cerr << "SysPdfVector::CovarianceGet Method not applicable" << std::endl; 90 | SymmetricMatrix Covar(DIM_SYS_VECTOR); 91 | assert(0); 92 | return Covar; 93 | } 94 | } // namespace BFL 95 | -------------------------------------------------------------------------------- /people_tracking_filter/src/measmodel_vector.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | 39 | namespace BFL 40 | { 41 | static const unsigned int NUM_MEASMODEL_VECTOR_COND_ARGS = 1; 42 | static const unsigned int DIM_MEASMODEL_VECTOR = 3; 43 | 44 | // Constructor 45 | MeasPdfVector::MeasPdfVector(const tf::Vector3& sigma) 46 | : ConditionalPdf(DIM_MEASMODEL_VECTOR, NUM_MEASMODEL_VECTOR_COND_ARGS), 47 | meas_noise_(tf::Vector3(0, 0, 0), sigma) 48 | {} 49 | 50 | // Destructor 51 | MeasPdfVector::~MeasPdfVector() 52 | {} 53 | 54 | Probability 55 | MeasPdfVector::ProbabilityGet(const tf::Vector3& measurement) const 56 | { 57 | return meas_noise_.ProbabilityGet(measurement - ConditionalArgumentGet(0)); 58 | } 59 | 60 | bool 61 | MeasPdfVector::SampleFrom(Sample& one_sample, int method, void *args) const 62 | { 63 | std::cerr << "MeasPdfVector::SampleFrom Method not applicable" << std::endl; 64 | assert(0); 65 | return false; 66 | } 67 | 68 | tf::Vector3 69 | MeasPdfVector::ExpectedValueGet() const 70 | { 71 | std::cerr << "MeasPdfVector::ExpectedValueGet Method not applicable" << std::endl; 72 | tf::Vector3 result; 73 | assert(0); 74 | return result; 75 | } 76 | 77 | SymmetricMatrix 78 | MeasPdfVector::CovarianceGet() const 79 | { 80 | std::cerr << "MeasPdfVector::CovarianceGet Method not applicable" << std::endl; 81 | SymmetricMatrix Covar(DIM_MEASMODEL_VECTOR); 82 | assert(0); 83 | return Covar; 84 | } 85 | 86 | void 87 | MeasPdfVector::CovarianceSet(const MatrixWrapper::SymmetricMatrix& cov) 88 | { 89 | tf::Vector3 cov_vec(sqrt(cov(1, 1)), sqrt(cov(2, 2)), sqrt(cov(3, 3))); 90 | meas_noise_.sigmaSet(cov_vec); 91 | } 92 | } // namespace BFL 93 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/measmodel_vector.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | 38 | #ifndef PEOPLE_TRACKING_FILTER_MEASMODEL_VECTOR_H 39 | #define PEOPLE_TRACKING_FILTER_MEASMODEL_VECTOR_H 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace BFL 49 | { 50 | class MeasPdfVector 51 | : public BFL::ConditionalPdf 52 | { 53 | public: 54 | /// Constructor 55 | explicit MeasPdfVector(const tf::Vector3& sigma); 56 | 57 | /// Destructor 58 | virtual ~MeasPdfVector(); 59 | 60 | // set covariance 61 | void CovarianceSet(const MatrixWrapper::SymmetricMatrix& cov); 62 | 63 | // Redefining pure virtual methods 64 | virtual BFL::Probability ProbabilityGet(const tf::Vector3& input) const; 65 | virtual bool SampleFrom(BFL::Sample& one_sample, int method, void *args) const; // Not applicable 66 | virtual tf::Vector3 ExpectedValueGet() const; // Not applicable 67 | virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable 68 | 69 | private: 70 | GaussianVector meas_noise_; 71 | }; // class 72 | 73 | class MeasModelVector 74 | : public BFL::MeasurementModel 75 | { 76 | public: 77 | /// constructor 78 | explicit MeasModelVector(const tf::Vector3& sigma) 79 | : BFL::MeasurementModel(new MeasPdfVector(sigma)) 80 | {}; 81 | 82 | /// destructor 83 | ~MeasModelVector() 84 | { 85 | delete MeasurementPdfGet(); 86 | }; 87 | }; // class 88 | } // namespace BFL 89 | #endif // PEOPLE_TRACKING_FILTER_MEASMODEL_VECTOR_H 90 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/measmodel_pos.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_MEASMODEL_POS_H 38 | #define PEOPLE_TRACKING_FILTER_MEASMODEL_POS_H 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace BFL 49 | { 50 | class MeasPdfPos 51 | : public BFL::ConditionalPdf 52 | { 53 | public: 54 | /// Constructor 55 | explicit MeasPdfPos(const tf::Vector3& sigma); 56 | 57 | /// Destructor 58 | virtual ~MeasPdfPos(); 59 | 60 | // set covariance 61 | void CovarianceSet(const MatrixWrapper::SymmetricMatrix& cov); 62 | 63 | // Redefining pure virtual methods 64 | virtual BFL::Probability ProbabilityGet(const tf::Vector3& input) const; 65 | virtual bool SampleFrom(BFL::Sample& one_sample, int method, void *args) const; // Not applicable 66 | virtual tf::Vector3 ExpectedValueGet() const; // Not applicable 67 | virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable 68 | 69 | private: 70 | GaussianVector meas_noise_; 71 | }; // class 72 | 73 | class MeasModelPos 74 | : public BFL::MeasurementModel 75 | { 76 | public: 77 | /// constructor 78 | explicit MeasModelPos(const tf::Vector3& sigma) 79 | : BFL::MeasurementModel(new MeasPdfPos(sigma)) 80 | {}; 81 | 82 | /// destructor 83 | ~MeasModelPos() 84 | { 85 | delete MeasurementPdfGet(); 86 | }; 87 | }; // class 88 | } // namespace BFL 89 | #endif // PEOPLE_TRACKING_FILTER_MEASMODEL_POS_H 90 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/sysmodel_vector.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_SYSMODEL_VECTOR_H 38 | #define PEOPLE_TRACKING_FILTER_SYSMODEL_VECTOR_H 39 | 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace BFL 48 | { 49 | 50 | class SysPdfVector 51 | : public ConditionalPdf 52 | { 53 | public: 54 | /// Constructor 55 | explicit SysPdfVector(const tf::Vector3& sigma); 56 | 57 | /// Destructor 58 | virtual ~SysPdfVector(); 59 | 60 | // set time 61 | void SetDt(double dt) 62 | { 63 | dt_ = dt; 64 | } 65 | 66 | // Redefining pure virtual methods 67 | virtual bool SampleFrom(BFL::Sample& one_sample, int method, void *args) const; 68 | virtual tf::Vector3 ExpectedValueGet() const; // not applicable 69 | virtual Probability ProbabilityGet(const tf::Vector3& state) const; // not applicable 70 | virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable 71 | 72 | private: 73 | GaussianVector noise_; 74 | double dt_; 75 | }; // class 76 | 77 | class SysModelVector 78 | : public SystemModel 79 | { 80 | public: 81 | explicit SysModelVector(const tf::Vector3& sigma) 82 | : SystemModel(new SysPdfVector(sigma)) 83 | {} 84 | 85 | /// destructor 86 | ~SysModelVector() 87 | { 88 | delete SystemPdfGet(); 89 | } 90 | 91 | // set time 92 | void SetDt(double dt) 93 | { 94 | static_cast(SystemPdfGet())->SetDt(dt); 95 | } 96 | }; // class 97 | } // namespace BFL 98 | #endif // PEOPLE_TRACKING_FILTER_SYSMODEL_VECTOR_H 99 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/tracker.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_TRACKER_H 38 | #define PEOPLE_TRACKING_FILTER_TRACKER_H 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | 46 | namespace estimation 47 | { 48 | 49 | class Tracker 50 | { 51 | public: 52 | /// constructor 53 | explicit Tracker(const std::string& name): name_(name) {} 54 | 55 | /// destructor 56 | virtual ~Tracker() {} 57 | 58 | /// return the name of the tracker 59 | const std::string& getName() const 60 | { 61 | return name_; 62 | }; 63 | 64 | /// initialize tracker 65 | virtual void initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time) = 0; 66 | 67 | /// return if tracker was initialized 68 | virtual bool isInitialized() const = 0; 69 | 70 | /// return measure for tracker quality: 0=bad 1=good 71 | virtual double getQuality() const = 0; 72 | 73 | /// return the lifetime of the tracker 74 | virtual double getLifetime() const = 0; 75 | 76 | /// return the time of the tracker 77 | virtual double getTime() const = 0; 78 | 79 | /// update tracker 80 | virtual bool updatePrediction(const double time) = 0; 81 | virtual bool updateCorrection(const tf::Vector3& meas, 82 | const MatrixWrapper::SymmetricMatrix& cov) = 0; 83 | 84 | /// get filter posterior 85 | virtual void getEstimate(BFL::StatePosVel& est) const = 0; 86 | virtual void getEstimate(people_msgs::PositionMeasurement& est) const = 0; 87 | 88 | private: 89 | std::string name_; 90 | }; // class 91 | } // namespace estimation 92 | #endif // PEOPLE_TRACKING_FILTER_TRACKER_H 93 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/sysmodel_pos_vel.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_SYSMODEL_POS_VEL_H 38 | #define PEOPLE_TRACKING_FILTER_SYSMODEL_POS_VEL_H 39 | 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace BFL 49 | { 50 | class SysPdfPosVel 51 | : public ConditionalPdf 52 | { 53 | public: 54 | /// Constructor 55 | explicit SysPdfPosVel(const StatePosVel& sigma); 56 | 57 | /// Destructor 58 | virtual ~SysPdfPosVel(); 59 | 60 | // set time 61 | void SetDt(double dt) 62 | { 63 | dt_ = dt; 64 | } 65 | 66 | // Redefining pure virtual methods 67 | virtual bool SampleFrom(BFL::Sample& one_sample, int method, void *args) const; 68 | virtual StatePosVel ExpectedValueGet() const; // not applicable 69 | virtual Probability ProbabilityGet(const StatePosVel& state) const; // not applicable 70 | virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable 71 | 72 | private: 73 | GaussianPosVel noise_; 74 | double dt_; 75 | }; // class 76 | 77 | class SysModelPosVel 78 | : public SystemModel 79 | { 80 | public: 81 | explicit SysModelPosVel(const StatePosVel& sigma) 82 | : SystemModel(new SysPdfPosVel(sigma)) 83 | {} 84 | 85 | /// destructor 86 | ~SysModelPosVel() 87 | { 88 | delete SystemPdfGet(); 89 | } 90 | 91 | // set time 92 | void SetDt(double dt) 93 | { 94 | static_cast(SystemPdfGet())->SetDt(dt); 95 | } 96 | }; // class 97 | } // namespace BFL 98 | #endif // PEOPLE_TRACKING_FILTER_SYSMODEL_POS_VEL_H 99 | -------------------------------------------------------------------------------- /people_tracking_filter/src/sysmodel_pos_vel.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | 39 | namespace BFL 40 | { 41 | 42 | static const unsigned int NUM_SYS_POS_VEL_COND_ARGS = 1; 43 | static const unsigned int DIM_SYS_POS_VEL = 6; 44 | 45 | // Constructor 46 | SysPdfPosVel::SysPdfPosVel(const BFL::StatePosVel& sigma) 47 | : ConditionalPdf(DIM_SYS_POS_VEL, NUM_SYS_POS_VEL_COND_ARGS), 48 | noise_(StatePosVel(tf::Vector3(0, 0, 0), tf::Vector3(0, 0, 0)), sigma) 49 | {} 50 | 51 | // Destructor 52 | SysPdfPosVel::~SysPdfPosVel() 53 | {} 54 | 55 | Probability 56 | SysPdfPosVel::ProbabilityGet(const StatePosVel& state) const 57 | { 58 | std::cerr << "SysPdfPosVel::ProbabilityGet Method not applicable" << std::endl; 59 | assert(0); 60 | return 0; 61 | } 62 | 63 | bool 64 | SysPdfPosVel::SampleFrom(Sample& one_sample, int method, void *args) const 65 | { 66 | StatePosVel& res = one_sample.ValueGet(); 67 | 68 | // get conditional argument: state 69 | res = this->ConditionalArgumentGet(0); 70 | 71 | // apply system model 72 | res.pos_ += (res.vel_ * dt_); 73 | 74 | // add noise 75 | Sample noise_sample; 76 | noise_.SetDt(dt_); 77 | noise_.SampleFrom(noise_sample, method, args); 78 | res += noise_sample.ValueGet(); 79 | 80 | return true; 81 | } 82 | 83 | StatePosVel 84 | SysPdfPosVel::ExpectedValueGet() const 85 | { 86 | std::cerr << "SysPdfPosVel::ExpectedValueGet Method not applicable" << std::endl; 87 | assert(0); 88 | return StatePosVel(); 89 | } 90 | 91 | SymmetricMatrix 92 | SysPdfPosVel::CovarianceGet() const 93 | { 94 | std::cerr << "SysPdfPosVel::CovarianceGet Method not applicable" << std::endl; 95 | SymmetricMatrix Covar(DIM_SYS_POS_VEL); 96 | assert(0); 97 | return Covar; 98 | } 99 | } // namespace BFL 100 | -------------------------------------------------------------------------------- /face_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(face_detector) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | actionlib 6 | actionlib_msgs 7 | cv_bridge 8 | geometry_msgs 9 | image_geometry 10 | image_transport 11 | message_filters 12 | message_generation 13 | people_msgs 14 | rosbag 15 | roscpp 16 | roslib 17 | rospy 18 | sensor_msgs 19 | std_msgs 20 | std_srvs 21 | stereo_msgs 22 | tf 23 | ) 24 | 25 | find_package(Boost REQUIRED COMPONENTS signals system thread) 26 | find_package(OpenCV) 27 | 28 | add_action_files( 29 | DIRECTORY action 30 | FILES FaceDetector.action 31 | ) 32 | generate_messages(DEPENDENCIES actionlib_msgs people_msgs) 33 | 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | CATKIN_DEPENDS 37 | actionlib 38 | actionlib_msgs 39 | cv_bridge 40 | geometry_msgs 41 | image_geometry 42 | image_transport 43 | message_filters 44 | message_runtime 45 | people_msgs 46 | roscpp 47 | roslib 48 | rospy 49 | sensor_msgs 50 | std_msgs 51 | stereo_msgs 52 | tf 53 | ) 54 | 55 | include_directories( 56 | include 57 | ${Boost_INCLUDE_DIRS} 58 | ${OpenCV_INCLUDE_DIRS} 59 | ${catkin_INCLUDE_DIRS} 60 | ) 61 | 62 | add_executable(face_detector 63 | src/face_detection.cpp 64 | src/faces.cpp) 65 | add_dependencies(face_detector ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 66 | target_link_libraries(face_detector 67 | ${catkin_LIBRARIES} 68 | ${Boost_LIBRARIES} 69 | ${OpenCV_LIBRARIES}) 70 | 71 | install(TARGETS face_detector 72 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 73 | ) 74 | install(DIRECTORY include/${PROJECT_NAME}/ 75 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 76 | ) 77 | 78 | install(DIRECTORY param 79 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ 80 | ) 81 | install(DIRECTORY launch 82 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ 83 | ) 84 | 85 | catkin_install_python(PROGRAMS scripts/face_detector_action_client.py 86 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 87 | ) 88 | 89 | if(CATKIN_ENABLE_TESTING) 90 | find_package(rostest REQUIRED) 91 | find_package(roslint REQUIRED) 92 | find_package(roslaunch REQUIRED) 93 | 94 | catkin_download_test_data(${PROJECT_NAME}_noface_test_diamondback.bag 95 | http://download.ros.org/data/face_detector/face_detector_noface_test_diamondback.bag 96 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 97 | MD5 37f043be780a4511c853379defdd9855) 98 | catkin_download_test_data(${PROJECT_NAME}_withface_test_diamondback.bag 99 | http://download.ros.org/data/face_detector/face_detector_withface_test_diamondback.bag 100 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 101 | MD5 59126117e049e69d577b7ee27251a6f8) 102 | 103 | # add_rostest(test/wide-stereo_true_rtest.xml) 104 | # add_rostest(test/wide-stereo_false_rtest.xml) 105 | # add_rostest(test/rgbd_true_rtest.xml) 106 | # add_rostest(test/rgbd_false_rtest.xml) 107 | # add_rostest(test/narrow-stereo_true_rtest.xml) 108 | # add_rostest(test/narrow-stereo_false_rtest.xml) 109 | # add_rostest(test/action-wide_true_rtest.xml) 110 | # add_rostest(test/action-wide_false_rtest.xml) 111 | # add_rostest(test/action-rgbd_true_rtest.xml) 112 | # add_rostest(test/action-rgbd_false_rtest.xml) 113 | 114 | roslint_cpp() 115 | roslaunch_add_file_check(launch) 116 | roslint_python() 117 | roslint_add_test() 118 | endif() 119 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/people_tracking_node.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_PEOPLE_TRACKING_NODE_H 38 | #define PEOPLE_TRACKING_FILTER_PEOPLE_TRACKING_NODE_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | // ros stuff 45 | #include 46 | #include 47 | #include 48 | 49 | // people tracking stuff 50 | #include 51 | #include 52 | #include 53 | 54 | // messages 55 | #include 56 | #include 57 | #include 58 | #include 59 | 60 | // log files 61 | #include 62 | 63 | 64 | namespace estimation 65 | { 66 | 67 | class PeopleTrackingNode 68 | { 69 | public: 70 | /// constructor 71 | explicit PeopleTrackingNode(ros::NodeHandle nh); 72 | 73 | /// destructor 74 | virtual ~PeopleTrackingNode(); 75 | 76 | /// callback for messages 77 | void callbackRcv(const people_msgs::PositionMeasurement::ConstPtr& message); 78 | 79 | /// callback for dropped messages 80 | void callbackDrop(const people_msgs::PositionMeasurement::ConstPtr& message); 81 | 82 | /// tracker loop 83 | void spin(); 84 | 85 | private: 86 | ros::NodeHandle nh_; 87 | 88 | ros::Publisher people_filter_pub_; 89 | ros::Publisher people_filter_vis_pub_; 90 | ros::Publisher people_tracker_vis_pub_; 91 | 92 | ros::Subscriber people_meas_sub_; 93 | 94 | /// message sequencer 95 | message_filters::TimeSequencer* message_sequencer_; 96 | 97 | /// trackers 98 | std::list trackers_; 99 | 100 | // tf listener 101 | tf::TransformListener robot_state_; 102 | 103 | unsigned int tracker_counter_; 104 | double freq_, start_distance_min_, reliability_threshold_; 105 | BFL::StatePosVel sys_sigma_; 106 | std::string fixed_frame_; 107 | boost::mutex filter_mutex_; 108 | 109 | sensor_msgs::PointCloud meas_cloud_; 110 | unsigned int meas_visualize_counter_; 111 | 112 | // Track only one person who the robot will follow. 113 | bool follow_one_person_; 114 | }; // class 115 | } // namespace estimation 116 | #endif // PEOPLE_TRACKING_FILTER_PEOPLE_TRACKING_NODE_H 117 | -------------------------------------------------------------------------------- /people_tracking_filter/src/uniform_vector.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace BFL 44 | { 45 | UniformVector::UniformVector(const tf::Vector3& mu, const tf::Vector3& size) 46 | : Pdf (1), 47 | mu_(mu), 48 | size_(size) 49 | { 50 | for (unsigned int i = 0; i < 3; i++) 51 | assert(size_[i] > 0); 52 | 53 | probability_ = 1 / (size_[0] * 2 * size_[1] * 2 * size_[2] * 2); 54 | } 55 | 56 | UniformVector::~UniformVector() {} 57 | 58 | UniformVector* UniformVector::Clone() const 59 | { 60 | return new UniformVector(mu_, size_); 61 | } 62 | 63 | std::ostream& operator<< (std::ostream& os, const UniformVector& g) 64 | { 65 | os << "Mu :\n" << g.ExpectedValueGet() << endl 66 | << "Size :\n" << g.CovarianceGet() << endl; 67 | return os; 68 | } 69 | 70 | Probability UniformVector::ProbabilityGet(const tf::Vector3& input) const 71 | { 72 | for (unsigned int i = 0; i < 3; i++) 73 | { 74 | if (input[i] < (mu_[0] - (size_[0]))) return 0; 75 | if (input[i] > (mu_[0] + (size_[0]))) return 0; 76 | } 77 | return probability_; 78 | } 79 | 80 | bool 81 | UniformVector::SampleFrom(vector >& list_samples, const int num_samples, 82 | int method, void* args) const 83 | { 84 | list_samples.resize(num_samples); 85 | vector >::iterator sample_it = list_samples.begin(); 86 | for (sample_it = list_samples.begin(); sample_it != list_samples.end(); sample_it++) 87 | SampleFrom(*sample_it, method, args); 88 | 89 | return true; 90 | } 91 | 92 | bool 93 | UniformVector::SampleFrom(Sample& one_sample, int method, void * args) const 94 | { 95 | one_sample.ValueSet(tf::Vector3(((runif() - 0.5) * 2 * size_[0]) + mu_[0], 96 | ((runif() - 0.5) * 2 * size_[1]) + mu_[1], 97 | ((runif() - 0.5) * 2 * size_[2]) + mu_[2])); 98 | return true; 99 | } 100 | 101 | tf::Vector3 102 | UniformVector::ExpectedValueGet() const 103 | { 104 | return mu_; 105 | } 106 | 107 | SymmetricMatrix 108 | UniformVector::CovarianceGet() const 109 | { 110 | SymmetricMatrix sigma(3); 111 | sigma = 0; 112 | for (unsigned int i = 0; i < 3; i++) 113 | sigma(i + 1, i + 1) = pow(size_[i], 2); 114 | return sigma; 115 | } 116 | 117 | } // End namespace BFL 118 | -------------------------------------------------------------------------------- /leg_detector/include/leg_detector/laser_processor.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef LEG_DETECTOR_LASER_PROCESSOR_H 36 | #define LEG_DETECTOR_LASER_PROCESSOR_H 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | 53 | namespace laser_processor 54 | { 55 | //! A struct representing a single sample from the laser. 56 | class Sample 57 | { 58 | public: 59 | int index; 60 | float range; 61 | float intensity; 62 | float x; 63 | float y; 64 | 65 | static Sample* Extract(int ind, const sensor_msgs::LaserScan& scan); 66 | 67 | private: 68 | Sample() {} 69 | }; 70 | 71 | //! The comparator allowing the creation of an ordered "SampleSet" 72 | struct CompareSample 73 | { 74 | CompareSample() {} 75 | 76 | inline bool operator()(const Sample* a, const Sample* b) 77 | { 78 | return (a->index < b->index); 79 | } 80 | }; 81 | 82 | 83 | //! An ordered set of Samples 84 | class SampleSet : public std::set 85 | { 86 | public: 87 | SampleSet() {} 88 | 89 | ~SampleSet() 90 | { 91 | clear(); 92 | } 93 | 94 | void clear(); 95 | 96 | void appendToCloud(sensor_msgs::PointCloud& cloud, int r = 0, int g = 0, int b = 0); 97 | 98 | tf::Point center(); 99 | }; 100 | 101 | //! A mask for filtering out Samples based on range 102 | class ScanMask 103 | { 104 | SampleSet mask_; 105 | 106 | bool filled; 107 | float angle_min; 108 | float angle_max; 109 | uint32_t size; 110 | 111 | public: 112 | ScanMask() : filled(false), angle_min(0), angle_max(0), size(0) { } 113 | 114 | inline void clear() 115 | { 116 | mask_.clear(); 117 | filled = false; 118 | } 119 | 120 | void addScan(sensor_msgs::LaserScan& scan); 121 | 122 | bool hasSample(Sample* s, float thresh); 123 | }; 124 | 125 | 126 | 127 | class ScanProcessor 128 | { 129 | std::list clusters_; 130 | sensor_msgs::LaserScan scan_; 131 | 132 | public: 133 | std::list& getClusters() 134 | { 135 | return clusters_; 136 | } 137 | 138 | ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold = 0.03); 139 | 140 | ~ScanProcessor(); 141 | 142 | void removeLessThan(uint32_t num); 143 | 144 | void splitConnected(float thresh); 145 | }; 146 | }; // namespace laser_processor 147 | 148 | #endif // LEG_DETECTOR_LASER_PROCESSOR_H 149 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/detector_particle.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_DETECTOR_PARTICLE_H 38 | #define PEOPLE_TRACKING_FILTER_DETECTOR_PARTICLE_H 39 | 40 | #include 41 | 42 | // bayesian filtering 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | // TF 49 | #include 50 | 51 | // msgs 52 | #include 53 | 54 | // log files 55 | #include 56 | 57 | namespace estimation 58 | { 59 | 60 | class DetectorParticle 61 | { 62 | public: 63 | /// constructor 64 | explicit DetectorParticle(unsigned int num_particles); 65 | 66 | /// destructor 67 | ~DetectorParticle(); 68 | 69 | /// initialize detector 70 | void initialize(const tf::Vector3& mu, const tf::Vector3& size, const double time); 71 | 72 | /// return if detector was initialized 73 | bool isInitialized() const 74 | { 75 | return detector_initialized_; 76 | }; 77 | 78 | /// return measure for detector quality: 0=bad 1=good 79 | double getQuality() const 80 | { 81 | return quality_; 82 | }; 83 | 84 | /// update detector 85 | bool updatePrediction(const double dt); 86 | bool updateCorrection(const tf::Vector3& meas, 87 | const MatrixWrapper::SymmetricMatrix& cov, 88 | const double time); 89 | 90 | /// get filter posterior 91 | void getEstimate(tf::Vector3& est) const; 92 | void getEstimate(people_msgs::PositionMeasurement& est) const; 93 | 94 | // get evenly spaced particle cloud 95 | void getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const; 96 | 97 | /// Get histogram from certain area 98 | MatrixWrapper::Matrix getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const; 99 | 100 | private: 101 | // pdf / model / filter 102 | BFL::MCPdfVector prior_; 103 | BFL::BootstrapFilter* filter_; 104 | BFL::SysModelVector sys_model_; 105 | BFL::MeasModelVector meas_model_; 106 | 107 | // vars 108 | bool detector_initialized_; 109 | double filter_time_, quality_; 110 | unsigned int num_particles_; 111 | }; // class 112 | } // namespace estimation 113 | #endif // PEOPLE_TRACKING_FILTER_DETECTOR_PARTICLE_H 114 | -------------------------------------------------------------------------------- /people_velocity_tracker/scripts/tracker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import math 4 | 5 | from easy_markers.generator import Marker, MarkerGenerator 6 | 7 | from geometry_msgs.msg import Point, Vector3 8 | 9 | from kalman_filter import Kalman 10 | 11 | from people_msgs.msg import People, Person, PositionMeasurementArray 12 | 13 | import rospy 14 | 15 | 16 | def distance(leg1, leg2): 17 | return math.sqrt(math.pow(leg1.x - leg2.x, 2) + 18 | math.pow(leg1.y - leg2.y, 2) + 19 | math.pow(leg1.z - leg2.z, 2)) 20 | 21 | 22 | def average(leg1, leg2): 23 | return Point((leg1.x + leg2.x) / 2, 24 | (leg1.y + leg2.y) / 2, 25 | (leg1.z + leg2.z) / 2) 26 | 27 | 28 | def add(v1, v2): 29 | return Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z) 30 | 31 | 32 | def subtract(v1, v2): 33 | return Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z) 34 | 35 | 36 | def scale(v, s): 37 | v.x *= s 38 | v.y *= s 39 | v.z *= s 40 | 41 | 42 | def printv(v): 43 | print('%.2f %.2f %.2f' % (v.x, v.y, v.z),) 44 | 45 | 46 | gen = MarkerGenerator() 47 | gen.type = Marker.ARROW 48 | gen.ns = 'velocities' 49 | gen.lifetime = .5 50 | 51 | 52 | class PersonEstimate(object): 53 | def __init__(self, msg): 54 | self.pos = msg 55 | self.reliability = 0.1 56 | self.k = Kalman() 57 | 58 | def update(self, msg): 59 | last = self.pos 60 | self.pos = msg 61 | self.reliability = max(self.reliability, msg.reliability) 62 | 63 | ivel = subtract(self.pos.pos, last.pos) 64 | time = (self.pos.header.stamp - last.header.stamp).to_sec() 65 | scale(ivel, 1.0 / time) 66 | 67 | self.k.update([ivel.x, ivel.y, ivel.z]) 68 | 69 | def age(self): 70 | return self.pos.header.stamp 71 | 72 | def get_id(self): 73 | return self.pos.object_id 74 | 75 | def velocity(self): 76 | k = self.k.values() 77 | if k is None: 78 | return Vector3() 79 | v = Vector3(k[0], k[1], k[2]) 80 | return v 81 | 82 | def publish_markers(self, pub): 83 | gen.scale = [.1, .3, 0] 84 | gen.color = [1, 1, 1, 1] 85 | vel = self.velocity() 86 | m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)]) 87 | m.header = self.pos.header 88 | pub.publish(m) 89 | 90 | def get_person(self): 91 | p = Person() 92 | p.name = self.get_id() 93 | p.position = self.pos.pos 94 | p.velocity = self.velocity() 95 | p.reliability = self.reliability 96 | return self.pos.header.frame_id, p 97 | 98 | 99 | class VelocityTracker(object): 100 | def __init__(self): 101 | self.people = {} 102 | self.TIMEOUT = rospy.Duration(rospy.get_param('~timeout', 1.0)) 103 | self.sub = rospy.Subscriber('/people_tracker_measurements', 104 | PositionMeasurementArray, 105 | self.pm_cb) 106 | self.mpub = rospy.Publisher('/visualization_marker', 107 | Marker, 108 | queue_size=10) 109 | self.ppub = rospy.Publisher('/people', 110 | People, 111 | queue_size=10) 112 | 113 | def pm_cb(self, msg): 114 | for pm in msg.people: 115 | if pm.object_id in self.people: 116 | self.people[pm.object_id].update(pm) 117 | else: 118 | p = PersonEstimate(pm) 119 | self.people[pm.object_id] = p 120 | 121 | def spin(self): 122 | rate = rospy.Rate(10) 123 | while not rospy.is_shutdown(): 124 | # Remove People Older Than timeout param 125 | now = rospy.Time.now() 126 | for p in self.people.values(): 127 | if now - p.age() > self.TIMEOUT: 128 | del self.people[p.id()] 129 | self.publish() 130 | rate.sleep() 131 | 132 | def publish(self): 133 | gen.counter = 0 134 | pl = People() 135 | pl.header.frame_id = None 136 | 137 | for p in self.people.values(): 138 | p.publish_markers(self.mpub) 139 | frame, person = p.get_person() 140 | pl.header.frame_id = frame 141 | pl.people.append(person) 142 | 143 | self.ppub.publish(pl) 144 | 145 | 146 | if __name__ == '__main__': 147 | rospy.init_node('people_velocity_tracker') 148 | vt = VelocityTracker() 149 | vt.spin() 150 | -------------------------------------------------------------------------------- /people_tracking_filter/src/gaussian_pos_vel.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace BFL 44 | { 45 | GaussianPosVel::GaussianPosVel(const StatePosVel& mu, const StatePosVel& sigma) 46 | : Pdf (1), 47 | mu_(mu), 48 | sigma_(sigma), 49 | gauss_pos_(mu.pos_, sigma.pos_), 50 | gauss_vel_(mu.vel_, sigma.vel_) 51 | {} 52 | 53 | GaussianPosVel::~GaussianPosVel() {} 54 | 55 | GaussianPosVel* GaussianPosVel::Clone() const 56 | { 57 | return new GaussianPosVel(mu_, sigma_); 58 | } 59 | 60 | std::ostream& operator<< (std::ostream& os, const GaussianPosVel& g) 61 | { 62 | os << "\nMu pos :\n" << g.ExpectedValueGet().pos_ << std::endl 63 | << "\nMu vel :\n" << g.ExpectedValueGet().vel_ << std::endl 64 | << "\nSigma:\n" << g.CovarianceGet() << std::endl; 65 | return os; 66 | } 67 | 68 | Probability GaussianPosVel::ProbabilityGet(const StatePosVel& input) const 69 | { 70 | return gauss_pos_.ProbabilityGet(input.pos_) * gauss_vel_.ProbabilityGet(input.vel_); 71 | } 72 | 73 | bool 74 | GaussianPosVel::SampleFrom(std::vector >& list_samples, const int num_samples, int method, 75 | void * args) const 76 | { 77 | list_samples.resize(num_samples); 78 | std::vector >::iterator sample_it = list_samples.begin(); 79 | for (sample_it = list_samples.begin(); sample_it != list_samples.end(); sample_it++) 80 | SampleFrom(*sample_it, method, args); 81 | 82 | return true; 83 | } 84 | 85 | bool 86 | GaussianPosVel::SampleFrom(Sample& one_sample, int method, void * args) const 87 | { 88 | one_sample.ValueSet(StatePosVel(tf::Vector3(rnorm(mu_.pos_[0], sigma_.pos_[0]*dt_), 89 | rnorm(mu_.pos_[1], sigma_.pos_[1]*dt_), 90 | rnorm(mu_.pos_[2], sigma_.pos_[2]*dt_)), 91 | tf::Vector3(rnorm(mu_.vel_[0], sigma_.vel_[0]*dt_), 92 | rnorm(mu_.vel_[1], sigma_.vel_[1]*dt_), 93 | rnorm(mu_.vel_[2], sigma_.vel_[2]*dt_)))); 94 | return true; 95 | } 96 | 97 | StatePosVel 98 | GaussianPosVel::ExpectedValueGet() const 99 | { 100 | return mu_; 101 | } 102 | 103 | SymmetricMatrix 104 | GaussianPosVel::CovarianceGet() const 105 | { 106 | SymmetricMatrix sigma(6); 107 | sigma = 0; 108 | for (unsigned int i = 0; i < 3; i++) 109 | { 110 | sigma(i + 1, i + 1) = pow(sigma_.pos_[i], 2); 111 | sigma(i + 4, i + 4) = pow(sigma_.vel_[i], 2); 112 | } 113 | return sigma; 114 | } 115 | } // End namespace BFL 116 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/tracker_kalman.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_TRACKER_KALMAN_H 38 | #define PEOPLE_TRACKING_FILTER_TRACKER_KALMAN_H 39 | 40 | #include 41 | 42 | // bayesian filtering 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | 49 | #include 50 | 51 | // TF 52 | #include 53 | 54 | // log files 55 | #include 56 | #include 57 | 58 | namespace estimation 59 | { 60 | 61 | class TrackerKalman: public Tracker 62 | { 63 | public: 64 | /// constructor 65 | TrackerKalman(const std::string& name, const BFL::StatePosVel& sysnoise); 66 | 67 | /// destructor 68 | virtual ~TrackerKalman(); 69 | 70 | /// initialize tracker 71 | virtual void initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time); 72 | 73 | /// return if tracker was initialized 74 | virtual bool isInitialized() const 75 | { 76 | return tracker_initialized_; 77 | }; 78 | 79 | /// return measure for tracker quality: 0=bad 1=good 80 | virtual double getQuality() const 81 | { 82 | return quality_; 83 | }; 84 | 85 | /// return the lifetime of the tracker 86 | virtual double getLifetime() const; 87 | 88 | /// return the time of the tracker 89 | virtual double getTime() const; 90 | 91 | /// update tracker 92 | virtual bool updatePrediction(const double time); 93 | virtual bool updateCorrection(const tf::Vector3& meas, 94 | const MatrixWrapper::SymmetricMatrix& cov); 95 | 96 | /// get filter posterior 97 | virtual void getEstimate(BFL::StatePosVel& est) const; 98 | virtual void getEstimate(people_msgs::PositionMeasurement& est) const; 99 | 100 | private: 101 | // pdf / model / filter 102 | BFL::Gaussian prior_; 103 | BFL::ExtendedKalmanFilter* filter_; 104 | BFL::LinearAnalyticConditionalGaussian* sys_pdf_; 105 | BFL::LinearAnalyticSystemModelGaussianUncertainty* sys_model_; 106 | BFL::LinearAnalyticConditionalGaussian* meas_pdf_; 107 | BFL::LinearAnalyticMeasurementModelGaussianUncertainty* meas_model_; 108 | MatrixWrapper::Matrix sys_matrix_; 109 | MatrixWrapper::SymmetricMatrix sys_sigma_; 110 | 111 | double calculateQuality(); 112 | 113 | // vars 114 | bool tracker_initialized_; 115 | double init_time_, filter_time_, quality_; 116 | }; // class 117 | } // namespace estimation 118 | #endif // PEOPLE_TRACKING_FILTER_TRACKER_KALMAN_H 119 | -------------------------------------------------------------------------------- /people_tracking_filter/src/gaussian_vector.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace BFL 44 | { 45 | GaussianVector::GaussianVector(const tf::Vector3& mu, const tf::Vector3& sigma) 46 | : Pdf (1), 47 | mu_(mu), 48 | sigma_(sigma), 49 | sigma_changed_(true) 50 | { 51 | for (unsigned int i = 0; i < 3; i++) 52 | assert(sigma[i] > 0); 53 | } 54 | 55 | GaussianVector::~GaussianVector() {} 56 | 57 | std::ostream& operator<< (std::ostream& os, const GaussianVector& g) 58 | { 59 | os << "Mu :\n" << g.ExpectedValueGet() << std::endl 60 | << "Sigma:\n" << g.CovarianceGet() << std::endl; 61 | return os; 62 | } 63 | 64 | void GaussianVector::sigmaSet(const tf::Vector3& sigma) 65 | { 66 | sigma_ = sigma; 67 | sigma_changed_ = true; 68 | } 69 | 70 | Probability GaussianVector::ProbabilityGet(const tf::Vector3& input) const 71 | { 72 | if (sigma_changed_) 73 | { 74 | sigma_changed_ = false; 75 | // 2 * sigma^2 76 | for (unsigned int i = 0; i < 3; i++) 77 | sigma_sq_[i] = 2 * sigma_[i] * sigma_[i]; 78 | // sqrt 79 | sqrt_ = 1 / sqrt(M_PI * M_PI * M_PI * sigma_sq_[0] * sigma_sq_[1] * sigma_sq_[2]); 80 | } 81 | 82 | tf::Vector3 diff = input - mu_; 83 | return sqrt_ * exp(- (diff[0] * diff[0] / sigma_sq_[0]) 84 | - (diff[1] * diff[1] / sigma_sq_[1]) 85 | - (diff[2] * diff[2] / sigma_sq_[2])); 86 | } 87 | 88 | bool 89 | GaussianVector::SampleFrom(std::vector >& list_samples, const int num_samples, int method, 90 | void * args) const 91 | { 92 | list_samples.resize(num_samples); 93 | std::vector >::iterator sample_it = list_samples.begin(); 94 | for (sample_it = list_samples.begin(); sample_it != list_samples.end(); sample_it++) 95 | SampleFrom(*sample_it, method, args); 96 | 97 | return true; 98 | } 99 | 100 | bool 101 | GaussianVector::SampleFrom(Sample& one_sample, int method, void * args) const 102 | { 103 | one_sample.ValueSet(tf::Vector3(rnorm(mu_[0], sigma_[0]), 104 | rnorm(mu_[1], sigma_[1]), 105 | rnorm(mu_[2], sigma_[2]))); 106 | return true; 107 | } 108 | 109 | tf::Vector3 110 | GaussianVector::ExpectedValueGet() const 111 | { 112 | return mu_; 113 | } 114 | 115 | SymmetricMatrix 116 | GaussianVector::CovarianceGet() const 117 | { 118 | SymmetricMatrix sigma(3); 119 | sigma = 0; 120 | for (unsigned int i = 0; i < 3; i++) 121 | sigma(i + 1, i + 1) = pow(sigma_[i], 2); 122 | return sigma; 123 | } 124 | 125 | GaussianVector* 126 | GaussianVector::Clone() const 127 | { 128 | return new GaussianVector(mu_, sigma_); 129 | } 130 | } // End namespace BFL 131 | -------------------------------------------------------------------------------- /people_tracking_filter/include/people_tracking_filter/tracker_particle.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef PEOPLE_TRACKING_FILTER_TRACKER_PARTICLE_H 38 | #define PEOPLE_TRACKING_FILTER_TRACKER_PARTICLE_H 39 | 40 | #include 41 | 42 | // bayesian filtering 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | // TF 50 | #include 51 | 52 | // msgs 53 | #include 54 | 55 | // log files 56 | #include 57 | #include 58 | 59 | namespace estimation 60 | { 61 | class TrackerParticle: public Tracker 62 | { 63 | public: 64 | /// constructor 65 | TrackerParticle(const std::string& name, unsigned int num_particles, const BFL::StatePosVel& sysnoise); 66 | 67 | /// destructor 68 | virtual ~TrackerParticle(); 69 | 70 | /// initialize tracker 71 | virtual void initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time); 72 | 73 | /// return if tracker was initialized 74 | virtual bool isInitialized() const 75 | { 76 | return tracker_initialized_; 77 | } 78 | 79 | /// return measure for tracker quality: 0=bad 1=good 80 | virtual double getQuality() const 81 | { 82 | return quality_; 83 | } 84 | 85 | /// return the lifetime of the tracker 86 | virtual double getLifetime() const; 87 | 88 | /// return the time of the tracker 89 | virtual double getTime() const; 90 | 91 | /// update tracker 92 | virtual bool updatePrediction(const double time); 93 | virtual bool updateCorrection(const tf::Vector3& meas, 94 | const MatrixWrapper::SymmetricMatrix& cov); 95 | 96 | /// get filter posterior 97 | virtual void getEstimate(BFL::StatePosVel& est) const; 98 | virtual void getEstimate(people_msgs::PositionMeasurement& est) const; 99 | 100 | // get evenly spaced particle cloud 101 | void getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const; 102 | 103 | /// Get histogram from certain area 104 | MatrixWrapper::Matrix getHistogramPos(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const; 105 | MatrixWrapper::Matrix getHistogramVel(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const; 106 | 107 | private: 108 | // pdf / model / filter 109 | BFL::MCPdfPosVel prior_; 110 | BFL::BootstrapFilter* filter_; 111 | BFL::SysModelPosVel sys_model_; 112 | BFL::MeasModelPos meas_model_; 113 | 114 | // vars 115 | bool tracker_initialized_; 116 | double init_time_, filter_time_, quality_; 117 | unsigned int num_particles_; 118 | }; // class 119 | } // namespace estimation 120 | #endif // PEOPLE_TRACKING_FILTER_TRACKER_PARTICLE_H 121 | -------------------------------------------------------------------------------- /people_tracking_filter/src/detector_particle.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | namespace estimation 42 | { 43 | using MatrixWrapper::Matrix; 44 | 45 | // constructor 46 | DetectorParticle::DetectorParticle(unsigned int num_particles): 47 | prior_(num_particles), 48 | filter_(NULL), 49 | sys_model_(tf::Vector3(0.1, 0.1, 0.1)), 50 | meas_model_(tf::Vector3(0.1, 0.1, 0.1)), 51 | detector_initialized_(false), 52 | num_particles_(num_particles) 53 | {} 54 | 55 | // destructor 56 | DetectorParticle::~DetectorParticle() 57 | { 58 | if (filter_) delete filter_; 59 | } 60 | 61 | // initialize prior density of filter 62 | void DetectorParticle::initialize(const tf::Vector3& mu, const tf::Vector3& size, const double time) 63 | { 64 | std::cout << "Initializing detector with " << num_particles_ << " particles, with uniform size " 65 | << size << " around " << mu << std::endl; 66 | 67 | BFL::UniformVector uniform_vector(mu, size); 68 | std::vector > prior_samples(num_particles_); 69 | uniform_vector.SampleFrom(prior_samples, num_particles_, CHOLESKY, NULL); 70 | prior_.ListOfSamplesSet(prior_samples); 71 | filter_ = new BFL::BootstrapFilter(&prior_, &prior_, 0, num_particles_ / 4.0); 72 | 73 | // detector initialized 74 | detector_initialized_ = true; 75 | quality_ = 1; 76 | filter_time_ = time; 77 | } 78 | 79 | // update filter prediction 80 | bool DetectorParticle::updatePrediction(const double dt) 81 | { 82 | // set de in sys model 83 | sys_model_.SetDt(dt); 84 | 85 | // update filter 86 | bool res = filter_->Update(&sys_model_); 87 | if (!res) quality_ = 0; 88 | 89 | return res; 90 | } 91 | 92 | // update filter correction 93 | bool DetectorParticle::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov, 94 | const double time) 95 | { 96 | assert(cov.columns() == 3); 97 | 98 | // set filter time 99 | filter_time_ = time; 100 | 101 | // set covariance 102 | static_cast(meas_model_.MeasurementPdfGet())->CovarianceSet(cov); 103 | 104 | // update filter 105 | bool res = filter_->Update(&meas_model_, meas); 106 | if (!res) quality_ = 0; 107 | 108 | return res; 109 | } 110 | 111 | // get evenly spaced particle cloud 112 | void DetectorParticle::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const 113 | { 114 | static_cast(filter_->PostGet())->getParticleCloud(step, threshold, cloud); 115 | } 116 | 117 | // get most recent filter posterior 118 | void DetectorParticle::getEstimate(tf::Vector3& est) const 119 | { 120 | est = static_cast(filter_->PostGet())->ExpectedValueGet(); 121 | } 122 | 123 | void DetectorParticle::getEstimate(people_msgs::PositionMeasurement& est) const 124 | { 125 | tf::Vector3 tmp = filter_->PostGet()->ExpectedValueGet(); 126 | 127 | est.pos.x = tmp[0]; 128 | est.pos.y = tmp[1]; 129 | est.pos.z = tmp[2]; 130 | 131 | est.header.stamp.fromSec(filter_time_); 132 | est.header.frame_id = "base_link"; 133 | } 134 | 135 | /// Get histogram from certain area 136 | Matrix DetectorParticle::getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const 137 | { 138 | return static_cast(filter_->PostGet())->getHistogram(min, max, step); 139 | } 140 | } // namespace estimation 141 | -------------------------------------------------------------------------------- /people_tracking_filter/src/tracker_particle.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace estimation 43 | { 44 | using MatrixWrapper::Matrix; 45 | 46 | // constructor 47 | TrackerParticle::TrackerParticle(const std::string& name, unsigned int num_particles, const BFL::StatePosVel& sysnoise): 48 | Tracker(name), 49 | prior_(num_particles), 50 | filter_(NULL), 51 | sys_model_(sysnoise), 52 | meas_model_(tf::Vector3(0.1, 0.1, 0.1)), 53 | tracker_initialized_(false), 54 | num_particles_(num_particles) 55 | {} 56 | 57 | // destructor 58 | TrackerParticle::~TrackerParticle() 59 | { 60 | if (filter_) delete filter_; 61 | } 62 | 63 | // initialize prior density of filter 64 | void TrackerParticle::initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time) 65 | { 66 | std::cout << "Initializing tracker with " << num_particles_ << " particles, with covariance " 67 | << sigma << " around " << mu << std::endl; 68 | 69 | BFL::GaussianPosVel gauss_pos_vel(mu, sigma); 70 | std::vector > prior_samples(num_particles_); 71 | gauss_pos_vel.SampleFrom(prior_samples, num_particles_, CHOLESKY, NULL); 72 | prior_.ListOfSamplesSet(prior_samples); 73 | filter_ = new BFL::BootstrapFilter(&prior_, &prior_, 0, num_particles_ / 4.0); 74 | 75 | // tracker initialized 76 | tracker_initialized_ = true; 77 | quality_ = 1; 78 | filter_time_ = time; 79 | init_time_ = time; 80 | } 81 | 82 | // update filter prediction 83 | bool TrackerParticle::updatePrediction(const double time) 84 | { 85 | bool res = true; 86 | if (time > filter_time_) 87 | { 88 | // set dt in sys model 89 | sys_model_.SetDt(time - filter_time_); 90 | filter_time_ = time; 91 | 92 | // update filter 93 | res = filter_->Update(&sys_model_); 94 | if (!res) quality_ = 0; 95 | } 96 | return res; 97 | }; 98 | 99 | // update filter correction 100 | bool TrackerParticle::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov) 101 | { 102 | assert(cov.columns() == 3); 103 | 104 | // set covariance 105 | static_cast(meas_model_.MeasurementPdfGet())->CovarianceSet(cov); 106 | 107 | // update filter 108 | bool res = filter_->Update(&meas_model_, meas); 109 | if (!res) quality_ = 0; 110 | 111 | return res; 112 | }; 113 | 114 | // get evenly spaced particle cloud 115 | void TrackerParticle::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const 116 | { 117 | static_cast(filter_->PostGet())->getParticleCloud(step, threshold, cloud); 118 | }; 119 | 120 | // get most recent filter posterior 121 | void TrackerParticle::getEstimate(BFL::StatePosVel& est) const 122 | { 123 | est = static_cast(filter_->PostGet())->ExpectedValueGet(); 124 | }; 125 | 126 | void TrackerParticle::getEstimate(people_msgs::PositionMeasurement& est) const 127 | { 128 | BFL::StatePosVel tmp = filter_->PostGet()->ExpectedValueGet(); 129 | 130 | est.pos.x = tmp.pos_[0]; 131 | est.pos.y = tmp.pos_[1]; 132 | est.pos.z = tmp.pos_[2]; 133 | 134 | est.header.stamp.fromSec(filter_time_); 135 | est.object_id = getName(); 136 | } 137 | 138 | /// Get histogram from certain area 139 | Matrix TrackerParticle::getHistogramPos(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const 140 | { 141 | return static_cast(filter_->PostGet())->getHistogramPos(min, max, step); 142 | }; 143 | 144 | Matrix TrackerParticle::getHistogramVel(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const 145 | { 146 | return static_cast(filter_->PostGet())->getHistogramVel(min, max, step); 147 | }; 148 | 149 | double TrackerParticle::getLifetime() const 150 | { 151 | if (tracker_initialized_) 152 | return filter_time_ - init_time_; 153 | else 154 | return 0; 155 | } 156 | 157 | double TrackerParticle::getTime() const 158 | { 159 | if (tracker_initialized_) 160 | return filter_time_; 161 | else 162 | return 0; 163 | } 164 | } // namespace estimation 165 | -------------------------------------------------------------------------------- /people_tracking_filter/src/mcpdf_vector.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace BFL 46 | { 47 | static const unsigned int NUM_CONDARG = 1; 48 | 49 | MCPdfVector::MCPdfVector(unsigned int num_samples) 50 | : MCPdf (num_samples, NUM_CONDARG) 51 | {} 52 | 53 | MCPdfVector::~MCPdfVector() {} 54 | 55 | WeightedSample 56 | MCPdfVector::SampleGet(unsigned int particle) const 57 | { 58 | assert(static_cast(particle) >= 0 && particle < _listOfSamples.size()); 59 | return _listOfSamples[particle]; 60 | } 61 | 62 | tf::Vector3 MCPdfVector::ExpectedValueGet() const 63 | { 64 | tf::Vector3 pos(0, 0, 0); 65 | double current_weight; 66 | std::vector >::const_iterator it_los; 67 | for (it_los = _listOfSamples.begin() ; it_los != _listOfSamples.end() ; it_los++) 68 | { 69 | current_weight = it_los->WeightGet(); 70 | pos += (it_los->ValueGet() * current_weight); 71 | } 72 | 73 | return tf::Vector3(pos); 74 | } 75 | 76 | /// Get evenly distributed particle cloud 77 | void MCPdfVector::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const 78 | { 79 | unsigned int num_samples = _listOfSamples.size(); 80 | assert(num_samples > 0); 81 | tf::Vector3 m = _listOfSamples[0].ValueGet(); 82 | tf::Vector3 M = _listOfSamples[0].ValueGet(); 83 | 84 | // calculate min and max 85 | for (unsigned int s = 0; s < num_samples; s++) 86 | { 87 | tf::Vector3 v = _listOfSamples[s].ValueGet(); 88 | for (unsigned int i = 0; i < 3; i++) 89 | { 90 | if (v[i] < m[i]) m[i] = v[i]; 91 | if (v[i] > M[i]) M[i] = v[i]; 92 | } 93 | } 94 | 95 | // get point cloud from histogram 96 | Matrix hist = getHistogram(m, M, step); 97 | unsigned int row = hist.rows(); 98 | unsigned int col = hist.columns(); 99 | unsigned int total = 0; 100 | unsigned int t = 0; 101 | for (unsigned int r = 1; r <= row; r++) 102 | for (unsigned int c = 1; c <= col; c++) 103 | if (hist(r, c) > threshold) total++; 104 | std::cout << "size total " << total << std::endl; 105 | 106 | std::vector points(total); 107 | std::vector weights(total); 108 | sensor_msgs::ChannelFloat32 channel; 109 | for (unsigned int r = 1; r <= row; r++) 110 | for (unsigned int c = 1; c <= col; c++) 111 | if (hist(r, c) > threshold) 112 | { 113 | for (unsigned int i = 0; i < 3; i++) 114 | points[t].x = m[0] + (step[0] * r); 115 | points[t].y = m[1] + (step[1] * c); 116 | points[t].z = m[2]; 117 | weights[t] = rgb[999 - static_cast(trunc(std::max(0.0, std::min(999.0, hist(r, c) * 2 * total * total))))]; 118 | t++; 119 | } 120 | std::cout << "points size " << points.size() << std::endl; 121 | cloud.header.frame_id = "base_link"; 122 | cloud.points = points; 123 | channel.name = "rgb"; 124 | channel.values = weights; 125 | cloud.channels.push_back(channel); 126 | } 127 | 128 | /// Get histogram from pos 129 | MatrixWrapper::Matrix MCPdfVector::getHistogram(const tf::Vector3& m, 130 | const tf::Vector3& M, 131 | const tf::Vector3& step) const 132 | { 133 | unsigned int num_samples = _listOfSamples.size(); 134 | unsigned int rows = round((M[0] - m[0]) / step[0]); 135 | unsigned int cols = round((M[1] - m[1]) / step[1]); 136 | Matrix hist(rows, cols); 137 | hist = 0; 138 | 139 | // calculate histogram 140 | for (unsigned int i = 0; i < num_samples; i++) 141 | { 142 | tf::Vector3 rel(_listOfSamples[i].ValueGet() - m); 143 | unsigned int r = round(rel[0] / step[0]); 144 | unsigned int c = round(rel[1] / step[1]); 145 | if (r >= 1 && c >= 1 && r <= rows && c <= cols) 146 | hist(r, c) += _listOfSamples[i].WeightGet(); 147 | } 148 | 149 | return hist; 150 | } 151 | 152 | unsigned int 153 | MCPdfVector::numParticlesGet() const 154 | { 155 | return _listOfSamples.size(); 156 | } 157 | } // namespace BFL 158 | -------------------------------------------------------------------------------- /people_tracking_filter/src/mcpdf_pos_vel.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | namespace BFL 45 | { 46 | static const unsigned int NUM_CONDARG = 1; 47 | 48 | MCPdfPosVel::MCPdfPosVel(unsigned int num_samples) 49 | : MCPdf (num_samples, NUM_CONDARG) 50 | {} 51 | 52 | MCPdfPosVel::~MCPdfPosVel() {} 53 | 54 | WeightedSample 55 | MCPdfPosVel::SampleGet(unsigned int particle) const 56 | { 57 | assert(static_cast(particle) >= 0 && particle < _listOfSamples.size()); 58 | return _listOfSamples[particle]; 59 | } 60 | 61 | StatePosVel MCPdfPosVel::ExpectedValueGet() const 62 | { 63 | tf::Vector3 pos(0, 0, 0); 64 | tf::Vector3 vel(0, 0, 0); 65 | double current_weight; 66 | std::vector >::const_iterator it_los; 67 | for (it_los = _listOfSamples.begin() ; it_los != _listOfSamples.end() ; it_los++) 68 | { 69 | current_weight = it_los->WeightGet(); 70 | pos += (it_los->ValueGet().pos_ * current_weight); 71 | vel += (it_los->ValueGet().vel_ * current_weight); 72 | } 73 | return StatePosVel(pos, vel); 74 | } 75 | 76 | /// Get evenly distributed particle cloud 77 | void MCPdfPosVel::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const 78 | { 79 | unsigned int num_samples = _listOfSamples.size(); 80 | assert(num_samples > 0); 81 | tf::Vector3 m = _listOfSamples[0].ValueGet().pos_; 82 | tf::Vector3 M = _listOfSamples[0].ValueGet().pos_; 83 | 84 | // calculate min and max 85 | for (unsigned int s = 0; s < num_samples; s++) 86 | { 87 | tf::Vector3 v = _listOfSamples[s].ValueGet().pos_; 88 | for (unsigned int i = 0; i < 3; i++) 89 | { 90 | if (v[i] < m[i]) m[i] = v[i]; 91 | if (v[i] > M[i]) M[i] = v[i]; 92 | } 93 | } 94 | 95 | // get point cloud from histogram 96 | Matrix hist = getHistogramPos(m, M, step); 97 | unsigned int row = hist.rows(); 98 | unsigned int col = hist.columns(); 99 | unsigned int total = 0; 100 | unsigned int t = 0; 101 | for (unsigned int r = 1; r <= row; r++) 102 | for (unsigned int c = 1; c <= col; c++) 103 | if (hist(r, c) > threshold) total++; 104 | 105 | std::vector points(total); 106 | std::vector weights(total); 107 | sensor_msgs::ChannelFloat32 channel; 108 | for (unsigned int r = 1; r <= row; r++) 109 | for (unsigned int c = 1; c <= col; c++) 110 | if (hist(r, c) > threshold) 111 | { 112 | for (unsigned int i = 0; i < 3; i++) 113 | points[t].x = m[0] + (step[0] * r); 114 | points[t].y = m[1] + (step[1] * c); 115 | points[t].z = m[2]; 116 | weights[t] = rgb[999 - static_cast(trunc(std::max(0.0, std::min(999.0, hist(r, c) * 2 * total * total))))]; 117 | t++; 118 | } 119 | cloud.header.frame_id = "odom_combined"; 120 | cloud.points = points; 121 | channel.name = "rgb"; 122 | channel.values = weights; 123 | cloud.channels.push_back(channel); 124 | } 125 | 126 | /// Get histogram from pos 127 | MatrixWrapper::Matrix MCPdfPosVel::getHistogramPos(const tf::Vector3& m, 128 | const tf::Vector3& M, 129 | const tf::Vector3& step) const 130 | { 131 | return getHistogram(m, M, step, true); 132 | } 133 | 134 | /// Get histogram from vel 135 | MatrixWrapper::Matrix MCPdfPosVel::getHistogramVel(const tf::Vector3& m, 136 | const tf::Vector3& M, 137 | const tf::Vector3& step) const 138 | { 139 | return getHistogram(m, M, step, false); 140 | } 141 | 142 | /// Get histogram from certain area 143 | MatrixWrapper::Matrix MCPdfPosVel::getHistogram(const tf::Vector3& m, const tf::Vector3& M, const tf::Vector3& step, 144 | bool pos_hist) const 145 | { 146 | unsigned int num_samples = _listOfSamples.size(); 147 | unsigned int rows = round((M[0] - m[0]) / step[0]); 148 | unsigned int cols = round((M[1] - m[1]) / step[1]); 149 | Matrix hist(rows, cols); 150 | hist = 0; 151 | 152 | // calculate histogram 153 | for (unsigned int i = 0; i < num_samples; i++) 154 | { 155 | tf::Vector3 rel; 156 | if (pos_hist) 157 | rel = _listOfSamples[i].ValueGet().pos_ - m; 158 | else 159 | rel = _listOfSamples[i].ValueGet().vel_ - m; 160 | 161 | unsigned int r = round(rel[0] / step[0]); 162 | unsigned int c = round(rel[1] / step[1]); 163 | if (r >= 1 && c >= 1 && r <= rows && c <= cols) 164 | hist(r, c) += _listOfSamples[i].WeightGet(); 165 | } 166 | 167 | return hist; 168 | } 169 | 170 | unsigned int 171 | MCPdfPosVel::numParticlesGet() const 172 | { 173 | return _listOfSamples.size(); 174 | } 175 | } // namespace BFL 176 | -------------------------------------------------------------------------------- /people_tracking_filter/src/tracker_kalman.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | static const double damping_velocity = 0.9; 42 | 43 | namespace estimation 44 | { 45 | 46 | using MatrixWrapper::Matrix; 47 | using MatrixWrapper::ColumnVector; 48 | using MatrixWrapper::SymmetricMatrix; 49 | 50 | // constructor 51 | TrackerKalman::TrackerKalman(const std::string& name, const BFL::StatePosVel& sysnoise): 52 | Tracker(name), 53 | filter_(NULL), 54 | sys_pdf_(NULL), 55 | sys_model_(NULL), 56 | meas_pdf_(NULL), 57 | meas_model_(NULL), 58 | sys_matrix_(6, 6), 59 | tracker_initialized_(false) 60 | { 61 | // create sys model 62 | sys_matrix_ = 0; 63 | for (unsigned int i = 1; i <= 3; i++) 64 | { 65 | sys_matrix_(i, i) = 1; 66 | sys_matrix_(i + 3, i + 3) = damping_velocity; 67 | } 68 | ColumnVector sys_mu(6); 69 | sys_mu = 0; 70 | sys_sigma_ = SymmetricMatrix(6); 71 | sys_sigma_ = 0; 72 | for (unsigned int i = 0; i < 3; i++) 73 | { 74 | sys_sigma_(i + 1, i + 1) = pow(sysnoise.pos_[i], 2); 75 | sys_sigma_(i + 4, i + 4) = pow(sysnoise.vel_[i], 2); 76 | } 77 | BFL::Gaussian sys_noise(sys_mu, sys_sigma_); 78 | sys_pdf_ = new BFL::LinearAnalyticConditionalGaussian(sys_matrix_, sys_noise); 79 | sys_model_ = new BFL::LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_); 80 | 81 | // create meas model 82 | Matrix meas_matrix(3, 6); 83 | meas_matrix = 0; 84 | for (unsigned int i = 1; i <= 3; i++) 85 | meas_matrix(i, i) = 1; 86 | 87 | ColumnVector meas_mu(3); 88 | meas_mu = 0; 89 | SymmetricMatrix meas_sigma(3); 90 | meas_sigma = 0; 91 | for (unsigned int i = 0; i < 3; i++) 92 | meas_sigma(i + 1, i + 1) = 0; 93 | BFL::Gaussian meas_noise(meas_mu, meas_sigma); 94 | meas_pdf_ = new BFL::LinearAnalyticConditionalGaussian(meas_matrix, meas_noise); 95 | meas_model_ = new BFL::LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_); 96 | }; 97 | 98 | // destructor 99 | TrackerKalman::~TrackerKalman() 100 | { 101 | if (filter_) delete filter_; 102 | if (sys_pdf_) delete sys_pdf_; 103 | if (sys_model_) delete sys_model_; 104 | if (meas_pdf_) delete meas_pdf_; 105 | if (meas_model_) delete meas_model_; 106 | }; 107 | 108 | // initialize prior density of filter 109 | void TrackerKalman::initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time) 110 | { 111 | ColumnVector mu_vec(6); 112 | SymmetricMatrix sigma_vec(6); 113 | sigma_vec = 0; 114 | for (unsigned int i = 0; i < 3; i++) 115 | { 116 | mu_vec(i + 1) = mu.pos_[i]; 117 | mu_vec(i + 4) = mu.vel_[i]; 118 | sigma_vec(i + 1, i + 1) = pow(sigma.pos_[i], 2); 119 | sigma_vec(i + 4, i + 4) = pow(sigma.vel_[i], 2); 120 | } 121 | prior_ = BFL::Gaussian(mu_vec, sigma_vec); 122 | filter_ = new BFL::ExtendedKalmanFilter(&prior_); 123 | 124 | // tracker initialized 125 | tracker_initialized_ = true; 126 | quality_ = 1; 127 | filter_time_ = time; 128 | init_time_ = time; 129 | } 130 | 131 | // update filter prediction 132 | bool TrackerKalman::updatePrediction(const double time) 133 | { 134 | bool res = true; 135 | if (time > filter_time_) 136 | { 137 | // set dt in sys model 138 | for (unsigned int i = 1; i <= 3; i++) 139 | sys_matrix_(i, i + 3) = time - filter_time_; 140 | sys_pdf_->MatrixSet(0, sys_matrix_); 141 | 142 | // scale system noise for dt 143 | sys_pdf_->AdditiveNoiseSigmaSet(sys_sigma_ * pow(time - filter_time_, 2)); 144 | filter_time_ = time; 145 | 146 | // update filter 147 | res = filter_->Update(sys_model_); 148 | if (!res) quality_ = 0; 149 | else quality_ = calculateQuality(); 150 | } 151 | return res; 152 | }; 153 | 154 | // update filter correction 155 | bool TrackerKalman::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov) 156 | { 157 | assert(cov.columns() == 3); 158 | 159 | // copy measurement 160 | ColumnVector meas_vec(3); 161 | for (unsigned int i = 0; i < 3; i++) 162 | meas_vec(i + 1) = meas[i]; 163 | 164 | // set covariance 165 | static_cast(meas_model_->MeasurementPdfGet())->AdditiveNoiseSigmaSet(cov); 166 | 167 | // update filter 168 | bool res = filter_->Update(meas_model_, meas_vec); 169 | if (!res) quality_ = 0; 170 | else quality_ = calculateQuality(); 171 | 172 | return res; 173 | }; 174 | 175 | void TrackerKalman::getEstimate(BFL::StatePosVel& est) const 176 | { 177 | ColumnVector tmp = filter_->PostGet()->ExpectedValueGet(); 178 | for (unsigned int i = 0; i < 3; i++) 179 | { 180 | est.pos_[i] = tmp(i + 1); 181 | est.vel_[i] = tmp(i + 4); 182 | } 183 | }; 184 | 185 | void TrackerKalman::getEstimate(people_msgs::PositionMeasurement& est) const 186 | { 187 | ColumnVector tmp = filter_->PostGet()->ExpectedValueGet(); 188 | 189 | est.pos.x = tmp(1); 190 | est.pos.y = tmp(2); 191 | est.pos.z = tmp(3); 192 | 193 | est.header.stamp.fromSec(filter_time_); 194 | est.object_id = getName(); 195 | } 196 | 197 | double TrackerKalman::calculateQuality() 198 | { 199 | double sigma_max = 0; 200 | SymmetricMatrix cov = filter_->PostGet()->CovarianceGet(); 201 | for (unsigned int i = 1; i <= 2; i++) 202 | sigma_max = std::max(sigma_max, sqrt(cov(i, i))); 203 | 204 | return 1.0 - std::min(1.0, sigma_max / 1.5); 205 | } 206 | 207 | double TrackerKalman::getLifetime() const 208 | { 209 | if (tracker_initialized_) 210 | return filter_time_ - init_time_; 211 | else 212 | return 0; 213 | } 214 | 215 | double TrackerKalman::getTime() const 216 | { 217 | if (tracker_initialized_) 218 | return filter_time_; 219 | else 220 | return 0; 221 | } 222 | } // namespace estimation 223 | -------------------------------------------------------------------------------- /face_detector/include/face_detector/faces.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Face-specific computer vision algorithms. 3 | * 4 | ********************************************************************** 5 | * 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2008, Willow Garage, Inc. 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions 13 | * are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright 16 | * notice, this list of conditions and the following disclaimer. 17 | * * Redistributions in binary form must reproduce the above 18 | * copyright notice, this list of conditions and the following 19 | * disclaimer in the documentation and/or other materials provided 20 | * with the distribution. 21 | * * Neither the name of the Willow Garage nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 26 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 27 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 28 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 29 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 30 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 31 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 32 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 34 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 35 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | * POSSIBILITY OF SUCH DAMAGE. 37 | *********************************************************************/ 38 | 39 | /* Author: Caroline Pantofaru */ 40 | 41 | #ifndef FACE_DETECTOR_FACES_H 42 | #define FACE_DETECTOR_FACES_H 43 | 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | 57 | #define FACE_SIZE_MIN_M 0.1 /**< Default minimum face size, in meters. Only use this for initialization. */ 58 | #define FACE_SIZE_MAX_M 0.5 /**< Default maximum face size, in meters. Only use this for initialization. */ 59 | #define MAX_FACE_Z_M 8.0 /**< Default maximum distance from the camera, in meters. Only use for initialization. */ 60 | // Default thresholds for face tracking. 61 | #define FACE_SEP_DIST_M 1.0 /**< Default separation distance for associating faces. Only use this for initialization. */ 62 | 63 | namespace people 64 | { 65 | 66 | /*! 67 | * \brief A structure for holding information about boxes in 2d and 3d. 68 | */ 69 | struct Box2D3D 70 | { 71 | cv::Point2d center2d; 72 | cv::Point3d center3d; 73 | double width2d; 74 | double width3d; 75 | cv::Rect box2d; 76 | std::string status; 77 | int id; 78 | }; 79 | 80 | /*! 81 | * \brief A structure containing the person's identifying data. 82 | */ 83 | struct Face 84 | { 85 | std::string id; 86 | std::string name; 87 | }; 88 | 89 | /*! 90 | * \brief Contains a list of faces and functions that can be performed on that list. 91 | * This includes utility tasks such as set/get data, to more complicated tasks such as detection or tracking. 92 | */ 93 | class Faces 94 | { 95 | public: 96 | // Default thresholds for the face detection algorithm. 97 | 98 | // Thresholds for the face detection algorithm. 99 | double face_size_min_m_; /**< Minimum face size, in meters. */ 100 | double face_size_max_m_; /**< Maximum face size, in meters. */ 101 | double max_face_z_m_; /**< Maximum distance from the camera, in meters. */ 102 | // Thresholds for face tracking. 103 | double face_sep_dist_m_; /**< Separation distance for associating faces. */ 104 | 105 | // Create an empty list of people. 106 | Faces(); 107 | 108 | // Destroy a list of people. 109 | ~Faces(); 110 | 111 | /*! 112 | * \brief Detect all faces in an image and disparity image. 113 | * 114 | * Input: 115 | * \param image The image in which to detect faces. 116 | * \param haar_classifier_filename Path to the xml file containing the trained haar classifier cascade. 117 | * \param threshold Detection threshold. Currently unused. 118 | * \param disparity_image Image of disparities (from stereo). 119 | * \param cam_model The camera model used to convert 2D points to 3D points. 120 | * Output: 121 | * A vector of Box2D3Ds containing the bounding boxes around found faces in 2D and 3D. 122 | */ 123 | std::vector detectAllFacesDisparity(const cv::Mat &image, double threshold, const cv::Mat &disparity_image, 124 | image_geometry::StereoCameraModel *cam_model); 125 | 126 | /*! 127 | * \brief Detect all faces in an image and depth image. 128 | * 129 | * Input: 130 | * \param image The image in which to detect faces. 131 | * \param haar_classifier_filename Path to the xml file containing the trained haar classifier cascade. 132 | * \param threshold Detection threshold. Currently unused. 133 | * \param depth_image Image of depth (e.g. from an RGBD camera). 134 | * \param cam_model The camera model used to convert 2D points to 3D points. 135 | * Output: 136 | * A vector of Box2D3Ds containing the bounding boxes around found faces in 2D and 3D. 137 | */ 138 | std::vector detectAllFacesDepth(const cv::Mat &image, double threshold, const cv::Mat &depth_image, 139 | image_geometry::StereoCameraModel *cam_model); 140 | 141 | /*! 142 | * \brief Initialize the face detector with images and disparities. 143 | * 144 | * Initialize the face detector, including loading in the classifier cascade. 145 | * Input: 146 | * \param num_cascades Should always be 1 (may change in the future.) 147 | * \param haar_classifier_filename Full path to the cascade file. 148 | */ 149 | void initFaceDetectionDisparity(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m, 150 | double face_size_max_m, double max_face_z_m, double face_sep_dist_m); 151 | 152 | /*! 153 | * \brief Initialize the face detector with images and depth. 154 | * 155 | * Initialize the face detector, including loading in the classifier cascade. 156 | * Input: 157 | * \param num_cascades Should always be 1 (may change in the future.) 158 | * \param haar_classifier_filename Full path to the cascade file. 159 | */ 160 | void initFaceDetectionDepth(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m, 161 | double face_size_max_m, double max_face_z_m, double face_sep_dist_m); 162 | 163 | //////////////////// 164 | private: 165 | std::vector list_; /**< The list of face ids. */ 166 | std::vector faces_; /**< The list of face positions. */ 167 | 168 | cv::Mat cv_image_gray_; /**< Grayscale image */ 169 | cv::Mat const* disparity_image_; /**< Disparity image */ 170 | cv::Mat const* depth_image_; /**< Depth image */ 171 | image_geometry::StereoCameraModel *cam_model_; /**< The stereo camera model for 2D-->3D conversions. */ 172 | 173 | boost::mutex face_mutex_, face_done_mutex_, t_mutex_; 174 | boost::mutex* face_go_mutex_; 175 | boost::thread_group threads_; 176 | boost::condition face_detection_ready_cond_, face_detection_done_cond_; 177 | int num_threads_to_wait_for_; 178 | int images_ready_; 179 | 180 | /* Structures for the face detector. */ 181 | cv::CascadeClassifier cascade_; /**< Classifier cascade for face detection. */ 182 | 183 | void faceDetectionThreadDisparity(uint i); 184 | void faceDetectionThreadDepth(uint i); 185 | }; 186 | } // namespace people 187 | 188 | #endif // FACE_DETECTOR_FACES_H 189 | -------------------------------------------------------------------------------- /leg_detector/src/laser_processor.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | namespace laser_processor 42 | { 43 | 44 | Sample* Sample::Extract(int ind, const sensor_msgs::LaserScan& scan) 45 | { 46 | Sample* s = new Sample(); 47 | 48 | s->index = ind; 49 | s->range = scan.ranges[ind]; 50 | s->x = cos(scan.angle_min + ind * scan.angle_increment) * s->range; 51 | s->y = sin(scan.angle_min + ind * scan.angle_increment) * s->range; 52 | if (s->range > scan.range_min && s->range < scan.range_max) 53 | return s; 54 | else 55 | { 56 | delete s; 57 | return NULL; 58 | } 59 | } 60 | 61 | void SampleSet::clear() 62 | { 63 | for (SampleSet::iterator i = begin(); 64 | i != end(); 65 | i++) 66 | { 67 | delete *i; 68 | } 69 | set::clear(); 70 | } 71 | 72 | void SampleSet::appendToCloud(sensor_msgs::PointCloud& cloud, int r, int g, int b) 73 | { 74 | float color_val = 0; 75 | 76 | int rgb = (r << 16) | (g << 8) | b; 77 | color_val = *(float*) & (rgb); // NOLINT(readability/casting) 78 | 79 | for (iterator sample_iter = begin(); 80 | sample_iter != end(); 81 | sample_iter++) 82 | { 83 | geometry_msgs::Point32 point; 84 | point.x = (*sample_iter)->x; 85 | point.y = (*sample_iter)->y; 86 | point.z = 0; 87 | 88 | cloud.points.push_back(point); 89 | 90 | if (cloud.channels[0].name == "rgb") 91 | cloud.channels[0].values.push_back(color_val); 92 | } 93 | } 94 | 95 | tf::Point SampleSet::center() 96 | { 97 | float x_mean = 0.0; 98 | float y_mean = 0.0; 99 | for (iterator i = begin(); 100 | i != end(); 101 | i++) 102 | 103 | { 104 | x_mean += ((*i)->x) / size(); 105 | y_mean += ((*i)->y) / size(); 106 | } 107 | 108 | return tf::Point(x_mean, y_mean, 0.0); 109 | } 110 | 111 | 112 | void ScanMask::addScan(sensor_msgs::LaserScan& scan) 113 | { 114 | if (!filled) 115 | { 116 | angle_min = scan.angle_min; 117 | angle_max = scan.angle_max; 118 | size = scan.ranges.size(); 119 | filled = true; 120 | } 121 | else if (angle_min != scan.angle_min || 122 | angle_max != scan.angle_max || 123 | size != scan.ranges.size()) 124 | { 125 | throw std::runtime_error("laser_scan::ScanMask::addScan: inconsistantly sized scans added to mask"); 126 | } 127 | 128 | for (uint32_t i = 0; i < scan.ranges.size(); i++) 129 | { 130 | Sample* s = Sample::Extract(i, scan); 131 | 132 | if (s != NULL) 133 | { 134 | SampleSet::iterator m = mask_.find(s); 135 | 136 | if (m != mask_.end()) 137 | { 138 | if ((*m)->range > s->range) 139 | { 140 | delete *m; 141 | mask_.erase(m); 142 | mask_.insert(s); 143 | } 144 | else 145 | { 146 | delete s; 147 | } 148 | } 149 | else 150 | { 151 | mask_.insert(s); 152 | } 153 | } 154 | } 155 | } 156 | 157 | 158 | bool ScanMask::hasSample(Sample* s, float thresh) 159 | { 160 | if (s != NULL) 161 | { 162 | SampleSet::iterator m = mask_.find(s); 163 | if (m != mask_.end()) 164 | if (((*m)->range - thresh) < s->range) 165 | return true; 166 | } 167 | return false; 168 | } 169 | 170 | 171 | 172 | ScanProcessor::ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold) 173 | { 174 | scan_ = scan; 175 | 176 | SampleSet* cluster = new SampleSet; 177 | 178 | for (uint32_t i = 0; i < scan.ranges.size(); i++) 179 | { 180 | Sample* s = Sample::Extract(i, scan); 181 | 182 | if (s != NULL) 183 | { 184 | if (!mask_.hasSample(s, mask_threshold)) 185 | { 186 | cluster->insert(s); 187 | } 188 | else 189 | { 190 | delete s; 191 | } 192 | } 193 | } 194 | 195 | clusters_.push_back(cluster); 196 | } 197 | 198 | ScanProcessor::~ScanProcessor() 199 | { 200 | for (std::list::iterator c = clusters_.begin(); 201 | c != clusters_.end(); 202 | c++) 203 | delete *c; 204 | } 205 | 206 | void 207 | ScanProcessor::removeLessThan(uint32_t num) 208 | { 209 | std::list::iterator c_iter = clusters_.begin(); 210 | while (c_iter != clusters_.end()) 211 | { 212 | if ((*c_iter)->size() < num) 213 | { 214 | delete *c_iter; 215 | clusters_.erase(c_iter++); 216 | } 217 | else 218 | { 219 | ++c_iter; 220 | } 221 | } 222 | } 223 | 224 | 225 | void 226 | ScanProcessor::splitConnected(float thresh) 227 | { 228 | std::list tmp_clusters; 229 | 230 | std::list::iterator c_iter = clusters_.begin(); 231 | 232 | // For each cluster 233 | while (c_iter != clusters_.end()) 234 | { 235 | // Go through the entire list 236 | while ((*c_iter)->size() > 0) 237 | { 238 | // Take the first element 239 | SampleSet::iterator s_first = (*c_iter)->begin(); 240 | 241 | // Start a new queue 242 | std::list sample_queue; 243 | sample_queue.push_back(*s_first); 244 | 245 | (*c_iter)->erase(s_first); 246 | 247 | // Grow until we get to the end of the queue 248 | std::list::iterator s_q = sample_queue.begin(); 249 | while (s_q != sample_queue.end()) 250 | { 251 | int expand = static_cast(asin(thresh / (*s_q)->range) / std::abs(scan_.angle_increment)); 252 | 253 | SampleSet::iterator s_rest = (*c_iter)->begin(); 254 | 255 | while ((s_rest != (*c_iter)->end() && 256 | (*s_rest)->index < (*s_q)->index + expand)) 257 | { 258 | if ((*s_rest)->range - (*s_q)->range > thresh) 259 | { 260 | break; 261 | } 262 | else if (sqrt(pow((*s_q)->x - (*s_rest)->x, 2.0f) + pow((*s_q)->y - (*s_rest)->y, 2.0f)) < thresh) 263 | { 264 | sample_queue.push_back(*s_rest); 265 | (*c_iter)->erase(s_rest++); 266 | break; 267 | } 268 | else 269 | { 270 | ++s_rest; 271 | } 272 | } 273 | s_q++; 274 | } 275 | 276 | // Move all the samples into the new cluster 277 | SampleSet* c = new SampleSet; 278 | for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++) 279 | c->insert(*s_q); 280 | 281 | // Store the temporary clusters 282 | tmp_clusters.push_back(c); 283 | } 284 | 285 | // Now that c_iter is empty, we can delete 286 | delete *c_iter; 287 | 288 | // And remove from the map 289 | clusters_.erase(c_iter++); 290 | } 291 | 292 | clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end()); 293 | } 294 | 295 | } // namespace laser_processor 296 | -------------------------------------------------------------------------------- /leg_detector/src/train_leg_detector.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | 49 | enum LoadType {LOADING_NONE, LOADING_POS, LOADING_NEG, LOADING_TEST}; 50 | 51 | class TrainLegDetector 52 | { 53 | public: 54 | laser_processor::ScanMask mask_; 55 | int mask_count_; 56 | 57 | std::vector< std::vector > pos_data_; 58 | std::vector< std::vector > neg_data_; 59 | std::vector< std::vector > test_data_; 60 | 61 | CvRTrees forest; 62 | 63 | float connected_thresh_; 64 | 65 | int feat_count_; 66 | 67 | TrainLegDetector() : mask_count_(0), connected_thresh_(0.06), feat_count_(0) 68 | { 69 | } 70 | 71 | void loadData(LoadType load, char* file) 72 | { 73 | if (load != LOADING_NONE) 74 | { 75 | switch (load) 76 | { 77 | case LOADING_POS: 78 | printf("Loading positive training data from file: %s\n", file); 79 | break; 80 | case LOADING_NEG: 81 | printf("Loading negative training data from file: %s\n", file); 82 | break; 83 | case LOADING_TEST: 84 | printf("Loading test data from file: %s\n", file); 85 | break; 86 | default: 87 | break; 88 | } 89 | 90 | ros::record::Player p; 91 | if (p.open(file, ros::Time())) 92 | { 93 | mask_.clear(); 94 | mask_count_ = 0; 95 | 96 | switch (load) 97 | { 98 | case LOADING_POS: 99 | p.addHandler(std::string("*"), &TrainLegDetector::loadCb, this, &pos_data_); 100 | break; 101 | case LOADING_NEG: 102 | mask_count_ = 1000; // effectively disable masking 103 | p.addHandler(std::string("*"), &TrainLegDetector::loadCb, this, &neg_data_); 104 | break; 105 | case LOADING_TEST: 106 | p.addHandler(std::string("*"), &TrainLegDetector::loadCb, this, &test_data_); 107 | break; 108 | default: 109 | break; 110 | } 111 | 112 | while (p.nextMsg()) 113 | {} 114 | } 115 | } 116 | } 117 | 118 | void loadCb(std::string name, sensor_msgs::LaserScan* scan, ros::Time t, ros::Time t_no_use, void* n) 119 | { 120 | std::vector< std::vector >* data = (std::vector< std::vector >*)(n); 121 | 122 | if (mask_count_++ < 20) 123 | { 124 | mask_.addScan(*scan); 125 | } 126 | else 127 | { 128 | laser_processor::ScanProcessor processor(*scan, mask_); 129 | processor.splitConnected(connected_thresh_); 130 | processor.removeLessThan(5); 131 | 132 | for (std::list::iterator i = processor.getClusters().begin(); 133 | i != processor.getClusters().end(); 134 | i++) 135 | data->push_back(calcLegFeatures(*i, *scan)); 136 | } 137 | } 138 | 139 | void train() 140 | { 141 | int sample_size = pos_data_.size() + neg_data_.size(); 142 | feat_count_ = pos_data_[0].size(); 143 | 144 | CvMat* cv_data = cvCreateMat(sample_size, feat_count_, CV_32FC1); 145 | CvMat* cv_resp = cvCreateMat(sample_size, 1, CV_32S); 146 | 147 | // Put positive data in opencv format. 148 | int j = 0; 149 | for (std::vector< std::vector >::iterator i = pos_data_.begin(); 150 | i != pos_data_.end(); 151 | i++) 152 | { 153 | float* data_row = reinterpret_cast(cv_data->data.ptr + cv_data->step * j); 154 | for (int k = 0; k < feat_count_; k++) 155 | data_row[k] = (*i)[k]; 156 | 157 | cv_resp->data.i[j] = 1; 158 | j++; 159 | } 160 | 161 | // Put negative data in opencv format. 162 | for (std::vector< std::vector >::iterator i = neg_data_.begin(); 163 | i != neg_data_.end(); 164 | i++) 165 | { 166 | float* data_row = reinterpret_cast(cv_data->data.ptr + cv_data->step * j); 167 | for (int k = 0; k < feat_count_; k++) 168 | data_row[k] = (*i)[k]; 169 | 170 | cv_resp->data.i[j] = -1; 171 | j++; 172 | } 173 | 174 | CvMat* var_type = cvCreateMat(1, feat_count_ + 1, CV_8U); 175 | cvSet(var_type, cvScalarAll(CV_VAR_ORDERED)); 176 | cvSetReal1D(var_type, feat_count_, CV_VAR_CATEGORICAL); 177 | 178 | float priors[] = {1.0, 1.0}; 179 | 180 | CvRTParams fparam(8, 20, 0, false, 10, priors, false, 5, 50, 0.001f, CV_TERMCRIT_ITER); 181 | fparam.term_crit = cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1); 182 | 183 | forest.train(cv_data, CV_ROW_SAMPLE, cv_resp, 0, 0, var_type, 0, 184 | fparam); 185 | 186 | 187 | cvReleaseMat(&cv_data); 188 | cvReleaseMat(&cv_resp); 189 | cvReleaseMat(&var_type); 190 | } 191 | 192 | void test() 193 | { 194 | CvMat* tmp_mat = cvCreateMat(1, feat_count_, CV_32FC1); 195 | 196 | int pos_right = 0; 197 | int pos_total = 0; 198 | for (std::vector< std::vector >::iterator i = pos_data_.begin(); 199 | i != pos_data_.end(); 200 | i++) 201 | { 202 | for (int k = 0; k < feat_count_; k++) 203 | tmp_mat->data.fl[k] = static_cast((*i)[k]); 204 | if (forest.predict(tmp_mat) > 0) 205 | pos_right++; 206 | pos_total++; 207 | } 208 | 209 | int neg_right = 0; 210 | int neg_total = 0; 211 | for (std::vector< std::vector >::iterator i = neg_data_.begin(); 212 | i != neg_data_.end(); 213 | i++) 214 | { 215 | for (int k = 0; k < feat_count_; k++) 216 | tmp_mat->data.fl[k] = static_cast((*i)[k]); 217 | if (forest.predict(tmp_mat) < 0) 218 | neg_right++; 219 | neg_total++; 220 | } 221 | 222 | int test_right = 0; 223 | int test_total = 0; 224 | for (std::vector< std::vector >::iterator i = test_data_.begin(); 225 | i != test_data_.end(); 226 | i++) 227 | { 228 | for (int k = 0; k < feat_count_; k++) 229 | tmp_mat->data.fl[k] = static_cast((*i)[k]); 230 | if (forest.predict(tmp_mat) > 0) 231 | test_right++; 232 | test_total++; 233 | } 234 | 235 | printf(" Pos train set: %d/%d %g\n", pos_right, pos_total, static_cast(pos_right) / pos_total); 236 | printf(" Neg train set: %d/%d %g\n", neg_right, neg_total, static_cast(neg_right) / neg_total); 237 | printf(" Test set: %d/%d %g\n", test_right, test_total, static_cast(test_right) / test_total); 238 | 239 | cvReleaseMat(&tmp_mat); 240 | } 241 | 242 | void save(char* file) 243 | { 244 | forest.save(file); 245 | } 246 | }; 247 | 248 | int main(int argc, char **argv) 249 | { 250 | TrainLegDetector tld; 251 | 252 | LoadType loading = LOADING_NONE; 253 | 254 | char save_file[100]; 255 | save_file[0] = 0; 256 | 257 | printf("Loading data...\n"); 258 | for (int i = 1; i < argc; i++) 259 | { 260 | if (!strcmp(argv[i], "--train")) 261 | loading = LOADING_POS; 262 | else if (!strcmp(argv[i], "--neg")) 263 | loading = LOADING_NEG; 264 | else if (!strcmp(argv[i], "--test")) 265 | loading = LOADING_TEST; 266 | else if (!strcmp(argv[i], "--save")) 267 | { 268 | if (++i < argc) 269 | strncpy(save_file, argv[i], 100); 270 | continue; 271 | } 272 | else 273 | tld.loadData(loading, argv[i]); 274 | } 275 | 276 | printf("Training classifier...\n"); 277 | tld.train(); 278 | 279 | printf("Evlauating classifier...\n"); 280 | tld.test(); 281 | 282 | if (strlen(save_file) > 0) 283 | { 284 | printf("Saving classifier as: %s\n", save_file); 285 | tld.save(save_file); 286 | } 287 | } 288 | -------------------------------------------------------------------------------- /leg_detector/src/calc_leg_features.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | std::vector calcLegFeatures(laser_processor::SampleSet* cluster, const sensor_msgs::LaserScan& scan) 43 | { 44 | std::vector features; 45 | 46 | // Number of points 47 | int num_points = cluster->size(); 48 | // features.push_back(num_points); 49 | 50 | // Compute mean and median points for future use 51 | float x_mean = 0.0; 52 | float y_mean = 0.0; 53 | std::vector x_median_set; 54 | std::vector y_median_set; 55 | for (laser_processor::SampleSet::iterator i = cluster->begin(); 56 | i != cluster->end(); 57 | i++) 58 | 59 | { 60 | x_mean += ((*i)->x) / num_points; 61 | y_mean += ((*i)->y) / num_points; 62 | x_median_set.push_back((*i)->x); 63 | y_median_set.push_back((*i)->y); 64 | } 65 | 66 | std::sort(x_median_set.begin(), x_median_set.end()); 67 | std::sort(y_median_set.begin(), y_median_set.end()); 68 | 69 | float x_median = 0.5 * (*(x_median_set.begin() + (num_points - 1) / 2) + * (x_median_set.begin() + num_points / 2)); 70 | float y_median = 0.5 * (*(y_median_set.begin() + (num_points - 1) / 2) + * (y_median_set.begin() + num_points / 2)); 71 | 72 | // Compute std and avg diff from median 73 | 74 | double sum_std_diff = 0.0; 75 | double sum_med_diff = 0.0; 76 | 77 | 78 | for (laser_processor::SampleSet::iterator i = cluster->begin(); 79 | i != cluster->end(); 80 | i++) 81 | 82 | { 83 | sum_std_diff += pow((*i)->x - x_mean, 2) + pow((*i)->y - y_mean, 2); 84 | sum_med_diff += sqrt(pow((*i)->x - x_median, 2) + pow((*i)->y - y_median, 2)); 85 | } 86 | 87 | float std = sqrt(1.0 / (num_points - 1.0) * sum_std_diff); 88 | float avg_median_dev = sum_med_diff / num_points; 89 | 90 | features.push_back(std); 91 | features.push_back(avg_median_dev); 92 | 93 | 94 | // Take first at last 95 | laser_processor::SampleSet::iterator first = cluster->begin(); 96 | laser_processor::SampleSet::iterator last = cluster->end(); 97 | last--; 98 | 99 | // Compute Jump distance 100 | int prev_ind = (*first)->index - 1; 101 | int next_ind = (*last)->index + 1; 102 | 103 | float prev_jump = 0; 104 | float next_jump = 0; 105 | 106 | if (prev_ind >= 0) 107 | { 108 | laser_processor::Sample* prev = laser_processor::Sample::Extract(prev_ind, scan); 109 | if (prev) 110 | { 111 | prev_jump = sqrt(pow((*first)->x - prev->x, 2) + pow((*first)->y - prev->y, 2)); 112 | delete prev; 113 | } 114 | } 115 | 116 | if (next_ind < static_cast(scan.ranges.size())) 117 | { 118 | laser_processor::Sample* next = laser_processor::Sample::Extract(next_ind, scan); 119 | if (next) 120 | { 121 | next_jump = sqrt(pow((*last)->x - next->x, 2) + pow((*last)->y - next->y, 2)); 122 | delete next; 123 | } 124 | } 125 | 126 | features.push_back(prev_jump); 127 | features.push_back(next_jump); 128 | 129 | // Compute Width 130 | float width = sqrt(pow((*first)->x - (*last)->x, 2) + pow((*first)->y - (*last)->y, 2)); 131 | features.push_back(width); 132 | 133 | // Compute Linearity 134 | 135 | CvMat* points = cvCreateMat(num_points, 2, CV_64FC1); 136 | { 137 | int j = 0; 138 | for (laser_processor::SampleSet::iterator i = cluster->begin(); 139 | i != cluster->end(); 140 | i++) 141 | { 142 | cvmSet(points, j, 0, (*i)->x - x_mean); 143 | cvmSet(points, j, 1, (*i)->y - y_mean); 144 | j++; 145 | } 146 | } 147 | 148 | CvMat* W = cvCreateMat(2, 2, CV_64FC1); 149 | CvMat* U = cvCreateMat(num_points, 2, CV_64FC1); 150 | CvMat* V = cvCreateMat(2, 2, CV_64FC1); 151 | cvSVD(points, W, U, V); 152 | 153 | CvMat* rot_points = cvCreateMat(num_points, 2, CV_64FC1); 154 | cvMatMul(U, W, rot_points); 155 | 156 | float linearity = 0.0; 157 | for (int i = 0; i < num_points; i++) 158 | { 159 | linearity += pow(cvmGet(rot_points, i, 1), 2); 160 | } 161 | 162 | cvReleaseMat(&points); 163 | points = 0; 164 | cvReleaseMat(&W); 165 | W = 0; 166 | cvReleaseMat(&U); 167 | U = 0; 168 | cvReleaseMat(&V); 169 | V = 0; 170 | cvReleaseMat(&rot_points); 171 | rot_points = 0; 172 | 173 | features.push_back(linearity); 174 | 175 | // Compute Circularity 176 | CvMat* A = cvCreateMat(num_points, 3, CV_64FC1); 177 | CvMat* B = cvCreateMat(num_points, 1, CV_64FC1); 178 | { 179 | int j = 0; 180 | for (laser_processor::SampleSet::iterator i = cluster->begin(); 181 | i != cluster->end(); 182 | i++) 183 | { 184 | float x = (*i)->x; 185 | float y = (*i)->y; 186 | 187 | cvmSet(A, j, 0, -2.0 * x); 188 | cvmSet(A, j, 1, -2.0 * y); 189 | cvmSet(A, j, 2, 1); 190 | 191 | cvmSet(B, j, 0, -pow(x, 2) - pow(y, 2)); 192 | j++; 193 | } 194 | } 195 | CvMat* sol = cvCreateMat(3, 1, CV_64FC1); 196 | 197 | cvSolve(A, B, sol, CV_SVD); 198 | 199 | float xc = cvmGet(sol, 0, 0); 200 | float yc = cvmGet(sol, 1, 0); 201 | float rc = sqrt(pow(xc, 2) + pow(yc, 2) - cvmGet(sol, 2, 0)); 202 | 203 | cvReleaseMat(&A); 204 | A = 0; 205 | cvReleaseMat(&B); 206 | B = 0; 207 | cvReleaseMat(&sol); 208 | sol = 0; 209 | 210 | float circularity = 0.0; 211 | for (laser_processor::SampleSet::iterator i = cluster->begin(); 212 | i != cluster->end(); 213 | i++) 214 | { 215 | circularity += pow(rc - sqrt(pow(xc - (*i)->x, 2) + pow(yc - (*i)->y, 2)), 2); 216 | } 217 | 218 | features.push_back(circularity); 219 | 220 | // Radius 221 | float radius = rc; 222 | 223 | features.push_back(radius); 224 | 225 | // Curvature: 226 | float mean_curvature = 0.0; 227 | 228 | // Boundary length: 229 | float boundary_length = 0.0; 230 | float last_boundary_seg = 0.0; 231 | 232 | float boundary_regularity = 0.0; 233 | double sum_boundary_reg_sq = 0.0; 234 | 235 | // Mean angular difference 236 | laser_processor::SampleSet::iterator left = cluster->begin(); 237 | left++; 238 | left++; 239 | laser_processor::SampleSet::iterator mid = cluster->begin(); 240 | mid++; 241 | laser_processor::SampleSet::iterator right = cluster->begin(); 242 | 243 | float ang_diff = 0.0; 244 | 245 | while (left != cluster->end()) 246 | { 247 | float mlx = (*left)->x - (*mid)->x; 248 | float mly = (*left)->y - (*mid)->y; 249 | float L_ml = sqrt(mlx * mlx + mly * mly); 250 | 251 | float mrx = (*right)->x - (*mid)->x; 252 | float mry = (*right)->y - (*mid)->y; 253 | float L_mr = sqrt(mrx * mrx + mry * mry); 254 | 255 | float lrx = (*left)->x - (*right)->x; 256 | float lry = (*left)->y - (*right)->y; 257 | float L_lr = sqrt(lrx * lrx + lry * lry); 258 | 259 | boundary_length += L_mr; 260 | sum_boundary_reg_sq += L_mr * L_mr; 261 | last_boundary_seg = L_ml; 262 | 263 | float A = (mlx * mrx + mly * mry) / pow(L_mr, 2); 264 | float B = (mlx * mry - mly * mrx) / pow(L_mr, 2); 265 | 266 | float th = atan2(B, A); 267 | 268 | if (th < 0) 269 | th += 2 * M_PI; 270 | 271 | ang_diff += th / num_points; 272 | 273 | float s = 0.5 * (L_ml + L_mr + L_lr); 274 | float area = sqrt(s * (s - L_ml) * (s - L_mr) * (s - L_lr)); 275 | 276 | if (th > 0) 277 | mean_curvature += 4 * (area) / (L_ml * L_mr * L_lr * num_points); 278 | else 279 | mean_curvature -= 4 * (area) / (L_ml * L_mr * L_lr * num_points); 280 | 281 | left++; 282 | mid++; 283 | right++; 284 | } 285 | 286 | boundary_length += last_boundary_seg; 287 | sum_boundary_reg_sq += last_boundary_seg * last_boundary_seg; 288 | 289 | boundary_regularity = sqrt((sum_boundary_reg_sq - pow(boundary_length, 2) / num_points) / (num_points - 1)); 290 | 291 | features.push_back(boundary_length); 292 | features.push_back(ang_diff); 293 | features.push_back(mean_curvature); 294 | 295 | features.push_back(boundary_regularity); 296 | 297 | 298 | // Mean angular difference 299 | first = cluster->begin(); 300 | mid = cluster->begin(); 301 | mid++; 302 | last = cluster->end(); 303 | last--; 304 | 305 | double sum_iav = 0.0; 306 | double sum_iav_sq = 0.0; 307 | 308 | while (mid != last) 309 | { 310 | float mlx = (*first)->x - (*mid)->x; 311 | float mly = (*first)->y - (*mid)->y; 312 | // float L_ml = sqrt(mlx*mlx + mly*mly); 313 | 314 | float mrx = (*last)->x - (*mid)->x; 315 | float mry = (*last)->y - (*mid)->y; 316 | float L_mr = sqrt(mrx * mrx + mry * mry); 317 | 318 | // float lrx = (*first)->x - (*last)->x; 319 | // float lry = (*first)->y - (*last)->y; 320 | // float L_lr = sqrt(lrx*lrx + lry*lry); 321 | 322 | float A = (mlx * mrx + mly * mry) / pow(L_mr, 2); 323 | float B = (mlx * mry - mly * mrx) / pow(L_mr, 2); 324 | 325 | float th = atan2(B, A); 326 | 327 | if (th < 0) 328 | th += 2 * M_PI; 329 | 330 | sum_iav += th; 331 | sum_iav_sq += th * th; 332 | 333 | mid++; 334 | } 335 | 336 | float iav = sum_iav / num_points; 337 | float std_iav = sqrt((sum_iav_sq - pow(sum_iav, 2) / num_points) / (num_points - 1)); 338 | 339 | features.push_back(iav); 340 | features.push_back(std_iav); 341 | 342 | return features; 343 | } 344 | 345 | --------------------------------------------------------------------------------