This version improves the multi-thread performances. We also add functions to support the robot track the pedestrians.