peter-nebe / stair-step-detector

Detection of stair steps

stair-step-detector

Detection of stair steps using a LiDAR depth camera

Dependencies

• Intel RealSense SDK
• OpenCV
• OpenGL
• Boost
• ROS (only for the ROS2 wrapper)

Overview

The camera is calibrated with the help of three points in the ground plane. There is more information about calibration here.

After calibration, the camera can measure the heights and corner points of the stair steps that are in the measurement area. External world coordinates are displayed on the left.

Knowing the exact positions of the individual steps can help a mobile robot to negotiate stairs, if it is able to do so in principle. For a robot, ascending and descending stairs may be different challenges. But there is only one position and line of sight for the camera to fully capture a step. If the camera is attached to the robot, it must always be brought into the correct measuring position using suitable mechanics. In principle it could look like this:

If the robot is clever, it ensures that the image is not upside down by rotating the camera 180 degrees around the optical axis. If it doesn't, the image has to be rotated (or mirrored) using software. Then the algorithm of the stair-step-detector can be used for both ascending and descending stairs.

The camera used

The stair-step-detector currently uses the Intel® RealSense™ LiDAR Camera L515. The depth accuracy of the L515 is better than that of the RealSense stereo cameras. But there can be many reasons why someone might want to use the stair-step-detector with a different camera, from RealSense or another manufacturer. In principle, any depth camera can be used. For this purpose, at least the class Camera must be adapted, and depending on the features of the camera in question, additional classes. The least effort is to be expected with another RealSense camera, because the RealSense SDK can then also be used. The stair-step-detector depends on the L515 and the RealSense SDK in the following way:

• An infrared image and the associated depth image are taken for the geometric calibration.
• The calibration marks are localized in the infrared image, and the 3D coordinates are determined for these 2D positions.
• In measurement mode, the SDK delivers point clouds in distortion-free Cartesian coordinates.
• For the sake of simplicity, the window class from the SDK examples is used for the application window.

The adaptation effort would be greatest when using two conventional cameras as a stereo pair, but thanks to OpenCV that is also possible in principle.

Integration with ROS

If you are familiar with the Robot Operating System, you can use the simple wrapper for ROS2 Galactic Geochelone. Among other things, it contains a ROS package with which the measured stair steps can be visualized. The display is done with RViz, the 3D visualization tool for ROS. Here you can see an example:

See ROS Wiki for more details.

MDE

It becomes even more convenient: If you are interested in Model-Driven Engineering (MDE), you can use the model of the stair-step-detector. You can combine this with other ROS components and create an ROS system without manual coding. The code generation is done by sophisticated and advanced tools.

Diagram of an exemplary ROS system

You can find the tools you need here: https://github.com/ipa320/ros-model

stair-step-detector's Issues

Research paper

Is there any related research papers that used this project?

Calibration and detection

Hi,

I've tried building and running the code, specifically testing it with handmade stairs (using boxes). Unfortunately, it seems that it couldn't accurately detect the stair. I suspect this might be related to calibration issues. However, after reviewing the documentation, I couldn't find clear instructions on how to calibrate the camera. Do you have any suggestions to improve my testing, especially regarding the calibration process for detecting boxes accurately?
Thanks

Erro in building

Hi.
I've tried to build the code. But I got this error. I wonder if you have any guidance on how can solve this issue.
My Ubuntu version is 20.04.
Thanks

Error:

