├── CMakeLists.txt
├── README.md
├── launch
    ├── dynamic_obs_detector.launch
    ├── frogbag.rviz
    └── my_laser_config.yaml
├── msg
    ├── DynamicObstacle.msg
    ├── DynamicObstacleStamped.msg
    └── DynamicObstacles.msg
├── package.xml
└── src
    ├── dynamic_obstacle_detector.cpp
    ├── laser_clustering.py
    └── obstacle_kf.hpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(dynamic_obstacle_detector)
 3 | 
 4 | 
 5 | find_package(catkin REQUIRED COMPONENTS
 6 |   geometry_msgs
 7 |   laser_geometry
 8 |   roscpp
 9 |   sensor_msgs
10 |   tf2_ros
11 |   message_generation
12 | )
13 | 
14 | 
15 |  add_message_files(
16 |    FILES
17 |    DynamicObstacle.msg
18 |    DynamicObstacleStamped.msg
19 |    DynamicObstacles.msg
20 |  )
21 | 
22 |  generate_messages(
23 |    DEPENDENCIES
24 |    geometry_msgs std_msgs
25 | )
26 | 
27 | catkin_package(
28 | #  INCLUDE_DIRS include
29 | #  LIBRARIES dynamic_obstacle_detector
30 |   CATKIN_DEPENDS geometry_msgs laser_geometry roscpp sensor_msgs tf2_ros message_runtime
31 | #  DEPENDS system_lib
32 | )
33 | 
34 | include_directories(
35 | # include
36 |   ${catkin_INCLUDE_DIRS}
37 | )
38 | 
39 |  add_executable(${PROJECT_NAME}_node src/dynamic_obstacle_detector.cpp)
40 | 
41 |  add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
42 | 
43 |  target_link_libraries(${PROJECT_NAME}_node
44 |    ${catkin_LIBRARIES}
45 |  )
46 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Dynamic Obstacle Detector
 2 | 
 3 | A simple (but effective) detector of dynamic obstacles in laser scans. 
 4 | It detect groups of points in a laser scan that are then tracked using Kalman Filters. Obstacles that do not exceed a minimum velocity threshold are removed. 
 5 | 
 6 | ## Parameters
 7 | 
 8 | - **input_scan_topic**. Name of the scan topic. (Def: "scan").
 9 | - **odom_frame**. Name of the odometry frame. (Def: "odom").
10 | - **cluster_max_distance_points**. Maximum distance [m] between consecutive points in the scan to be grouped together. (Def: 0.6).
11 | - **cluster_min_points**. Minimun number of points in a group to be considered as an obstacle. (Def: 3)
12 | - **cluster_max_points**. Maximum number of points in a group to be considered as an obstacle. (Def: 35)
13 | - **min_vel_tracked**. Minimum velocity [m/s] of the obstacles to be considered as a dynamic obstacle. (Def: 0.35).
14 | - **max_vel_tracked**. Maximum velocity [m/s] of the obstacles to be considered as a dynamic obstacle. (Def: 2.0).
15 | - **max_tracked_distance**. Maximum distance [m] between obstacles to be considered the same one. (Def: 0.55).
16 | - **max_tracked_sec**. Maximum time [sec] between detections of an obstacle to be tracked. (Def: 1.0).
17 | 
18 | 
19 | ## Subscriptions
20 | 
21 | - */scan*: <sensor_msgs::LaserScan>. Laser scan topic. Modified by the parameter **input_scan_topic**.
22 | 
23 | 
24 | ## Publications
25 | 
26 | - */dynamic_obstacles* <dynamic_obstacle_detector::DynamicObstacles>. message with the obstacle information similarly to the people msg.
27 | - */dynamic_obstacles/static_markers* <visualization_msgs::MarkerArray>. RViz marker with the dynamic obstacles candidates.
28 | - */dynamic_obstacles/dynamic_markers* <visualization_msgs::MarkerArray>. Rviz marker with the dynamic obstacles detected.
29 | 
30 | 
31 | 
32 | 
33 | 


--------------------------------------------------------------------------------
/launch/dynamic_obs_detector.launch:
--------------------------------------------------------------------------------
 1 | <!-- -->
 2 | <launch>
 3 | 
 4 |   <arg name="debug" default="false"/>
 5 |   <arg name="launch_prefix" value="gdb -ex run --args" if="$(arg debug)"/> #xterm -e gdb --args
 6 |   <arg name="launch_prefix" value="" unless="$(arg debug)"/>
 7 | 
 8 |   <arg name="frogbag" default="false"/>
 9 |   <arg name="scan_topic" value="scan_filtered_drop" if="$(arg frogbag)"/>
10 |   <arg name="scan_topic" value="scan_raw" unless="$(arg frogbag)"/>
11 | 
12 |   <group if="$(arg frogbag)">
13 | 
14 |     <param name="use_sim_time" value="true" />
15 | 
16 |     <node pkg="rosbag" type="play" name="bagplayer" args="--clock /home/kenny/Downloads/UPO_pioneer_sensors_2014-04-29-11-36-22.bag"/>
17 | 
18 |     <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter">
19 |         <rosparam command="load" file="$(find obstacle_detector)/launch/my_laser_config.yaml" />
20 |         <remap from="scan" to="scanfront" /> #scanfront in FROG bags
21 |     </node>
22 | 
23 |     <node name="topic_drop" pkg="topic_tools" type="drop" args="scan_filtered 3 4" />
24 | 
25 |     <node type="rviz" name="rviz" pkg="rviz" args="-d $(find dynamic_obstacle_detector)/launch/frogbag.rviz" />
26 |   
27 |   </group>
28 | 
29 |   <!--<node type="laser_scan_assembler" pkg="laser_assembler" name="my_assembler">
30 |     <remap from="scan" to="scan_filtered"/>
31 |     <param name="max_scans" type="int" value="1" />
32 |     <param name="fixed_frame" type="string" value="odom" />
33 |   </node>-->
34 |   <include unless="$(arg frogbag)" file="$(find gazebo_sfm_plugin)/launch/tiago_pedestrians.launch" />
35 | 
36 |   <node pkg="dynamic_obstacle_detector" type="dynamic_obstacle_detector_node" name="dynamic_obstacle_detector" output="screen" launch-prefix="$(arg launch_prefix)" >
37 |         <param name="input_scan_topic" value="$(arg scan_topic)" />
38 |         <param name="odom_frame" value="odom" />
39 |         <param name="cluster_max_distance_points" value="0.6" />
40 |         <param name="cluster_min_points" value="3" />
41 |         <param name="cluster_max_points" value="40" />
42 |         <param name="min_vel_tracked" value="0.37" />
43 |         <param name="max_vel_tracked" value="1.8" />
44 |         <param name="max_tracked_distance" value="0.55" />
45 |         <param name="max_tracked_sec" value="0.8" />
46 |   </node>
47 |   
48 |   <!--<node pkg="dynamic_obstacle_detector" type="laser_clustering.py" name="laser_clustering" output="screen" >
49 |         <param name="input_scan_topic" value="$(arg scan_topic)" />
50 |         <param name="dist_from_obs" value="0.2" />
51 |         <param name="cluster_bandwidth" value="0.6"/>
52 |   </node>-->
53 | 
54 | 
55 |   
56 | </launch>
57 | 


--------------------------------------------------------------------------------
/launch/frogbag.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 78
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Status1
  9 |         - /MarkerArray1
 10 |         - /MarkerArray2
 11 |         - /LaserScan1
 12 |         - /Marker1
 13 |       Splitter Ratio: 0.3794117569923401
 14 |     Tree Height: 719
 15 |   - Class: rviz/Selection
 16 |     Name: Selection
 17 |   - Class: rviz/Tool Properties
 18 |     Expanded:
 19 |       - /2D Pose Estimate1
 20 |       - /2D Nav Goal1
 21 |       - /Publish Point1
 22 |     Name: Tool Properties
 23 |     Splitter Ratio: 0.5886790156364441
 24 |   - Class: rviz/Views
 25 |     Expanded:
 26 |       - /Current View1
 27 |     Name: Views
 28 |     Splitter Ratio: 0.5
 29 |   - Class: rviz/Time
 30 |     Experimental: false
 31 |     Name: Time
 32 |     SyncMode: 0
 33 |     SyncSource: LaserScan
 34 | Preferences:
 35 |   PromptSaveOnExit: true
 36 | Toolbars:
 37 |   toolButtonStyle: 2
 38 | Visualization Manager:
 39 |   Class: ""
 40 |   Displays:
 41 |     - Alpha: 0.5
 42 |       Cell Size: 1
 43 |       Class: rviz/Grid
 44 |       Color: 160; 160; 164
 45 |       Enabled: true
 46 |       Line Style:
 47 |         Line Width: 0.029999999329447746
 48 |         Value: Lines
 49 |       Name: Grid
 50 |       Normal Cell Count: 0
 51 |       Offset:
 52 |         X: 0
 53 |         Y: 0
 54 |         Z: 0
 55 |       Plane: XY
 56 |       Plane Cell Count: 10
 57 |       Reference Frame: <Fixed Frame>
 58 |       Value: true
 59 |     - Class: rviz/TF
 60 |       Enabled: true
 61 |       Frame Timeout: 15
 62 |       Frames:
 63 |         All Enabled: true
 64 |         base_link:
 65 |           Value: true
 66 |         bumblebee:
 67 |           Value: true
 68 |         laserback:
 69 |           Value: true
 70 |         laserfront:
 71 |           Value: true
 72 |         laservtcal:
 73 |           Value: true
 74 |         odom:
 75 |           Value: true
 76 |       Marker Alpha: 1
 77 |       Marker Scale: 1
 78 |       Name: TF
 79 |       Show Arrows: true
 80 |       Show Axes: true
 81 |       Show Names: true
 82 |       Tree:
 83 |         odom:
 84 |           base_link:
 85 |             bumblebee:
 86 |               {}
 87 |             laserback:
 88 |               {}
 89 |             laserfront:
 90 |               {}
 91 |             laservtcal:
 92 |               {}
 93 |       Update Interval: 0
 94 |       Value: true
 95 |     - Class: rviz/MarkerArray
 96 |       Enabled: true
 97 |       Marker Topic: /dynamic_obstacles/dynamic_markers
 98 |       Name: MarkerArray
 99 |       Namespaces:
