Stair detection algorithm based on C++ and PCL