├── .gitignore
├── CMakeLists.txt
├── README.md
├── launch
    └── edge_leg_detector.launch
├── package.xml
└── src
    └── edge_leg_detector
        └── detector.cpp


/.gitignore:
--------------------------------------------------------------------------------
 1 | # Compiled Object files
 2 | *.slo
 3 | *.lo
 4 | *.o
 5 | 
 6 | # Compiled Dynamic libraries
 7 | *.so
 8 | *.dylib
 9 | 
10 | # Compiled Static libraries
11 | *.lai
12 | *.la
13 | *.a
14 | 
15 | *~
16 | server.log
17 | *.user
18 | *.swp
19 | *.swo
20 | 
21 | *.pyc
22 | 
23 | # Folder
24 | .ropeproject/
25 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(edge_leg_detector)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 |   geometry_msgs
 6 |   roscpp
 7 |   rospy
 8 |   sensor_msgs
 9 | )
10 | 
11 | find_package(Boost REQUIRED COMPONENTS thread)
12 | 
13 | ###################################
14 | ## catkin specific configuration ##
15 | ###################################
16 |     
17 | catkin_package(
18 |   CATKIN_DEPENDS roscpp rospy sensor_msgs geometry_msgs
19 | )
20 | 
21 | ###########
22 | ## Build ##
23 | ###########
24 | 
25 | include_directories(
26 |   ${catkin_INCLUDE_DIRS}
27 |   ${Boost_INCLUDE_DIRS}
28 | )
29 | 
30 | add_executable(detector src/edge_leg_detector/detector.cpp)
31 | 
32 | target_link_libraries(detector
33 |   ${catkin_LIBRARIES}
34 |   ${Boost_LIBRARIES}
35 | )
36 | 
37 | #############
38 | ## Install ##
39 | #############
40 | 
41 | install(TARGETS detector 
42 |    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
43 |    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
44 |    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
45 |  )
46 | 
47 | install(DIRECTORY launch
48 |    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
49 |  )
50 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Leg Detector
 2 | This is a ROS package that implements a leg detector from a LaserScan message, and publish the position of humans in a PoseArray message.
 3 | 
 4 | This code is based on the work described in:
 5 | 
 6 | Bellotto, N.; Huosheng Hu, "Multisensor-Based Human Detection and Tracking for Mobile Service Robots," Systems, Man, and Cybernetics, Part B: Cybernetics, IEEE Transactions on , vol.39, no.1, pp.167,181, Feb. 2009
 7 | doi: 10.1109/TSMCB.2008.2004050
 8 | 
 9 | Run this package by typing 
10 | 
11 | ```
12 | roslaunch edge_leg_detector edge_leg_detector.launch laser_scan:=[LASER_SCAN_TOPIC]
13 | ```
14 | 
15 | Where ```[LASER_SCAN_TOPIC]``` is the name of the topic with the LaserScan sensor data. For example, if the name of the topic is ```/scan``` then, the leg detector can be initialized as: 
16 | 
17 | ```
18 | roslaunch edge_leg_detector edge_leg_detector.launch laser_scan:=/scan
19 | ```
20 | 


--------------------------------------------------------------------------------
/launch/edge_leg_detector.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <arg name="laser_scan" default="/scan" />
 3 |     <arg name="machine" default="localhost" />
 4 |     <arg name="user" default="" />
 5 | 
 6 |     <machine name="$(arg machine)" address="$(arg machine)" env-loader="$(optenv ROS_ENV_LOADER )" user="$(arg user)" default="true"/>
 7 |     
 8 |     <node pkg="edge_leg_detector" type="detector" name="edge_leg_detector" output="screen">
 9 |         <param name="laser_scan" value="$(arg laser_scan)" type="string"/>
10 |     </node>
11 | 
12 | </launch> 
13 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>edge_leg_detector</name>
 4 |   <version>0.0.1</version>
 5 |   <description>A leg detector that identifies three different leg patterns based on the paper of Nicholas Belloto</description>
 6 | 
 7 |   <maintainer email="marcobecerrap@gmail.com">Marco Antonio Becerra Pedraza</maintainer>
 8 |   <maintainer email="fxj345@cs.bham.ac.uk">Ferdian Jovan</maintainer>
 9 | 