100 |         /dynamic_obstacle_detector: true
101 |       Queue Size: 100
102 |       Value: true
103 |     - Class: rviz/MarkerArray
104 |       Enabled: true
105 |       Marker Topic: /dynamic_obstacles/static_markers
106 |       Name: MarkerArray
107 |       Namespaces:
108 |         obstacles: true
109 |       Queue Size: 100
110 |       Value: true
111 |     - Alpha: 1
112 |       Autocompute Intensity Bounds: true
113 |       Autocompute Value Bounds:
114 |         Max Value: 10
115 |         Min Value: -10
116 |         Value: true
117 |       Axis: Z
118 |       Channel Name: intensity
119 |       Class: rviz/LaserScan
120 |       Color: 173; 127; 168
121 |       Color Transformer: FlatColor
122 |       Decay Time: 0
123 |       Enabled: false
124 |       Invert Rainbow: false
125 |       Max Color: 255; 255; 255
126 |       Min Color: 0; 0; 0
127 |       Name: LaserScan
128 |       Position Transformer: XYZ
129 |       Queue Size: 10
130 |       Selectable: true
131 |       Size (Pixels): 3
132 |       Size (m): 0.05000000074505806
133 |       Style: Flat Squares
134 |       Topic: /scan_filtered_drop
135 |       Unreliable: false
136 |       Use Fixed Frame: true
137 |       Use rainbow: true
138 |       Value: false
139 |     - Alpha: 1
140 |       Autocompute Intensity Bounds: true
141 |       Autocompute Value Bounds:
142 |         Max Value: 10
143 |         Min Value: -10
144 |         Value: true
145 |       Axis: Z
146 |       Channel Name: intensity
147 |       Class: rviz/LaserScan
148 |       Color: 255; 255; 255
149 |       Color Transformer: Intensity
150 |       Decay Time: 0
151 |       Enabled: true
152 |       Invert Rainbow: false
153 |       Max Color: 255; 255; 255
154 |       Min Color: 0; 0; 0
155 |       Name: LaserScan
156 |       Position Transformer: XYZ
157 |       Queue Size: 10
158 |       Selectable: true
159 |       Size (Pixels): 3
160 |       Size (m): 0.009999999776482582
161 |       Style: Flat Squares
162 |       Topic: /scanfront
163 |       Unreliable: false
164 |       Use Fixed Frame: true
165 |       Use rainbow: true
166 |       Value: true
167 |     - Class: rviz/Marker
168 |       Enabled: true
169 |       Marker Topic: /dynamic_obstacles/points
170 |       Name: Marker
171 |       Namespaces:
172 |         dyn_points: true
173 |       Queue Size: 100
174 |       Value: true
175 |   Enabled: true
176 |   Global Options:
177 |     Background Color: 48; 48; 48
178 |     Default Light: true
179 |     Fixed Frame: base_link
180 |     Frame Rate: 30
181 |   Name: root
182 |   Tools:
183 |     - Class: rviz/Interact
184 |       Hide Inactive Objects: true
185 |     - Class: rviz/MoveCamera
186 |     - Class: rviz/Select
187 |     - Class: rviz/FocusCamera
188 |     - Class: rviz/Measure
189 |     - Class: rviz/SetInitialPose
190 |       Theta std deviation: 0.2617993950843811
191 |       Topic: /initialpose
192 |       X std deviation: 0.5
193 |       Y std deviation: 0.5
194 |     - Class: rviz/SetGoal
195 |       Topic: /move_base_simple/goal
196 |     - Class: rviz/PublishPoint
197 |       Single click: true
198 |       Topic: /clicked_point
199 |   Value: true
200 |   Views:
201 |     Current:
202 |       Class: rviz/Orbit
203 |       Distance: 12.843451499938965
204 |       Enable Stereo Rendering:
205 |         Stereo Eye Separation: 0.05999999865889549
206 |         Stereo Focal Distance: 1
207 |         Swap Stereo Eyes: false
208 |         Value: false
209 |       Field of View: 0.7853981852531433
210 |       Focal Point:
211 |         X: 3.684680461883545
212 |         Y: -1.1804018020629883
213 |         Z: -0.44005271792411804
214 |       Focal Shape Fixed Size: true
215 |       Focal Shape Size: 0.05000000074505806
216 |       Invert Z Axis: false
217 |       Name: Current View
218 |       Near Clip Distance: 0.009999999776482582
219 |       Pitch: 1.5697963237762451
220 |       Target Frame: <Fixed Frame>
221 |       Yaw: 3.1404037475585938
222 |     Saved: ~
223 | Window Geometry:
224 |   Displays:
225 |     collapsed: true
226 |   Height: 1016
227 |   Hide Left Dock: true
228 |   Hide Right Dock: true
229 |   QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005720000003efc0100000002fb0000000800540069006d0065010000000000000572000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005720000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
230 |   Selection:
231 |     collapsed: false
232 |   Time:
233 |     collapsed: false
234 |   Tool Properties:
235 |     collapsed: false
236 |   Views:
237 |     collapsed: true
238 |   Width: 1394
239 |   X: 526
240 |   Y: 27
241 | 


--------------------------------------------------------------------------------
/launch/my_laser_config.yaml:
--------------------------------------------------------------------------------
 1 | scan_filter_chain:
 2 | - name: range
 3 |   type: laser_filters/LaserScanRangeFilter
 4 |   params:
 5 |     use_message_range_limits: false
 6 |     lower_threshold: 0.04
 7 |     upper_threshold: 8.0
 8 |     lower_replacement_value: -.inf
 9 |     upper_replacement_value: .inf
10 | - name: shadows
11 |   type: laser_filters/ScanShadowsFilter
12 |   params:
13 |     min_angle: 10
14 |     max_angle: 170
15 |     neighbors: 20
16 |     window: 1
17 | #- name: speckle_filter
18 | #  type: laser_filters/LaserScanSpeckleFilter
19 | #  params:
20 | #    filter_type: 0
21 | #    max_range: 2.0
22 | #    max_range_difference: 0.1
23 | #    filter_window: 2
24 | - name: dark_shadows
25 |   type: laser_filters/LaserScanIntensityFilter
26 |   params:
27 |     lower_threshold: 100
28 |     upper_threshold: 10000
29 |     disp_histogram: 0
30 | 


--------------------------------------------------------------------------------
/msg/DynamicObstacle.msg:
--------------------------------------------------------------------------------
1 | string              name
2 | geometry_msgs/Point position
3 | geometry_msgs/Point velocity
4 | float64             reliability
5 | string[]            tagnames
6 | string[]            tags
7 | 


--------------------------------------------------------------------------------
/msg/DynamicObstacleStamped.msg:
--------------------------------------------------------------------------------
1 | std_msgs/Header header
2 | dynamic_obstacle_detector/DynamicObstacle obstacle
3 | 


--------------------------------------------------------------------------------
/msg/DynamicObstacles.msg:
--------------------------------------------------------------------------------
1 | std_msgs/Header header
2 | dynamic_obstacle_detector/DynamicObstacle[] obstacles
3 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>dynamic_obstacle_detector</name>
 4 |   <version>1.0.0</version>
 5 |   <description>The dynamic_obstacle_detector package</description>
 6 | 
 7 |   <maintainer email="noeperez@upo.es">Noé Pérez-Higueras</maintainer>
 8 | 
 9 |   <license>BSD</license>
10 | 
11 |   <url type="website">https://github.com/robotics-upo/dynamic_obstacle_detector</url>
12 | 
13 | 
14 |   <buildtool_depend>catkin</buildtool_depend>
15 |   <build_depend>geometry_msgs</build_depend>
16 |   <build_depend>laser_geometry</build_depend>
17 |   <build_depend>roscpp</build_depend>
18 |   <build_depend>sensor_msgs</build_depend>
19 |   <build_depend>tf2_ros</build_depend>
20 |   <build_depend>message_generation</build_depend>
21 |   <build_export_depend>geometry_msgs</build_export_depend>
22 |   <build_export_depend>laser_geometry</build_export_depend>
23 |   <build_export_depend>roscpp</build_export_depend>
24 |   <build_export_depend>sensor_msgs</build_export_depend>
25 |   <build_export_depend>tf2_ros</build_export_depend>
26 |   <exec_depend>geometry_msgs</exec_depend>
27 |   <exec_depend>laser_geometry</exec_depend>
28 |   <exec_depend>roscpp</exec_depend>
29 |   <exec_depend>sensor_msgs</exec_depend>
30 |   <exec_depend>tf2_ros</exec_depend>
31 |   <exec_depend>message_runtime</exec_depend>
32 | 
33 | 
34 |   <!-- The export tag contains other, unspecified, tags -->
35 |   <export>
36 |     <!-- Other tools can request additional information be placed here -->
37 | 
38 |   </export>
39 | </package>
40 | 