[ 45%] Building CXX object CMakeFiles/detect-stairs.dir/stairs.cpp.o
[ 50%] Building CXX object CMakeFiles/detect-stairs.dir/transformation.cpp.o
[ 54%] Building CXX object CMakeFiles/detect-stairs.dir/window.cpp.o
[ 59%] Linking CXX executable detect-stairs
/usr/bin/ld: CMakeFiles/detect-stairs.dir/geometricCalibration.cpp.o: in function device_with_streams(std::vector<rs2_stream, std::allocator<rs2_stream> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)': geometricCalibration.cpp:(.text+0x4fe): multiple definition of device_with_streams(std::vector<rs2_stream, std::allocator<rs2_stream> >, std::__cxx11::basic_string<char, std::char_traits, std::allocator >&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x4fe): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/geometricCalibration.cpp.o: in function set_viewport(rect const&)': geometricCalibration.cpp:(.text+0xd86): multiple definition of set_viewport(rect const&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0xd86): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/geometricCalibration.cpp.o: in function draw_pointcloud(float, float, glfw_state&, rs2::points&)': geometricCalibration.cpp:(.text+0xe35): multiple definition of draw_pointcloud(float, float, glfw_state&, rs2::points&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0xe35): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/geometricCalibration.cpp.o: in function quat2mat(rs2_quaternion&, float*)': geometricCalibration.cpp:(.text+0x11dd): multiple definition of quat2mat(rs2_quaternion&, float*)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x11dd): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/geometricCalibration.cpp.o: in function draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)': geometricCalibration.cpp:(.text+0x1538): multiple definition of draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x1538): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/geometricCalibration.cpp.o: in function register_glfw_callbacks(window&, glfw_state&)': geometricCalibration.cpp:(.text+0x1ceb): multiple definition of register_glfw_callbacks(window&, glfw_state&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x1ceb): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/geometricCalibration.cpp.o: in function get_screen_resolution(unsigned int&, unsigned int&)': geometricCalibration.cpp:(.text+0x1d9d): multiple definition of get_screen_resolution(unsigned int&, unsigned int&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x1d9d): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/pointcloud.cpp.o: in function device_with_streams(std::vector<rs2_stream, std::allocator<rs2_stream> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)': pointcloud.cpp:(.text+0x4fe): multiple definition of device_with_streams(std::vector<rs2_stream, std::allocator<rs2_stream> >, std::__cxx11::basic_string<char, std::char_traits, std::allocator >&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x4fe): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/pointcloud.cpp.o: in function set_viewport(rect const&)': pointcloud.cpp:(.text+0xd86): multiple definition of set_viewport(rect const&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0xd86): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/pointcloud.cpp.o: in function draw_pointcloud(float, float, glfw_state&, rs2::points&)': pointcloud.cpp:(.text+0xe35): multiple definition of draw_pointcloud(float, float, glfw_state&, rs2::points&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0xe35): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/pointcloud.cpp.o: in function quat2mat(rs2_quaternion&, float*)': pointcloud.cpp:(.text+0x11dd): multiple definition of quat2mat(rs2_quaternion&, float*)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x11dd): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/pointcloud.cpp.o: in function draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)': pointcloud.cpp:(.text+0x1538): multiple definition of draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x1538): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/pointcloud.cpp.o: in function register_glfw_callbacks(window&, glfw_state&)': pointcloud.cpp:(.text+0x1ceb): multiple definition of register_glfw_callbacks(window&, glfw_state&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x1ceb): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/pointcloud.cpp.o: in function get_screen_resolution(unsigned int&, unsigned int&)': pointcloud.cpp:(.text+0x1d9d): multiple definition of get_screen_resolution(unsigned int&, unsigned int&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x1d9d): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/window.cpp.o: in function device_with_streams(std::vector<rs2_stream, std::allocator<rs2_stream> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)': window.cpp:(.text+0x4fe): multiple definition of device_with_streams(std::vector<rs2_stream, std::allocator<rs2_stream> >, std::__cxx11::basic_string<char, std::char_traits, std::allocator >&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x4fe): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/window.cpp.o: in function set_viewport(rect const&)': window.cpp:(.text+0xd86): multiple definition of set_viewport(rect const&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0xd86): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/window.cpp.o: in function draw_pointcloud(float, float, glfw_state&, rs2::points&)': window.cpp:(.text+0xe35): multiple definition of draw_pointcloud(float, float, glfw_state&, rs2::points&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0xe35): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/window.cpp.o: in function quat2mat(rs2_quaternion&, float*)': window.cpp:(.text+0x11dd): multiple definition of quat2mat(rs2_quaternion&, float*)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x11dd): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/window.cpp.o: in function draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)': window.cpp:(.text+0x1538): multiple definition of draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x1538): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/window.cpp.o: in function register_glfw_callbacks(window&, glfw_state&)': window.cpp:(.text+0x1ceb): multiple definition of register_glfw_callbacks(window&, glfw_state&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x1ceb): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/window.cpp.o: in function get_screen_resolution(unsigned int&, unsigned int&)': window.cpp:(.text+0x1d9d): multiple definition of get_screen_resolution(unsigned int&, unsigned int&)'; CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o:detect-stairs.cpp:(.text+0x1d9d): first defined here
/usr/bin/ld: CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o: in function draw_pointcloud(float, float, glfw_state&, rs2::points&)': detect-stairs.cpp:(.text+0xf00): undefined reference to gluPerspective'
/usr/bin/ld: detect-stairs.cpp:(.text+0xf5e): undefined reference to gluLookAt' /usr/bin/ld: CMakeFiles/detect-stairs.dir/detect-stairs.cpp.o: in function draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)':
detect-stairs.cpp:(.text+0x1630): undefined reference to gluPerspective' /usr/bin/ld: CMakeFiles/detect-stairs.dir/geometricCalibration.cpp.o: in function draw_pointcloud(float, float, glfw_state&, rs2::points&)':
geometricCalibration.cpp:(.text+0xf00): undefined reference to gluPerspective' /usr/bin/ld: geometricCalibration.cpp:(.text+0xf5e): undefined reference to gluLookAt'
/usr/bin/ld: CMakeFiles/detect-stairs.dir/geometricCalibration.cpp.o: in function draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)': geometricCalibration.cpp:(.text+0x1630): undefined reference to gluPerspective'
/usr/bin/ld: CMakeFiles/detect-stairs.dir/pointcloud.cpp.o: in function draw_pointcloud(float, float, glfw_state&, rs2::points&)': pointcloud.cpp:(.text+0xf00): undefined reference to gluPerspective'
/usr/bin/ld: pointcloud.cpp:(.text+0xf5e): undefined reference to gluLookAt' /usr/bin/ld: CMakeFiles/detect-stairs.dir/pointcloud.cpp.o: in function draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)':
pointcloud.cpp:(.text+0x1630): undefined reference to gluPerspective' /usr/bin/ld: CMakeFiles/detect-stairs.dir/window.cpp.o: in function draw_pointcloud(float, float, glfw_state&, rs2::points&)':
window.cpp:(.text+0xf00): undefined reference to gluPerspective' /usr/bin/ld: window.cpp:(.text+0xf5e): undefined reference to gluLookAt'
/usr/bin/ld: CMakeFiles/detect-stairs.dir/window.cpp.o: in function draw_pointcloud_wrt_world(float, float, glfw_state&, rs2::points&, rs2_pose&, float*, std::vector<rs2_vector, std::allocator<rs2_vector> >&)': window.cpp:(.text+0x1630): undefined reference to gluPerspective'
collect2: error: ld returned 1 exit status
gmake[2]: *** [CMakeFiles/detect-stairs.dir/build.make:273: detect-stairs] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:85: CMakeFiles/detect-stairs.dir/all] Error 2
gmake: *** [Makefile:91: all] Error 2