10 |   <license>MIT</license>
11 | 
12 |   <buildtool_depend>catkin</buildtool_depend>
13 | 
14 |   <build_depend>boost</build_depend>
15 |   <build_depend>roscpp</build_depend>
16 |   <build_depend>rospy</build_depend>
17 |   <build_depend>sensor_msgs</build_depend>
18 |   <build_depend>geometry_msgs</build_depend>
19 | 
20 |   <run_depend>boost</run_depend>
21 |   <run_depend>roscpp</run_depend>
22 |   <run_depend>rospy</run_depend>
23 |   <run_depend>sensor_msgs</run_depend>
24 |   <run_depend>geometry_msgs</run_depend>
25 | 
26 |   <export>
27 |   </export>
28 | </package>
29 | 


--------------------------------------------------------------------------------
/src/edge_leg_detector/detector.cpp:
--------------------------------------------------------------------------------
  1 | // FILE: "Leg_detector_node" 
  2 | // AUTHOR: Marco Antonio Becerra Pedraza (http://www.marcobecerrap.com)
  3 | // MODIFIED BY: Ferdian Jovan (http://github.com/ferdianjovan)
  4 | // SUMMARY: This program receives the LaserScan msgs and executes a leg detector algorithm
  5 | // > to search for persons. At the end publishes a a vector with all the persons found 
  6 | // > and their position relative to the sensor.
  7 | //
  8 | // NOTES: This leg detector is based on the work described in:
  9 | // Bellotto, N. & Hu, H. 
 10 | // Multisensor-Based Human Detection and Tracking for Mobile Service Robots 
 11 | // IEEE Trans. on Systems, Man, and Cybernetics -- Part B, 2009, 39, 167-181
 12 | 
 13 | #include <ros/ros.h>
 14 | #include <sensor_msgs/LaserScan.h>
 15 | #include <geometry_msgs/PoseArray.h>
 16 | #include <iostream>
 17 | #include <cmath>
 18 | #include <vector>
 19 | #include <list>
 20 | #include <string>
 21 | #include <boost/thread/mutex.hpp>
 22 | 
 23 | #define PI 3.1416
 24 | #define FILTER_SIZE 2
 25 | #define FLANK_THRESHOLD 0.1
 26 | 
 27 | #define FLANK_U 1
 28 | #define FLANK_D -1
 29 | 
 30 | //Antropometric parameters
 31 | #define ANTRO_a0 0.1 //|
 32 | #define ANTRO_a1 0.2 //|-> Leg width (min-max)
 33 | #define ANTRO_b0 0   //  |
 34 | #define ANTRO_b1 0.4 //  |-> Free space between two legs (min-max)
 35 | #define ANTRO_c0 0.1 //    |
 36 | #define ANTRO_c1 0.4 //    |-> Two legs together width (min-max)
 37 | 
 38 | // Pattern Type
 39 | #define TYPE_LA 1 // Legs separated
 40 | #define TYPE_FS 2 // Legs dephased
 41 | #define TYPE_SL 3 // Legs together
 42 | 
 43 | 
 44 | using namespace std;
 45 | 
 46 | bool sensor_on   = false;
 47 | 
 48 | int g_counter = 0;
 49 | 
 50 | vector < double > rec_x;
 51 | vector < double > rec_y;
 52 | string sensor_frame_id;
 53 | sensor_msgs::LaserScan SensorMsg;
 54 | boost::mutex mutex;
 55 | 
 56 | void LaserFilter_Mean( vector <double> *vector_r, unsigned size );
 57 | void LaserCallback (const sensor_msgs::LaserScan::ConstPtr& msg);
 58 | void FindPattern( string str, string pattern, list <int> *element_found );
 59 | void ValidatePattern( list <int> *Pattern_list, int TYPE,  vector <int> flank_id0,  vector <int> flank_id1, vector <double> laser_x, vector <double> laser_y);
 60 | double Dist2D( double x0, double y0, double x1, double y1 );
 61 | void HumanPose( vector <double> *r_x, vector <double> *r_y, list <int> Pattern_list, int TYPE,  vector <int> flank_id0,  vector <int> flank_id1, vector <double> laser_x, vector <double> laser_y );
 62 | 
 63 | // added by Ferdian Jovan
 64 | // function to restrict a possibility of persons standing next to each other
 65 | void ValidateDistance();
 66 | 
 67 | 
 68 | int main(int argc, char **argv){
 69 | 
 70 |   ros::init(argc, argv, "edge_leg_detector");
 71 |   ros::NodeHandle n;
 72 |   ros::Publisher  node_pub = n.advertise <geometry_msgs::PoseArray>("edge_leg_detector", 2); // Humans in the environment
 73 | 
 74 |   // get param from launch file
 75 |   string laser_scan = "/scan";
 76 |   ros::param::get("~laser_scan", laser_scan);
 77 |   ros::Subscriber node_sub = n.subscribe(laser_scan, 2, LaserCallback);
 78 |   geometry_msgs::PoseArray msgx;
 79 |   ros::Rate loop_rate(15);
 80 | 
 81 | 
 82 |   int seq_counter = 0;
 83 |   
 84 |   while( ros::ok() ){
 85 |     if( sensor_on == true ){
 86 | 
 87 |       // delete persons who are too near to each other
 88 |       void ValidateDistance();
 89 | 
 90 |       //------------------------------------------
 91 |       // Copying to proper PoseArray data structure
 92 |       vector < geometry_msgs::Pose > HumanPoseVector;
 93 |       for( int K = 0; K < rec_x.size(); K++ ){
 94 | 	geometry_msgs::Point HumanPoint;
 95 | 	geometry_msgs::Quaternion HumanQuaternion;
 96 | 	
 97 | 	HumanPoint.x = rec_x[ K ];
 98 | 	HumanPoint.y = rec_y[ K ];
 99 | 	HumanPoint.z = 0; 
100 | 	
101 | 	HumanQuaternion.x = 0;//|-> Orientation is ignored
102 | 	HumanQuaternion.y = 0;//|
103 | 	HumanQuaternion.z = 0;//|
104 | 	HumanQuaternion.w = 1;//|
105 | 
106 | 	geometry_msgs::Pose HumanPose;
107 | 	HumanPose.position = HumanPoint;
108 | 	HumanPose.orientation= HumanQuaternion;
109 | 	HumanPoseVector.push_back( HumanPose );
110 |       }
111 | 
112 |       // Header config
113 |       msgx.header.stamp = ros::Time::now();
114 |       msgx.header.frame_id = SensorMsg.header.frame_id;
115 |       msgx.header.seq = seq_counter;
116 |       msgx.poses = HumanPoseVector;
117 |       //------------------------------------------
118 | 
119 |       node_pub.publish( msgx );
120 |     }
121 |     ros::spinOnce();
122 |     loop_rate.sleep();
123 |     seq_counter++;
124 |   }
125 | 
126 |   return 0;
127 | }
128 | 
129 | 
130 | void LaserCallback (const sensor_msgs::LaserScan::ConstPtr& msg){
131 | 
132 |   // To get header data from sensor msg
133 |   SensorMsg = *msg;
134 | 
135 |   // Vectors...
136 |   rec_x.clear(); 
137 |   rec_y.clear(); 
138 |   
139 |   sensor_on = true;
140 |   
141 |   double px, py, pr, pt;
142 |   vector < double >  laser_x;
143 |   vector < double >  laser_y;
144 |   vector < double >  laser_r;
145 |   vector < double >  laser_t;
146 |   for( unsigned i = 0; i < msg->ranges.size(); i++ ){    
147 |     pr = msg->ranges[ i ];
148 |     pt = msg->angle_min + ( i * msg->angle_increment);
149 |     laser_r.push_back( pr );
150 |     laser_t.push_back( pt );
151 |   }
152 |   
153 |   // Filtering laser scan
154 |   LaserFilter_Mean( &laser_r, FILTER_SIZE );
155 |   for( unsigned i = 0; i < msg->ranges.size(); i++ ){    
156 |     px = laser_r[ i ] * cos( laser_t[ i ] );
157 |     py = laser_r[ i ] * sin( laser_t[ i ] );
158 |     laser_x.push_back( px );
159 |     laser_y.push_back( py );
160 |   }
161 | 	 
162 |   string str_aux = "";
163 |   // Finding flanks in the laser scan...
164 |   vector < int > laser_flank;
165 |   laser_flank.assign(laser_r.size(), 0);
166 |   for( unsigned i = 1; i < laser_flank.size(); i++ ){
167 |     if( fabs( laser_r[ i ] - laser_r[ i - 1 ] ) > FLANK_THRESHOLD )
168 |       laser_flank[ i ] = ( ( laser_r[ i ] - laser_r[ i - 1 ] ) > 0 ) ? FLANK_U : FLANK_D;
169 |   }
170 |   
171 |   vector < int >    flank_id0;
172 |   vector < int >    flank_id1;
173 |   string flank_string = "";
174 |   int past_value = 0;
175 |   int idx = 0;
176 |   for( unsigned i = 1; i < laser_flank.size(); i++ ){
177 |     if( laser_flank[ i ] != 0 ){
178 |       if( past_value != laser_flank[ i ] ){
179 | 	flank_id0.push_back( i - 1 );
180 | 	flank_id1.push_back( i );
181 | 	flank_string += ( laser_flank[ i ] > 0 ) ? "S" : "B";
182 | 	idx++;
183 |       }
184 |       else
185 |       	flank_id1[ idx - 1 ] =  i;    
186 |     }
187 |     past_value = laser_flank[ i ];
188 |   }  
189 |     
190 |   // PATTERN RECOGNITION
191 |   string LEGS_LA  = "BSBS";
192 |   string LEGS_FS1 = "BBS";
193 |   string LEGS_FS2 = "BSS";
194 |   string LEGS_SL = "BS";
195 |   
196 |   list <int> Pattern_LA;
197 |   list <int> Pattern_FS1;
198 |   list <int> Pattern_FS2;
199 |   list <int> Pattern_SL;
200 |  
201 |   FindPattern( flank_string, LEGS_LA,  &Pattern_LA  );
202 |   FindPattern( flank_string, LEGS_FS1, &Pattern_FS1 );
203 |   FindPattern( flank_string, LEGS_FS2, &Pattern_FS2 );
204 |   FindPattern( flank_string, LEGS_SL,  &Pattern_SL  );  
205 | 
206 |   // ANTROPOMETRIC VALIDATION (the non antropometric patterns are erased from the list)
207 |   ValidatePattern( &Pattern_LA,  TYPE_LA, flank_id0, flank_id1,  laser_x, laser_y);
208 |   ValidatePattern( &Pattern_FS1, TYPE_FS, flank_id0, flank_id1,  laser_x, laser_y);
209 |   ValidatePattern( &Pattern_FS2, TYPE_FS, flank_id0, flank_id1,  laser_x, laser_y);
210 |   ValidatePattern( &Pattern_SL,  TYPE_SL, flank_id0, flank_id1,  laser_x, laser_y);
211 | 
212 |   // ERASE REDUNDANT PATTERNS FROM ACCEPTED ONES (If a LA or FS pattern is accepted, we erase the SL on it)
213 |   // a) Erase SL from LA
214 |   list<int>::iterator it_K;
215 |   for( it_K = Pattern_LA.begin(); it_K != Pattern_LA.end(); it_K++ ){
216 |     list<int>::iterator it_M;
217 |     // Erase first leg
218 |     for( it_M = Pattern_SL.begin(); it_M != Pattern_SL.end(); it_M++ )
219 |       if( flank_id0[ *it_K ] == flank_id0[ *it_M ] ){
220 | 	Pattern_SL.erase( it_M );
221 | 	break;
222 |       }
223 |     // Erase second leg
224 |     for( it_M = Pattern_SL.begin(); it_M != Pattern_SL.end(); it_M++ )
225 |       if( flank_id0[ *it_K + 2 ] == flank_id0[ *it_M ] ){
226 |     	Pattern_SL.erase( it_M );
227 |     	break;
228 |       }
229 | 
230 |   }
231 |   // b) Erase SL from FS1 "BBS"
232 |   for( it_K = Pattern_FS1.begin(); it_K != Pattern_FS1.end(); it_K++ ){
233 |     list<int>::iterator it_M;
234 |     for( it_M = Pattern_SL.begin(); it_M != Pattern_SL.end(); it_M++ )
235 |       if( flank_id0[ *it_K + 1 ] == flank_id0[ *it_M ] ){
236 |   	Pattern_SL.erase( it_M );
237 |   	break;
238 |       }
239 |   }
240 |   // c) Erase SL from FS2 "BSS"
241 |   for( it_K = Pattern_FS1.begin(); it_K != Pattern_FS1.end(); it_K++ ){
242 |     list<int>::iterator it_M;
243 |     for( it_M = Pattern_SL.begin(); it_M != Pattern_SL.end(); it_M++ )
244 |       if( flank_id0[ *it_K ] == flank_id0[ *it_M ] ){
245 |   	Pattern_SL.erase( it_M );
246 |   	break;
247 |       }
248 |   }
249 | 
250 |   
251 |   boost::mutex::scoped_lock lock(mutex);
252 |   //CENTROID PATTERN COMPUTATION & UNCERTAINTY
253 |   rec_x.clear();
254 |   rec_y.clear();
255 |   
256 |   HumanPose( &rec_x, &rec_y, Pattern_LA,  TYPE_LA,  flank_id0, flank_id1,  laser_x, laser_y);
257 |   HumanPose( &rec_x, &rec_y, Pattern_FS1, TYPE_FS,  flank_id0, flank_id1,  laser_x, laser_y);
258 |   HumanPose( &rec_x, &rec_y, Pattern_FS2, TYPE_FS,  flank_id0, flank_id1,  laser_x, laser_y);
259 |   HumanPose( &rec_x, &rec_y, Pattern_SL,  TYPE_SL,  flank_id0, flank_id1,  laser_x, laser_y);
260 | }
261 | 
262 | 
263 | // Mean value of the 'size' adjacent values
264 | void LaserFilter_Mean( vector <double> *vector_r, unsigned size ){
265 |   for( unsigned i = 0; i < ( (*vector_r).size() - size ); i++ ){
266 |       double mean = 0;
267 |       for( unsigned k = 0; k < size; k++  ){
268 | 	mean += (*vector_r)[ i + k ];
269 |       }
270 |       (*vector_r)[ i ] = mean / size;
271 |   }
272 | }
273 | 
274 | 
275 | // Reports a found string pattern in a list
276 | void FindPattern( string str, string pattern, list <int> *element_found ){
277 |   size_t found = 0;
278 | 
279 |   while( string::npos != ( found = str.find( pattern, found ) ) ){
280 |     (*element_found).push_back( found ); 
281 |     found++;
282 |   }
283 |   
284 | } 
285 | 
286 | 
287 | // Performs the antropometric validation of the leg patterns
288 | void ValidatePattern( list <int> *Pattern_list, int TYPE,  vector <int> flank_id0,  vector <int> flank_id1, vector <double> laser_x, vector <double> laser_y){
289 |   
290 |   double ANTRO_a_1, ANTRO_a_2, ANTRO_b, ANTRO_c; // Antropometric values from patterns to compare with constants.
291 |   bool SavePattern = true;
292 |   bool cond_a = true, cond_b = true, cond_c = true;
293 |   list<int>::iterator it;
294 |   
295 |   for( it = (*Pattern_list).begin(); it != (*Pattern_list).end(); it++ ){
296 | 
297 |     // Obtain antropometric values
298 |     switch( TYPE ){
299 |       case TYPE_LA: //BSBS
300 | 	ANTRO_a_1 = Dist2D( laser_x[ flank_id1[ *it ] ], laser_y[ flank_id1[ *it ] ], laser_x[ flank_id0[ *it + 1 ] ], laser_y[ flank_id0[ *it + 1 ] ]);
301 | 	ANTRO_a_2 = Dist2D( laser_x[ flank_id1[ *it + 2 ] ], laser_y[ flank_id1[ *it + 2 ] ], laser_x[ flank_id0[ *it + 3 ] ], laser_y[ flank_id0[ *it + 3 ] ]);
302 | 	ANTRO_b = Dist2D( laser_x[ flank_id0[ *it + 1 ] ], laser_y[ flank_id0[ *it + 1 ] ], laser_x[ flank_id1[ *it + 2 ] ], laser_y[ flank_id1[ *it + 2 ] ] );
303 | 	ANTRO_c = 0;
304 | 	cond_a = ( ( ANTRO_a_1 >= ANTRO_a0 ) && ( ANTRO_a_1 <= ANTRO_a1 ) ) && ( ( ANTRO_a_2 >= ANTRO_a0 ) && ( ANTRO_a_2 <= ANTRO_a1 ) );
305 | 	cond_b = ( ( ANTRO_b >= ANTRO_b0 ) && ( ANTRO_b <= ANTRO_b1 ) );
306 | 	cond_c = true;
307 |         break;
308 |       case TYPE_FS: // BBS & BSS
309 | 	ANTRO_a_1 = Dist2D( laser_x[ flank_id1[ *it ] ], laser_y[ flank_id1[ *it ] ], laser_x[ flank_id0[ *it + 1 ] ], laser_y[ flank_id0[ *it + 1 ] ]);
310 | 	ANTRO_a_2 = Dist2D( laser_x[ flank_id1[ *it + 1 ] ], laser_y[ flank_id1[ *it + 1 ] ], laser_x[ flank_id0[ *it + 2 ] ], laser_y[ flank_id0[ *it + 2 ] ]);
311 | 	ANTRO_b = Dist2D( laser_x[ flank_id0[ *it + 1 ] ], laser_y[ flank_id0[ *it + 1 ] ], laser_x[ flank_id1[ *it + 1 ] ], laser_y[ flank_id1[ *it + 1 ] ] );
312 | 	ANTRO_c = 0;
313 | 	cond_a = ( ( ANTRO_a_1 >= ANTRO_a0 ) && ( ANTRO_a_1 <= ANTRO_a1 ) ) && ( ( ANTRO_a_2 >= ANTRO_a0 ) && ( ANTRO_a_2 <= ANTRO_a1 ) );
314 | 	cond_b = ( ( ANTRO_b >= ANTRO_b0 ) && ( ANTRO_b <= ANTRO_b1 ) );
315 | 	cond_c = true;
316 |         break;
317 |     case TYPE_SL: // BS
318 |       	ANTRO_a_1 = 0;
319 | 	ANTRO_a_2 = 0;
320 | 	ANTRO_b = 0;
321 | 	ANTRO_c = Dist2D( laser_x[ flank_id1[ *it ] ], laser_y[ flank_id1[ *it ] ], laser_x[ flank_id0[ *it + 1 ] ], laser_y[ flank_id0[ *it + 1 ] ]);
322 | 	cond_a = true;
323 | 	cond_b = true;	
324 | 	cond_c = ( ( ANTRO_c >= ANTRO_c0 ) && ( ANTRO_c <= ANTRO_c1 ) );
325 | 	break;
326 |     }
327 | 
328 |     SavePattern = cond_a && cond_b && cond_c;
329 |     
330 |     if( !SavePattern ){
331 |       it = (*Pattern_list).erase( it );
332 |       it--;
333 |     }
334 |   }  
335 | }
336 | 
337 | 
338 | // Euclidean distance between two coordinate points
339 | double Dist2D( double x0, double y0, double x1, double y1 ){
340 |   return sqrt( pow( x0 - x1, 2 ) + pow( y0 - y1, 2 ) );
341 | }
342 | 
343 | 
344 | void HumanPose( vector <double> *r_x, vector <double> *r_y, list <int> Pattern_list, int TYPE,  vector <int> flank_id0,  vector <int> flank_id1, vector <double> laser_x, vector <double> laser_y ){
345 |   
346 |   double c_x, c_y;
347 |   int l1, l2, l3, l4;
348 |   int count; 
349 |   list<int>::iterator it;
350 | 
351 |   for( it = Pattern_list.begin(); it != Pattern_list.end(); it++ ){
352 |     c_x = 0;
353 |     c_y = 0;
354 |     count = 0;
355 | 
356 |     l1 = flank_id1[ *it ];
357 |     l2 = flank_id0[ *it + 1 ];
358 |     
359 |     switch( TYPE ){
360 |     case TYPE_LA:
361 |       l3 = flank_id1[ *it + 2 ];
362 |       l4 = flank_id0[ *it + 3 ];
363 |       break;
364 |     case TYPE_FS:
365 |       l3 = flank_id1[ *it + 1 ];
366 |       l4 = flank_id0[ *it + 2 ];
367 |       break;
368 |     case TYPE_SL:
369 |       l3 = 1;
370 |       l4 = 0;
371 |       break;
372 |     }
373 | 
374 |     for( int i = l1; i <= l2; i++ ){
375 |       c_x += laser_x[ i ];
376 |       c_y += laser_y[ i ];
377 |       count++;
378 |     }
379 |     for( int i = l3; i <= l4; i++ ){
380 |       c_x += laser_x[ i ];
381 |       c_y += laser_y[ i ];
382 |       count++;
383 |     }
384 |     
385 |     c_x /= (double) count;
386 |     c_y /= (double) count;
387 |     
388 |     (*r_x).push_back( c_x );
389 |     (*r_y).push_back( c_y );
390 |   }
391 | }
392 | 
393 | 
394 | // Validate distance between persons
395 | void ValidateDistance(){
396 |     boost::mutex::scoped_lock lock(mutex);
397 |     int j = 0;
398 |     while(j < (rec_x.size() - 1))
399 |     {
400 |         // if the Euclidean distance between two persons are smaller than
401 |         // the maximum width of a leg then the second person must be eliminated
402 |         if (ANTRO_b1 > Dist2D(rec_x[j], rec_y[j], rec_x[j+1], rec_y[j+1]))
403 |         {
404 |             rec_x.erase(rec_x.begin() + (j + 1));
405 |             rec_y.erase(rec_y.begin() + (j + 1));
406 |         }
407 |         else
408 |         {
409 |             j++;
410 |         }
411 |     }
412 | }
413 | 


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