--------------------------------------------------------------------------------
/src/dynamic_obstacle_detector.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include <ros/ros.h>
  3 | #include <sensor_msgs/LaserScan.h>
  4 | #include <sensor_msgs/PointCloud2.h>
  5 | //#include <sensor_msgs/point_cloud2_iterator.h>
  6 | #include <geometry_msgs/PointStamped.h>
  7 | #include <laser_geometry/laser_geometry.h>
  8 | #include <sensor_msgs/point_cloud_conversion.h>
  9 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 10 | #include <tf2_ros/buffer.h>
 11 | #include <tf2_ros/transform_listener.h>
 12 | #include <visualization_msgs/MarkerArray.h>
 13 | 
 14 | #include <dynamic_obstacle_detector/DynamicObstacles.h>
 15 | 
 16 | #include "obstacle_kf.hpp"
 17 | 
 18 | #include <math.h> /* isinf, sqrt */
 19 | #include <string>
 20 | #include <vector>
 21 | 
 22 | class DynamicObstacleDetector {
 23 | 
 24 | public:
 25 |   ros::NodeHandle nh_;
 26 |   ros::NodeHandle n_;
 27 |   sensor_msgs::PointCloud pre_cloud_;
 28 |   ros::Subscriber scan_sub_;
 29 |   ros::Publisher obs_pub_, dyn_obs_pub_, points_pub_, obstacles_pub_;
 30 |   std::string input_scan_topic_;
 31 |   std::string odom_frame_;
 32 |   tf2_ros::Buffer tf_buffer_;
 33 |   tf2_ros::TransformListener tf_listener_;
 34 |   laser_geometry::LaserProjection projector_;
 35 | 
 36 |   std::vector<ObstacleKF> tracked_obstacles_;
 37 | 
 38 |   int scan_buffer_size_;
 39 |   float thres_point_dist_;
 40 |   int thresh_min_points_;
 41 |   int thresh_max_points_;
 42 |   float min_vel_tracked_;
 43 |   float max_vel_tracked_;
 44 | 
 45 |   // for the KF
 46 |   float track_distance_;
 47 |   float track_timeout_;
 48 |   int obstacle_count_;
 49 | 
 50 |   // TODO: publish trajectory of trackedObs
 51 | 
 52 |   struct Point {
 53 |     int id;
 54 |     float x;
 55 |     float y;
 56 |     Point() {
 57 |       id = -1;
 58 |       x = 0.0;
 59 |       y = 0.0;
 60 |     }
 61 |   };
 62 | 
 63 |   struct Obstacle {
 64 |     int id;
 65 |     ros::Time t;
 66 |     Point center;
 67 |     float width;
 68 |     int seen;
 69 |     bool matched;
 70 |     std::vector<Point> points;
 71 |     Obstacle() {
 72 |       t = ros::Time(0);
 73 |       id = -1;
 74 |       width = 0.0;
 75 |       seen = 1;
 76 |       matched = false;
 77 |     }
 78 |   };
 79 | 
 80 |   struct TrackedObstacle {
 81 |     // Obstacle obstacle;
 82 |     int id;
 83 |     std::vector<Point> traj;
 84 |     std::vector<ros::Time> time;
 85 |   };
 86 | 
 87 |   // std::vector<Obstacle> prev_obs_;
 88 | 
 89 |   std::vector<std::vector<Point>> points_;
 90 |   std::vector<std::vector<Obstacle>> obstacles_;
 91 | 
 92 |   DynamicObstacleDetector() : nh_("~"), n_(), tf_listener_(tf_buffer_) {
 93 | 
 94 |     nh_.param("input_scan_topic", input_scan_topic_, std::string("scan"));
 95 |     nh_.param("odom_frame", odom_frame_, std::string("odom"));
 96 |     nh_.param("scan_buffer_size", scan_buffer_size_, 3);
 97 |     nh_.param("cluster_max_distance_points", thres_point_dist_, float(0.6));
 98 |     nh_.param("cluster_min_points", thresh_min_points_, 3);
 99 |     nh_.param("cluster_max_points", thresh_max_points_, 35);
100 |     nh_.param("min_vel_tracked", min_vel_tracked_, float(0.35));
101 |     nh_.param("max_vel_tracked", max_vel_tracked_, float(2.0));
102 | 
103 |     nh_.param("max_tracked_distance", track_distance_, float(0.55));
104 |     nh_.param("max_tracked_sec", track_timeout_, float(1.0));
105 | 
106 |     ROS_INFO_STREAM(
107 |         "DYNAMIC OBSTACLE DETECTOR:"
108 |         << std::endl
109 |         << "- input scan topic: " << input_scan_topic_ << std::endl
110 |         << "- odom_frame: " << odom_frame_ << std::endl
111 |         << "- cluster_max_distance_points: " << thres_point_dist_ << std::endl
112 |         << "- cluster_min_points: " << thresh_min_points_ << std::endl
113 |         << "- cluster_max_points: " << thresh_max_points_ << std::endl
114 |         << "- min_vel_tracked: " << min_vel_tracked_ << std::endl
115 |         << "- max_vel_tracked: " << max_vel_tracked_ << std::endl
116 |         << "- max_tracked_distance: " << track_distance_ << std::endl
117 |         << "- max_tracked_sec: " << track_timeout_ << std::endl
118 |         << std::endl);
119 | 
120 |     obstacle_count_ = 0;
121 | 
122 |     scan_sub_ = n_.subscribe<sensor_msgs::LaserScan>(
123 |         input_scan_topic_.c_str(), 1, &DynamicObstacleDetector::scan_cb, this);
124 | 
125 |     obs_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(
126 |         "/dynamic_obstacles/static_markers", 0);
127 |     dyn_obs_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(
128 |         "/dynamic_obstacles/dynamic_markers", 1);
129 |     points_pub_ = nh_.advertise<visualization_msgs::Marker>(
130 |         "/dynamic_obstacles/points", 0);
131 | 
132 |     obstacles_pub_ = nh_.advertise<dynamic_obstacle_detector::DynamicObstacles>(
133 |         "/dynamic_obstacles", 0);
134 |   }
135 | 
136 |   ~DynamicObstacleDetector() {}
137 | 
138 |   void scan_cb(const sensor_msgs::LaserScan::ConstPtr &msg) {
139 |     std::vector<Point> points;
140 |     float angle_min = msg->angle_min;
141 |     float angle = angle_min + (msg->angle_increment / 2.0);
142 | 
143 |     // Transform scan to a set of points in odom_frame_
144 |     geometry_msgs::PointStamped ps;
145 |     ps.header.frame_id = msg->header.frame_id;
146 |     ps.header.stamp = ros::Time(0);
147 |     for (unsigned int i = 0; i < msg->ranges.size(); i++) {
148 |       if (!isinf(msg->ranges[i]) && !isnan(msg->ranges[i])) {
149 |         Point p;
150 |         p.id = i + 1;
151 |         ps.point.x = msg->ranges[i] * cos(angle);
152 |         ps.point.y = msg->ranges[i] * sin(angle);
153 |         ps.point.z = 0.0;
154 |         geometry_msgs::PointStamped psn;
155 |         try {
156 |           psn = tf_buffer_.transform(ps, odom_frame_);
157 |         } catch (tf2::TransformException &ex) {
158 |           ROS_WARN("Could NOT transform point to %s: %s", odom_frame_.c_str(),
159 |                    ex.what());
160 |           return;
161 |         }
162 |         p.x = psn.point.x;
163 |         p.y = psn.point.y;
164 |         points.push_back(p);
165 |       }
166 |       angle = angle + msg->angle_increment;
167 |     }
168 | 
169 |     if (!points.empty()) {
170 |       // Publish the points in RViz
171 |       publish_points(points);
172 |       // Find obstacles candidates in the point set
173 |       std::vector<Obstacle> obs = findObs(points, msg->header.stamp);
174 |       // printf("found %i obstacles!\n", (int)obs.size());
175 |       publish_obs(obs, std::string("obstacles"), 2, obs_pub_, 0.1);
176 |       // Track with the KFs
177 |       trackMovingObstaclesKF(obs);
178 |     }
179 |   }
180 | 
181 |   void trackMovingObstaclesKF(std::vector<Obstacle> &obs) {
182 |     // printf("\nNew obstacles size: %i\n\n", (int)obs.size());
183 |     // Vector with flags to consider when a detection have been used
184 |     std::vector<bool> used(obs.size(), false);
185 | 
186 |     double posDev = 0.2; // 0.1; //laser
187 | 
188 |     // Update tracked obstacles with the detections according to min distance
189 |     int k;
190 |     double x1, y1, x2, y2, dist, minDist;
191 |     for (int i = 0; i < (int)tracked_obstacles_.size(); i++) {
192 |       // if(tracked_obstacles_[i].updated)
193 |       //	continue;
194 | 
195 |       // Search the closest detection to every person tracked
196 |       k = 0;
197 |       x1 = tracked_obstacles_[i].x(0, 0);
198 |       y1 = tracked_obstacles_[i].x(1, 0);
199 |       minDist = 1000000.0;
200 |       for (int j = 0; j < (int)obs.size(); j++) {
201 |         if (!used[j]) {
202 |           x2 = obs[j].center.x;
203 |           y2 = obs[j].center.y;
204 |           dist = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
205 |           if (dist < minDist) {
206 |             k = j;
207 |             minDist = dist;
208 |           }
209 |         }
210 |       }
211 | 
212 |       // Check if the distant is too high
213 |       if (minDist < track_distance_) {
214 |         // Predict the position of the obstacle to this instant
215 |         tracked_obstacles_[i].predict(
216 |             (ros::Time::now() - tracked_obstacles_[i].tStamp).toSec());
217 | 
218 |         // Update the filter
219 |         tracked_obstacles_[i].update(obs[k].center.x, obs[k].center.y, posDev,
220 |                                      ros::Time::now());
221 | 
222 |         // Mark the detection as used
223 |         used[k] = true;
224 |         // std::cout << "Done" << std::endl;
225 |         // std::cout << "Updating obstacle " << i << " with detection " << k <<
226 |         // std::endl;
227 |       }
228 |     }
229 | 
230 |     // Add new persons to the list
231 |     for (int i = 0; i < (int)obs.size(); i++) {
232 |       if (!used[i]) {
233 |         // std::cout << "New obstacle added to the list: " <<
234 |         // tracked_obstacles_.size() << std::endl;
235 |         ObstacleKF obstacle;
236 |         obstacle.init(obstacle_count_++, obs[i].center.x, obs[i].center.y, 0,
237 |                       0);
238 |         tracked_obstacles_.push_back(obstacle);
239 |         // std::cout << "New obstacle added! size: " <<
240 |         // tracked_obstacles_.size() << std::endl;
241 | 
242 |         used[i] = true;
243 |       }
244 |     }
245 | 
246 |     // Remove too old estimations without update and static obstacles
247 |     std::vector<ObstacleKF> temp;
248 |     for (int i = 0; i < (int)tracked_obstacles_.size(); i++) {
249 | 
250 |       // printf("obstacle %i, linvel: %.3f\n", i, linvel);
251 |       if ((ros::Time::now() - tracked_obstacles_[i].tStamp).toSec() <
252 |           track_timeout_) //&& linvel >= min_vel_tracked_
253 |         temp.push_back(tracked_obstacles_[i]);
254 |     }
255 | 
256 |     tracked_obstacles_.clear();
257 |     tracked_obstacles_ = temp;
258 | 
259 |     // Publish dynamic obstacles
260 |     // publishDynObsMarker();
261 | 
262 |     // Publish DynamicObstacles message
263 |     publishDynamicObstacles();
264 |   }
265 | 
266 |   void publishDynamicObstacles() {
267 | 
268 |     dynamic_obstacle_detector::DynamicObstacles dyn_obs;
269 |     dynamic_obstacle_detector::DynamicObstacle ob;
270 | 
271 |     visualization_msgs::MarkerArray obsMarkers;
272 |     visualization_msgs::Marker marker;
273 | 
274 |     // Get marker color
275 |     float r, g, b;
276 |     r = 0.0;
277 |     g = 1.0;
278 |     b = 0.0;
279 | 
280 |     dyn_obs.header.frame_id = odom_frame_;
281 |     dyn_obs.header.stamp = ros::Time::now();
282 |     marker.header.frame_id = dyn_obs.header.frame_id;
283 |     marker.header.stamp = dyn_obs.header.stamp;
284 |     marker.ns = ros::this_node::getName();
285 |     for (int i = 0; i < (int)tracked_obstacles_.size(); i++) {
286 |       if (tracked_obstacles_[i].updated) {
287 |         double time = (ros::Time::now() - tracked_obstacles_[i].tStamp).toSec();
288 |         double vx = tracked_obstacles_[i].x(2, 0);
289 |         double vy = tracked_obstacles_[i].x(3, 0);
290 |         double linvel = sqrt((vx * vx) + (vy * vy));
291 |         double x_arrow = vx * time;
292 |         double y_arrow = vy * time;
293 |         double yaw = atan2(vy, vx);
294 |         // printf("Tracked obs %i, linvel: %.3f\n", tracked_obstacles_[i].id,
295 |         // linvel); printf("tracked ob %i linvel: %.3f, min_vel_tracked:
296 |         // %.3f\n", tracked_obstacles_[i].id, linvel, min_vel_tracked_);
297 | 
298 |         if (linvel >= min_vel_tracked_) {
299 | 
300 |           // Dynamic obstacle
301 |           ob.name = std::to_string(tracked_obstacles_[i].id);
302 |           ob.position.x = tracked_obstacles_[i].x(0, 0);
303 |           ob.position.y = tracked_obstacles_[i].x(1, 0);
304 |           ob.position.z = 0.0;
305 |           ob.velocity.z = 0.0;
306 |           ob.velocity.x = vx;
307 |           ob.velocity.y = vy;
308 |           ob.reliability = 0.8;
309 |           dyn_obs.obstacles.push_back(ob);
310 | 
311 |           // Cylinder
312 |           marker.id = 20 * tracked_obstacles_[i].id;
313 |           marker.type = visualization_msgs::Marker::CYLINDER;
314 |           marker.action = visualization_msgs::Marker::ADD;
315 |           marker.pose.position = ob.position;
316 |           marker.pose.position.z = 0.5;
317 |           marker.pose.orientation.x = 0.0;
318 |           marker.pose.orientation.y = 0.0;
319 |           marker.pose.orientation.z = 0.0;
320 |           marker.pose.orientation.w = 1.0;
321 |           marker.scale.x = 0.25;
322 |           marker.scale.y = 0.25;
323 |           marker.scale.z = 1.0;
324 |           marker.color.a = 1.0;
325 |           marker.color.r = r;
326 |           marker.color.g = g;
327 |           marker.color.b = b;
328 |           marker.lifetime = ros::Duration(0.1);
329 |           obsMarkers.markers.push_back(marker);
330 | 
331 |           // Arrow
332 |           marker.id = 20 * tracked_obstacles_[i].id + 1;
333 |           marker.type = visualization_msgs::Marker::ARROW;
334 |           marker.action = visualization_msgs::Marker::ADD;
335 |           marker.pose.position = marker.pose.position;
336 | 
337 |           // marker.points.push_back(marker.pose.position);
338 |           // geometry_msgs::Point end;
339 |           // end.x = x_arrow;
340 |           // end.y = y_arrow;
341 |           // end.z = marker.pose.position.z;
342 |           // marker.points.push_back(end);
343 |           marker.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
344 |           // marker.pose.orientation.x = 0.0;
345 |           // marker.pose.orientation.y = 0.0;
346 |           // marker.pose.orientation.z = 0.0;
347 |           // marker.pose.orientation.w = 1.0;
348 |           marker.scale.x = 1.2 * (linvel / max_vel_tracked_);
349 |           marker.scale.y = 0.15;
350 |           marker.scale.z = 0.15;
351 |           obsMarkers.markers.push_back(marker);
352 |         }
353 |       }
354 |       // else
355 |       // {
356 |       // 	// Delete body marker
357 |       // 	marker.id = tracked_obstacles_[i].id;
358 |       // 	marker.type = visualization_msgs::Marker::CYLINDER;
359 |       // 	marker.action = visualization_msgs::Marker::DELETE;
360 |       // 	obsMarkers.markers.push_back(marker);
361 |       // }
362 |     }
363 | 
364 |     // publish obstacle
365 |     obstacles_pub_.publish(dyn_obs);
366 |     // publish marker:
367 |     dyn_obs_pub_.publish(obsMarkers);
368 |   }
369 | 
370 |   std::vector<Obstacle> findMovingObs3() {
371 |     // use the obs of the first scan as an initial set of tracked obs
372 |     std::vector<TrackedObstacle> tobs;
373 |     int id = 1;
374 |     for (unsigned int i = 0; i < obstacles_[0].size(); i++) {
375 |       TrackedObstacle to;
376 |       to.id = id;
377 |       to.traj.push_back(obstacles_[0][i].center);
378 |       to.time.push_back(obstacles_[0][i].t);
379 |       tobs.push_back(to);
380 |       id++;
381 |     }
382 |     // printf("initial set of obstacles: %i\n", (int)tobs.size());
383 | 
384 |     // Look for the obstacles in the next scan and update the trajectory
385 |     float min_total_dist = 0.0;
386 |     float max_total_dist = 0.0;
387 |     for (unsigned int c = 1; c < scan_buffer_size_; c++) {
388 |       float dt = (obstacles_[c][0].t - obstacles_[c - 1][0].t).toSec();
389 |       float max_dist_step = max_vel_tracked_ * dt;
390 |       float min_dist_step = min_vel_tracked_ * dt;
391 |       min_total_dist += min_dist_step;
392 |       max_total_dist += max_dist_step;
393 |       for (unsigned int i = 0; i < tobs.size(); i++) {
394 |         TrackedObstacle *candidate = &tobs[i];
395 |         Obstacle min;
396 |         float min_dist = 1000;
397 |         for (unsigned int j = 0; j < obstacles_[c].size(); j++) {
398 |           float d = dist(candidate->traj[c - 1], obstacles_[c][j].center);
399 |           if (d < min_dist) {
400 |             min_dist = d;
401 |             min = obstacles_[c][j];
402 |           }
403 |         }
404 |         // printf("TrackedOb %i with ob %i. min_dist: %.2f\n", candidate->id,
405 |         // min.id, min_dist);
406 |         if (min_dist > min_dist_step && min_dist < max_dist_step) {
407 |           candidate->traj.push_back(min.center);
408 |           candidate->time.push_back(obstacles_[c][0].t);
409 |         }
410 |       }
411 |     }
412 |     std::vector<Obstacle> movingobs;
413 |     for (TrackedObstacle tracked : tobs) {
414 | 
415 |       // if not tracked in all (or nearly all) the scans, discard it
416 |       if (tracked.traj.size() < scan_buffer_size_ - 1) {
417 |         continue;
418 |       }
419 |       // printf("\nTracked ob: %i. traj size: %i\n", tracked.id,
420 |       // (int)tracked.traj.size()); printf("\tdist: %.3f", dist(tracked.traj[0]
421 |       // , tracked.traj[tracked.traj.size()-1]));
422 | 
423 |       // check the trajectory is not noise
424 |       if (dist(tracked.traj[0], tracked.traj[tracked.traj.size() - 1]) >=
425 |           min_total_dist) {
426 |         Obstacle o;
427 |         o.id = tracked.id;
428 |         o.center = tracked.traj[tracked.traj.size() - 1];
429 |         movingobs.push_back(o);
430 |       }
431 |     }
432 |     return movingobs;
433 |   }
434 | 
435 |   std::vector<Obstacle> findObs(const std::vector<Point> &points, ros::Time t) {
436 |     std::vector<Obstacle> obs;
437 |     unsigned int id = 1;
438 |     unsigned int i = 0;
439 |     while (i < points.size() - 1) {
440 | 
441 |       Obstacle o;
442 | 
443 |       // Joint the points
444 |       o.points.push_back(points[i]);
445 |       while ((i + 1) < points.size() &&
446 |              dist(points[i], points[i + 1]) <= thres_point_dist_) {
447 |         o.points.push_back(points[i + 1]);
448 |         i++;
449 |       }
450 |       // printf("Found a candidate group of %i points.", (int)o.points.size());
451 | 
452 |       // Create the obstacle if the conditions are fulfilled
453 |       if ((int)o.points.size() > thresh_min_points_ &&
454 |           (int)o.points.size() < thresh_max_points_) {
455 |         // printf("Found a candidate group of %i points.",
456 |         // (int)o.points.size()); printf(" --> ACCEPTED!\n"); Create obstacle
457 |         o.id = id;
458 |         o.t = t;
459 |         o.width = dist(o.points[0], o.points[o.points.size() - 1]);
460 |         o.center.id = id;
461 |         id++;
462 |         o.seen = 1;
463 |         for (Point p : o.points) {
464 |           o.center.x += p.x;
465 |           o.center.y += p.y;
466 |         }
467 |         o.center.x /= (float)o.points.size();
468 |         o.center.y /= (float)o.points.size();
469 |         obs.push_back(o);
470 | 
471 |       } else {
472 |         // printf(" --> DISCARDED!\n");
473 |       }
474 |       i++;
475 |       // remove the point candidates
476 |       o.points.clear();
477 |     }
478 |     return obs;
479 |   }
480 | 
481 |   float dist(const Point &p1, const Point &p2) {
482 |     // printf("p1x:%.2f, p2x:%.2f, p1y:%.2f, p2y:%.2f, -Dist: %.3f\n",p1.x,
483 |     // p2.x, p1.y, p2.y, std::hypotf((p1.x - p2.x), (p1.y - p2.y)));
484 |     return std::hypotf((p1.x - p2.x), (p1.y - p2.y));
485 |   }
486 | 
487 |   void publish_obs(const std::vector<Obstacle> &obs, std::string namespc,
488 |                    int color, ros::Publisher &pub, double time) {
489 |     visualization_msgs::MarkerArray ma;
490 |     visualization_msgs::Marker m;
491 |     m.header.frame_id = odom_frame_;
492 |     m.header.stamp = ros::Time();
493 |     m.ns = namespc;
494 |     m.type = visualization_msgs::Marker::CYLINDER;
495 |     m.action = visualization_msgs::Marker::ADD;
496 |     m.pose.orientation.x = 0.0;
497 |     m.pose.orientation.y = 0.0;
498 |     m.pose.orientation.z = 0.0;
499 |     m.pose.orientation.w = 1.0;
500 |     m.scale.x = 0.2;
501 |     m.scale.y = 0.2;
502 |     m.scale.z = 0.4;
503 |     switch (color) {
504 |     case 0:
505 |       m.color.r = 1.0;
506 |       m.color.g = 0.0;
507 |       m.color.b = 0.0;
508 |       break;
509 |     case 1:
510 |       m.color.r = 0.0;
511 |       m.color.g = 1.0;
512 |       m.color.b = 0.0;
513 |       break;
514 |     case 2:
515 |       m.color.r = 0.0;
516 |       m.color.g = 0.0;
517 |       m.color.b = 1.0;
518 |       break;
519 |     default:
520 |       m.color.r = 1.0;
521 |       m.color.g = 0.0;
522 |       m.color.b = 0.0;
523 |       break;
524 |     }
525 |     m.color.a = 1.0;
526 |     m.lifetime = ros::Duration(time);
527 |     for (Obstacle o : obs) {
528 |       m.id = o.id;
529 |       m.pose.position.x = o.center.x;
530 |       m.pose.position.y = o.center.y;
531 |       m.pose.position.z = 0.3;
532 |       ma.markers.push_back(m);
533 |     }
534 |     pub.publish(ma);
535 |   }
536 | 
537 |   void publish_points(const std::vector<Point> &points) {
538 |     visualization_msgs::Marker m;
539 |     m.header.frame_id = odom_frame_;
540 |     m.header.stamp = ros::Time::now();
541 |     m.ns = "dyn_points";
542 |     m.type = visualization_msgs::Marker::POINTS;
543 |     m.action = visualization_msgs::Marker::ADD;
544 |     m.pose.orientation.x = 0.0;
545 |     m.pose.orientation.y = 0.0;
546 |     m.pose.orientation.z = 0.0;
547 |     m.pose.orientation.w = 1.0;
548 |     m.scale.x = 0.03;
549 |     m.scale.y = 0.03;
550 |     m.scale.z = 0.03;
551 |     m.color.r = 1.0;
552 |     m.color.g = 0.0;
553 |     m.color.b = 0.0;
554 |     m.color.a = 1.0;
555 |     m.id = points[0].id;
556 |     m.lifetime = ros::Duration(0.1);
557 |     for (Point p : points) {
558 |       // if(p.id == -1)
559 |       //  continue;
560 |       geometry_msgs::Point pt;
561 |       pt.x = p.x;
562 |       pt.y = p.y;
563 |       pt.z = 0.03;
564 |       m.points.push_back(pt);
565 |     }
566 |     points_pub_.publish(m);
567 |   }
568 | 
569 |   // void scan_cb(const sensor_msgs::LaserScan::ConstPtr &msg) {
570 |   //   // sensor_msgs::LaserScan new_scan;
571 |   //   sensor_msgs::PointCloud new_cloud;
572 | 
573 |   //   try {
574 |   //     geometry_msgs::TransformStamped transformStamped =
575 |   //         tf_buffer_.lookupTransform(
576 |   //             msg->header.frame_id, "odom",
577 |   //             msg->header.stamp + ros::Duration().fromSec(msg->ranges.size()
578 |   //             *
579 |   //                                                         msg->time_increment),
580 |   //             ros::Duration(1.0));
581 |   //   } catch (tf2::TransformException &ex) {
582 |   //     ROS_WARN("Could NOT transform between %s and odom: %s",
583 |   //              msg->header.frame_id.c_str(), ex.what());
584 |   //     return;
585 |   //   }
586 | 
587 |   //   sensor_msgs::PointCloud2 cloud;
588 |   //   projector_.transformLaserScanToPointCloud("odom", *msg, cloud,
589 |   //   tf_buffer_);
590 | 
591 |   //   sensor_msgs::convertPointCloud2ToPointCloud(cloud, new_cloud);
592 | 
593 |   //   if (initiated_) {
594 |   //     // Compare pre_cloud_ and new_cloud in odom frame.
595 |   //     printf("eoeoeooeheheheheh");
596 |   //     new_cloud = compareCloud(pre_cloud_, new_cloud);
597 |   //     cloud_pub_.publish(new_cloud);
598 |   //   }
599 |   //   // pre_scan_ = new_scan;
600 |   //   pre_cloud_ = new_cloud;
601 |   //   initiated_ = true;
602 |   // }
603 | };
604 | 
605 | int main(int argc, char **argv) {
606 | 
607 |   ros::init(argc, argv, "dynamic_obstacle_detector");
608 |   DynamicObstacleDetector node;
609 |   ros::spin();
610 |   return 0;
611 | }
612 | 


--------------------------------------------------------------------------------
/src/laser_clustering.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/python3
  2 | # This Python file uses the following encoding: utf-8
  3 | 
  4 | import rospy
  5 | import sensor_msgs.point_cloud2 as pc2
  6 | from sensor_msgs.msg import PointCloud2, LaserScan, PointCloud
  7 | import laser_geometry.laser_geometry as lg
  8 | from visualization_msgs.msg import Marker, MarkerArray
  9 | from nav_msgs.srv import GetMap
 10 | from nav_msgs.msg import OccupancyGrid
 11 | from geometry_msgs.msg import PointStamped, Pose, Point
 12 | 
 13 | import math
 14 | 
 15 | import numpy as np
 16 | from sklearn.cluster import MeanShift, estimate_bandwidth
 17 | import matplotlib.pyplot as plt
 18 | #from sklearn.datasets import make_blobs
 19 | 
 20 | import roslib; roslib.load_manifest('laser_assembler')
 21 | from laser_assembler.srv import *
 22 | 
 23 | import tf2_ros
 24 | import tf2_py as tf2
 25 | import tf2_geometry_msgs
 26 | from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
 27 | 
 28 | import cv2 as cv
 29 | 
 30 | 
 31 | 
 32 | class laserClustering(object):
 33 | 
 34 |     def __init__(self):
 35 | 
 36 |         laser_topic = rospy.get_param('~input_scan_topic', 'scanfront')
 37 |         self.dist_from_obs = rospy.get_param('~dist_from_obs', 0.1)
 38 |         self.bandwidth = rospy.get_param('~cluster_bandwidth', 0.5)
 39 | 
 40 |         rospy.loginfo("---laser clustering initiated!---")
 41 |         rospy.loginfo(" - scan topic: %s", laser_topic)
 42 |         rospy.loginfo(" - dist from obstacles: %.2f m", self.dist_from_obs)
 43 |         rospy.loginfo(" - cluster bandwith: %.2f m\n", self.bandwidth)
 44 | 
 45 |         self.tf_buffer = tf2_ros.Buffer()
 46 |         self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
 47 | 
 48 |         self.lp = lg.LaserProjection()
 49 | 
 50 |         #self.point_lists = np.array()
 51 | 
 52 |         self.Freespace = 0
 53 |         self.Unknown = -1
 54 |         self.Occupied = 100
 55 | 
 56 |         self.bandwidth = 0.8
 57 |         self.ms = MeanShift(bandwidth=self.bandwidth, bin_seeding=True, min_bin_freq=3, cluster_all=False) #min_bin_freq=1
 58 | 
 59 |         self.use_static_map = False
 60 | 
 61 |         if self.use_static_map:
 62 |             self.static_map = OccupancyGrid
 63 |             rospy.loginfo("Waiting to receive the static map...")
 64 |             rospy.wait_for_service('static_map')
 65 |             try:
 66 |                 get_map = rospy.ServiceProxy('static_map', GetMap)
 67 |                 self.static_map = get_map().map
 68 |                 self.dist_trans = self.computeDistanceTransform()
 69 |                 rospy.loginfo("I got the static map")
 70 | 
 71 |             except rospy.ServiceException as e:
 72 |                 rospy.loginfo("Service call failed: %s", e)
 73 | 
 74 |         self.pc_pub = rospy.Publisher('my_pc2', PointCloud2, queue_size=1)
 75 |         self.marker_pub = rospy.Publisher('obstacles', MarkerArray, queue_size=1)
 76 |         self.marker2_pub = rospy.Publisher('points', MarkerArray, queue_size=1)
 77 |         rospy.Subscriber(laser_topic, LaserScan, self.laser_cb2, queue_size=1)
 78 | 
 79 | 
 80 |     def computeDistanceTransform(self):
 81 |         size = self.static_map.info.height, self.static_map.info.width, 1
 82 |         map_image = np.zeros(size, dtype=np.uint8)
 83 | 
 84 |         for i in range(len(self.static_map.data)-1):
 85 |             if(self.static_map.data[i]==self.Freespace):
 86 |                 cell = self.indexToCell(i)
 87 |                 map_image[cell[0], cell[1]] = 255
 88 |         try:
 89 |             dist_trans = cv.distanceTransform(map_image, cv.DIST_L1, 3)
 90 |         except:
 91 |             rospy.logerr("Error obtaining the distance transform!!!")
 92 |         return dist_trans
 93 | 
 94 | 
 95 | 
 96 |     def indexToCell(self, idx):
 97 |         index = idx
 98 |         if (index >= (self.static_map.info.width * self.static_map.info.height)):
 99 |             index = (self.static_map.info.width * self.static_map.info.height) - 1;
100 | 
101 |         x = index % self.static_map.info.width
102 |         y = index // self.static_map.info.width
103 |         cell = [x, y]
104 |         return cell
105 | 
106 |     def pointToCell(self, p):
107 |         # We account for no rotation of the map
108 |         x = math.floor((p.point.x - self.static_map.info.origin.position.x) / self.static_map.info.resolution);
109 |         y = math.floor((p.point.y - self.static_map.info.origin.position.y) / self.static_map.info.resolution);
110 |         return x, y
111 | 
112 |     def pointToIndex(self, p):
113 | 
114 |         # We account for no rotation of the map
115 |         #x = math.floor((p.point.x - self.static_map.info.origin.position.x) / self.static_map.info.resolution);
116 |         #y = math.floor((p.point.y - self.static_map.info.origin.position.y) / self.static_map.info.resolution);
117 |         x,y = self.pointToCell(p)
118 | 
119 |         # Cell to index
120 |         if (x >= self.static_map.info.width):
121 |             x = (self.static_map.info.width - 1)
122 | 
123 |         if (y >= self.static_map.info.height):
124 |             y = (self.static_map.info.height - 1)
125 |         # Get the index in the array
126 |         index = x + y * self.static_map.info.width
127 |         return index
128 | 
129 | 
130 | 
131 |     def valid_nhood4(self, idx):
132 |         # get 4-connected neighbourhood indexes, check for edge of map
133 |         if (idx > self.static_map.info.width * self.static_map.info.height - 1):
134 |             return False
135 | 
136 |         if self.static_map.data[idx] == self.Occupied or self.static_map.data[idx] == self.Unknown:
137 |             return False
138 | 
139 |         if (idx % self.static_map.info.width > 0):
140 |             if self.static_map.data[idx-1] == self.Occupied or self.static_map.data[idx-1] == self.Unknown:
141 |                 return False
142 |         if ((idx % self.static_map.info.width) < self.static_map.info.width - 1):
143 |             if self.static_map.data[idx+1] == self.Occupied or self.static_map.data[idx+1] == self.Unknown:
144 |                 return False
145 |         if (idx >= self.static_map.info.width):
146 |             if self.static_map.data[idx - self.static_map.info.width] == self.Occupied or self.static_map.data[idx - self.static_map.info.width] == self.Unknown:
147 |                 return False
148 |         if (idx < self.static_map.info.width * (self.static_map.info.height - 1)):
149 |             if self.static_map.data[idx + self.static_map.info.width] == self.Occupied or self.static_map.data[idx + self.static_map.info.width] == self.Unknown:
150 |                 return False
151 | 
152 |         return True
153 | 
154 | 
155 | 
156 |     def valid_nhood8(self, idx):
157 |         # get 8-connected neighbourhood indexes, check for edge of map
158 |         if self.valid_nhood4(idx) == False:
159 |             return False
160 | 
161 |         if (idx % self.static_map.info.width > 0 and idx >= self.static_map.info.width):
162 |             if self.static_map.data[idx - 1 - self.static_map.info.width] == self.Occupied or self.static_map.data[idx - 1 - self.static_map.info.width] == self.Unknown:
163 |                 return False
164 |         if (idx % self.static_map.info.width > 0 and idx < self.static_map.info.width * (self.static_map.info.height - 1)):
165 |             if self.static_map.data[idx - 1 + self.static_map.info.width] == self.Occupied or self.static_map.data[idx - 1 + self.static_map.info.width] == self.Unknown:
166 |                 return False
167 |         if (idx % self.static_map.info.width < self.static_map.info.width - 1 and idx >= self.static_map.info.width):
168 |             if self.static_map.data[idx + 1 - self.static_map.info.width] == self.Occupied or self.static_map.data[idx + 1 - self.static_map.info.width] == self.Unknown:
169 |                 return False
170 |         if (idx % self.static_map.info.width < self.static_map.info.width - 1 and idx < self.static_map.info.width * (self.static_map.info.height - 1)):
171 |             if self.static_map.data[idx + 1 + self.static_map.info.width] == self.Occupied or self.static_map.data[idx + 1 + self.static_map.info.width] == self.Unknown:
172 |                 return False
173 | 
174 |         return True
175 | 
176 | 
177 |     def publish_points(self, points):
178 |         ma = MarkerArray()
179 | #        m = Marker()
180 | #        m.header.stamp = rospy.Time()
181 | #        m.header.frame_id = points[0].header.frame_id
182 | #        m.type = 2 # 8-points, 7-sphere list
183 | #        m.action = 0 #0-add
184 | #        m.ns = 'points'
185 | #        m.scale.x = 0.1
186 | #        m.scale.y = 0.1
187 | #        m.scale.z = 0.1
188 | #        m.color.r = 0.0
189 | #        m.color.g = 0.0
190 | #        m.color.b = 1.0
191 | #        m.color.a = 1.0
192 | #        m.frame_locked = False
193 | #        #m.lifetime
194 | #        m.pose = Pose()
195 | #        m.pose.orientation.w = 1.0
196 |         #point = Point()
197 |         #point.z = 0.1
198 |         i = 1
199 |         #print("\npoints: %d" % len(points))
200 |         for p in points:
201 |             #point.x = p.point.x
202 |             #point.y = p.point.y
203 |             #m.points.append(point)
204 |             m = Marker()
205 |             m.header.stamp = rospy.Time()
206 |             m.header.frame_id = points[0].header.frame_id
207 |             m.type = 2 # 8-points, 7-sphere list
208 |             m.action = 0 #0-add
209 |             m.ns = 'points'
210 |             m.scale.x = 0.1
211 |             m.scale.y = 0.1
212 |             m.scale.z = 0.1
213 |             m.color.r = 0.0
214 |             m.color.g = 0.0
215 |             m.color.b = 1.0
216 |             m.color.a = 1.0
217 |             m.frame_locked = False
218 |             m.lifetime = rospy.Duration.from_sec(0.25)
219 |             m.pose = Pose()
220 |             m.pose.orientation.w = 1.0
221 |             m.pose.position.x = p.point.x
222 |             m.pose.position.y = p.point.y
223 |             m.id = i
224 |             #print(i)
225 |             i = i+1
226 |             ma.markers.append(m)
227 | 
228 |         #print("size of m.points: %d" % len(m.points))
229 |         self.marker2_pub.publish(ma)
230 | 
231 | 
232 |     def publish_centers(self, points, frame):
233 |         ma = MarkerArray()
234 |         i = 1
235 |         for p in points:
236 |             m = Marker()
237 |             m.header.stamp = rospy.Time()
238 |             m.header.frame_id = frame
239 |             m.type = 3 # 8-points, 7-sphere list
240 |             m.action = 0 #0-add
241 |             m.ns = 'centers'
242 |             m.scale.x = 0.35
243 |             m.scale.y = 0.35
244 |             m.scale.z = 1.5
245 |             m.color.r = 0.0
246 |             m.color.g = 1.0
247 |             m.color.b = 0.0
248 |             m.color.a = 1.0
249 |             m.frame_locked = False
250 |             m.lifetime = rospy.Duration.from_sec(0.25)
251 |             m.pose = Pose()
252 |             m.pose.orientation.w = 1.0
253 |             m.pose.position.x = p[0]
254 |             m.pose.position.y = p[1]
255 |             m.id = i
256 |             i = i+1
257 |             ma.markers.append(m)
258 |         self.marker_pub.publish(ma)
259 | 
260 | 
261 |     def laser_cb2(self, msg):
262 |         # convert the message of type LaserScan to a PointCloud2
263 |         pc2_msg = self.lp.projectLaser(msg)
264 | 
265 |         try:
266 |             trans = self.tf_buffer.lookup_transform('odom', msg.header.frame_id,
267 |                                                       msg.header.stamp,
268 |                                                       rospy.Duration(0.01))
269 |         except tf2.LookupException as ex:
270 |             rospy.logwarn(ex)
271 |             return
272 |         except tf2.ExtrapolationException as ex:
273 |             rospy.logwarn(ex)
274 |             return
275 |         try:
276 |             pc2_msg = do_transform_cloud(pc2_msg, trans)
277 |         except:
278 |             rospy.logerr("Could not transform point cloud")
279 | 
280 |         points = np.asarray(pc2.read_points_list(pc2_msg, field_names = ("x", "y"), skip_nans=False))
281 |         rospy.loginfo(" Range: %d", len(msg.ranges))
282 |         rospy.loginfo(" scanned points size: %d", len(points))
283 | 
284 | 
285 | 
286 | 
287 | 
288 | 
289 |     def laser_cb(self, msg):
290 | 
291 |         # convert the message of type LaserScan to a PointCloud2
292 |         pc2_msg = self.lp.projectLaser(msg)
293 | 
294 |         #self.pc_pub.publish(pc2_msg)
295 | 
296 |         # what is faster? -> 2!
297 |         # 1) list
298 |         #point_list = np.asarray(list(pc2.read_points(pc2_msg, field_names = ("x", "y"), skip_nans=True)))
299 |         # or 2) - a list of the individual points which is less efficient
300 |         #print(pc2.read_points_list(pc2_msg, field_names = ("x", "y"), skip_nans=True))
301 |         point_list = np.asarray(pc2.read_points_list(pc2_msg, field_names = ("x", "y"), skip_nans=True))
302 |         #print(point_list)
303 | 
304 |         p = PointStamped()
305 |         p.header.frame_id = pc2_msg.header.frame_id
306 |         p.header.stamp = rospy.Time()
307 |         filtered_pl = []
308 |         point_stamped = []
309 |         for i in point_list:
310 |             p.point.x = i[0]
311 |             p.point.y = i[1]
312 |             try:
313 |                 pt = self.tf_buffer.transform(p, 'map')
314 |             except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
315 |                 print("Could not transform point to map: %s"%e)
316 |                 return
317 |             x,y = self.pointToCell(pt)
318 |             distance = self.dist_trans[x, y] * self.static_map.info.resolution
319 |             if distance > self.dist_from_obs:
320 |                 filtered_pl.append(i)
321 |                 point_stamped.append(pt)
322 | 
323 | 
324 |         filtered_pl = np.asarray(filtered_pl)
325 | 
326 |         if len(point_stamped) > 0:
327 |             self.publish_points(point_stamped)
328 |         # The following bandwidth can be automatically detected using
329 |         #bandwidth = estimate_bandwidth(point_list, quantile=0.2, n_samples=500)
330 | 
331 |         #ms = MeanShift(bandwidth=bandwidth) #bin_seeding=True
332 |         if len(filtered_pl) > 0:
333 |             try:
334 |                 self.ms.fit(filtered_pl)
335 |                 labels = self.ms.labels_  #with the labels I know the points for each cluster
336 |                 cluster_centers = self.ms.cluster_centers_
337 |                 self.publish_centers(cluster_centers, pc2_msg.header.frame_id)
338 |             except:
339 |                 rospy.logwarn("No cluster obtained..")
340 | 
341 | 
342 |         #self.ms = MeanShift(bandwidth=self.bandwidth, min_bin_freq=2, seeds=cluster_centers) #bin_seeding is ignored if seeds are given
343 |         #print("mumber of cluster centers: %d" % len(cluster_centers))
344 |         #print("centers:")
345 |         #print(cluster_centers)
346 | 
347 |         #labels_unique = np.unique(labels)
348 |         #n_clusters_ = len(labels_unique)
349 |         #print("number of estimated clusters : %d" % n_clusters_)
350 | 
351 |         #P = ms.predict(point_list)
352 | 
353 |         #print("P:")
354 |         #print(P)
355 | 
356 | 
357 |         #plt.scatter(point_list[:,0], point_list[:,1], marker=".", picker=True) #c=colors
358 |         #plt.scatter(cluster_centers[:,0], cluster_centers[:,1], marker="o", c='red')
359 |         #plt.plot(point_list[:,0], point_list[:,1], marker=".")
360 |         #plt.plot(point_list, marker=".")
361 |         #plt.title(f'Estimated number of clusters = {n_clusters_}')
362 |         #plt.xlabel('X')
363 |         #plt.ylabel('Y')
364 |         #plt.show()
365 | 
366 | 
367 | 
368 | if __name__ == '__main__':
369 |     rospy.init_node('clusterLaser', anonymous=True)
370 |     lc = laserClustering()
371 |     rospy.spin()
372 | 


--------------------------------------------------------------------------------
/src/obstacle_kf.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef __OBSTACLE_KF_H__
  2 | #define __OBSTACLE_KF_H__
  3 | 
  4 | #include <eigen3/Eigen/Core>
  5 | #include <eigen3/Eigen/Dense>
  6 | #include <ros/ros.h>
  7 | //#include <Eigen/LU>
  8 | 
  9 | #define OBSTACLEKF_POS_VAR 0.1
 10 | #define OBSTACLEKF_VEL_NOISE_VAR 0.2  
 11 | 
 12 | struct ObstacleKF
 13 | {
 14 | 	// State vector: [x (m), y (m), vx (m/s), vy (m/s)]
 15 | 	Eigen::MatrixXd x;
 16 | 	Eigen::MatrixXd P;
 17 | 	
 18 | 	// Obstacle ID
 19 | 	int id;
 20 | 	std::string name;
 21 | 	
 22 | 	// Last time update
 23 | 	ros::Time tStamp;
 24 | 	
 25 | 	// Updated flag
 26 | 	bool updated;
 27 | 
 28 | 	// number of detections
 29 | 	//int seen;
 30 | 	
 31 | 	// Default constructor
 32 | 	ObstacleKF(void) : x(4,1), P(4,4) 
 33 | 	{
 34 | 		x.setZero(4, 1);
 35 | 		P.setIdentity(4, 4);
 36 | 		id = -1;
 37 | 		tStamp = ros::Time::now();
 38 | 		updated = false;
 39 | 		name = "";
 40 | 		//seen = 0;
 41 | 	}
 42 | 	
 43 | 	ObstacleKF(const ObstacleKF &data) : x(4,1), P(4,4)
 44 | 	{
 45 | 		x = data.x;
 46 | 		P = data.P;
 47 | 		id = data.id;
 48 | 		tStamp = data.tStamp;
 49 | 		updated = data.updated;
 50 | 		name = data.name;
 51 | 		//seen = 0;
 52 | 	}
 53 | 	
 54 | 	ObstacleKF &operator=(const ObstacleKF &data)
 55 | 	{
 56 | 		x = data.x;
 57 | 		P = data.P;
 58 | 		id = data.id;
 59 | 		tStamp = data.tStamp;
 60 | 		updated = data.updated;
 61 | 		name = data.name;
 62 | 		//seen = data.seen;
 63 | 		return *this;
 64 | 	}
 65 | 	
 66 | 	// Filter initialization, position in m and velocity on m/s 
 67 | 	void init(int _id, double _x, double _y, double _vx, double _vy, std::string _name = "")
 68 | 	{
 69 | 		// Get Obstacle ID
 70 | 		id = _id;
 71 | 
 72 | 		//increase seen times
 73 | 		//seen+=1;
 74 | 		//printf("Initializing obstacle %i, seen: %i\n", id, seen);
 75 | 		
 76 | 		// Setup state vector
 77 | 		x.setZero(4, 1);
 78 | 		x(0,0) = _x;
 79 | 		x(1,0) = _y;
 80 | 		x(2,0) = _vx;
 81 | 		x(3,0) = _vy;
 82 | 		
 83 | 		// Setup cov matrix
 84 | 		P.setIdentity(4, 4);
 85 | 		P(0,0) = OBSTACLEKF_POS_VAR;
 86 | 		P(1,1) = OBSTACLEKF_POS_VAR;
 87 | 		P(2,2) = 1.0*1.0;
 88 | 		P(3,3) = 1.0*1.0;
 89 | 		
 90 | 		// Update time stamp
 91 | 		tStamp = ros::Time::now();
 92 | 		updated = false;
 93 | 		name = _name;
 94 | 	}
 95 | 	
 96 | 	// State prediction, time in seconds 
 97 | 	void predict(double _dt)
 98 | 	{
 99 | 		// State vector prediction
100 | 		x(0,0) += x(2,0)*_dt;
101 | 		x(1,0) += x(3,0)*_dt;
102 | 		
103 | 		// Convariance matrix prediction
104 | 		Eigen::Matrix<double, 4, 4> F;
105 | 		F.setIdentity(4, 4);
106 | 		F(0,2) = _dt;
107 | 		F(1,3) = _dt;
108 | 		Eigen::Matrix<double, 4, 4> Q;
109 | 		Q.setZero(4, 4);
110 | 		Q(2,2) = OBSTACLEKF_VEL_NOISE_VAR*_dt*_dt;
111 | 		Q(3,3) = OBSTACLEKF_VEL_NOISE_VAR*_dt*_dt;
112 | 		P = F*P*F.transpose() + Q;
113 | 		
114 | 		updated = false;
115 | 	}
116 | 	
117 | 	// State update
118 | 	void update(double _x, double _y,  double _posVar = OBSTACLEKF_POS_VAR, ros::Time _t = ros::Time::now())
119 | 	{
120 | 		// Update time stamp
121 | 		tStamp = _t;
122 | 
123 | 		//increase seen times
124 | 		//seen+=1;
125 | 		//printf("Updating obstacle %i, seen: %i\n", id, seen);
126 | 		
127 | 		// Compute update jacobian
128 | 		Eigen::Matrix<double, 2, 4> H;
129 | 		H.setZero(2, 4);
130 | 		H(0,0) = 1.0;
131 | 		H(1,1) = 1.0;
132 | 		
133 | 		// Compute update noise matrix
134 | 		Eigen::Matrix<double, 2, 2> R;
135 | 		R.setZero(2, 2);
136 | 		R(0,0) = _posVar;
137 | 		R(1,1) = _posVar;
138 | 		
139 | 		// Calculate innovation matrix
140 | 		Eigen::Matrix<double, 2, 2> S;
141 | 		S = H*P*H.transpose() + R;
142 | 		
143 | 		// Calculate kalman gain
144 | 		Eigen::Matrix<double, 4, 2> K;
145 | 		K = P*H.transpose()*S.inverse();
146 | 		
147 | 		// Calculate innovation vector
148 | 		Eigen::Matrix<double, 2, 1> y;
149 | 		y(0,0) = _x - x(0,0);
150 | 		y(1,0) = _y - x(1,0);
151 | 		
152 | 		// Calculate new state vector
153 | 		x = x + K*y;
154 | 		
155 | 		// Calculate new cov matrix
156 | 		Eigen::Matrix<double, 4, 4> I;
157 | 		I.setIdentity(4, 4);
158 | 		P = (I - K*H)*P;
159 | 		
160 | 		updated = true;
161 | 	}
162 | 	
163 | 	// State update
164 | 	void update(double _x1, double _y1,  double _posVar1, double _x2, double _y2,  double _posVar2, ros::Time _t = ros::Time::now())
165 | 	{
166 | 		// Update time stamp
167 | 		tStamp = _t;
168 | 
169 | 		//seen+=1;
170 | 		
171 | 		// Compute update jacobian
172 | 		Eigen::Matrix<double, 4, 4> H;
173 | 		H.setZero(4, 4);
174 | 		H(0,0) = 1.0;
175 | 		H(1,1) = 1.0;
176 | 		H(2,0) = 1.0;
177 | 		H(3,1) = 1.0;
178 | 		
179 | 		// Compute update noise matrix
180 | 		Eigen::Matrix<double, 4, 4> R;
181 | 		R.setZero(4, 4);
182 | 		R(0,0) = _posVar1;
183 | 		R(1,1) = _posVar1;
184 | 		R(2,2) = _posVar2;
185 | 		R(3,3) = _posVar2;
186 | 		
187 | 		// Calculate innovation matrix
188 | 		Eigen::Matrix<double, 4, 4> S;
189 | 		S = H*P*H.transpose() + R;
190 | 		
191 | 		// Calculate kalman gain
192 | 		Eigen::Matrix<double, 4, 4> K;
193 | 		K = P*H.transpose()*S.inverse();
194 | 		
195 | 		// Calculate innovation vector
196 | 		Eigen::Matrix<double, 4, 1> y;
197 | 		y(0,0) = _x1 - x(0,0);
198 | 		y(1,0) = _y1 - x(1,0);
199 | 		y(2,0) = _x2 - x(0,0);
200 | 		y(3,0) = _y2 - x(1,0);
201 | 		
202 | 		// Calculate new state vector
203 | 		x = x + K*y;
204 | 		
205 | 		// Calculate new cov matrix
206 | 		Eigen::Matrix<double, 4, 4> I;
207 | 		I.setIdentity(4, 4);
208 | 		P = (I - K*H)*P;
209 | 		
210 | 		updated = true;
211 | 	}
212 | 	
213 | 	// State update
214 | 	void update(double _x1, double _y1,  double _posVar1, 
215 | 	            double _x2, double _y2,  double _posVar2, 
216 | 	            double _x3, double _y3,  double _posVar3, ros::Time _t = ros::Time::now())
217 | 	{
218 | 		// Update time stamp
219 | 		tStamp = _t;
220 | 
221 | 		//seen+=1;
222 | 		
223 | 		// Compute update jacobian
224 | 		Eigen::Matrix<double, 6, 4> H;
225 | 		H.setZero(6, 4);
226 | 		H(0,0) = 1.0;
227 | 		H(1,1) = 1.0;
228 | 		H(2,0) = 1.0;
229 | 		H(3,1) = 1.0;
230 | 		H(4,0) = 1.0;
231 | 		H(5,1) = 1.0;
232 | 		
233 | 		// Compute update noise matrix
234 | 		Eigen::Matrix<double, 6, 6> R;
235 | 		R.setZero(4, 4);
236 | 		R(0,0) = _posVar1;
237 | 		R(1,1) = _posVar1;
238 | 		R(2,2) = _posVar2;
239 | 		R(3,3) = _posVar2;
240 | 		R(4,4) = _posVar3;
241 | 		R(5,5) = _posVar3;
242 | 		
243 | 		// Calculate innovation matrix
244 | 		Eigen::Matrix<double, 6, 6> S;
245 | 		S = H*P*H.transpose() + R;
246 | 		
247 | 		// Calculate kalman gain
248 | 		Eigen::Matrix<double, 4, 6> K;
249 | 		K = P*H.transpose()*S.inverse();
250 | 		
251 | 		// Calculate innovation vector
252 | 		Eigen::Matrix<double, 6, 1> y;
253 | 		y(0,0) = _x1 - x(0,0);
254 | 		y(1,0) = _y1 - x(1,0);
255 | 		y(2,0) = _x2 - x(0,0);
256 | 		y(3,0) = _y2 - x(1,0);
257 | 		y(4,0) = _x3 - x(0,0);
258 | 		y(5,0) = _y3 - x(1,0);
259 | 		
260 | 		// Calculate new state vector
261 | 		x = x + K*y;
262 | 		
263 | 		// Calculate new cov matrix
264 | 		Eigen::Matrix<double, 4, 4> I;
265 | 		I.setIdentity(4, 4);
266 | 		P = (I - K*H)*P;
267 | 		
268 | 		updated = true;
269 | 	}
270 | };
271 | 
272 | #endif
273 | 


--------------------------------------------------------------------------------