Robotics 2017: Basic Implementation and Measurements of Plane Detection in Point Clouds

A project of the Robotics 2017 class of the School of Information Science and Technology (SIST) of ShanghaiTech University. Course Instructor: Prof. Sören Schwertfeger.

Team Members: Chen Chen, Wentao Lv, Yuan Yuan


In practical robotics research, plane detection is an important prerequisite to a wide variety of vision tasks. Plane detection means to detect the plane information from some basic disjoint information, for example, the point cloud. In this project, we will use point clouds from FARO devices which contains a set of position(x,y,z, and theta) and the RGB color. At first, we will try to implement some of algorithms to extract plane information from point cloud data and do measurement between them. And then we might do some path planning based on this data and try to find a state-of-art solution on point cloud environment information path planning on indoor situation.

Software Platform

Point Cloud Library (PCL) is a standalone, large scale, open project for 2D/3D image and point cloud processing, which is released under BSD license and free for research use. It is crossplatform, and has been successfully compiled and deployed on Linux, macOS, Windows and Android/iOS. The PCL framework contains numerous state-of-the art algorithms including filtering, feature estimation, surface reconstruction, registration, model fitting and segmentation. These algorithms can be used, for example, to filter outliers from noisy data, stitch 3D point clouds together, segment relevant parts of a scene, extract key points and compute descriptors to recognize objects in the world based on their geometric appearance, and create surfaces from point clouds and visualize them. To work as a multi-functional software, the PCL has some independent parts which could be compiled separately, to serve as a module. The modularity design is important for distributing PCL on different platforms with reduced computational or size constraints, and could possibly gain a better performance. The PCL has its ROS package version, and we can utilize its functions with other ROS packages together.

System Description


As is suggested by the contents above, we have known that there are some algorithms which have proved the point cloud plane detection is feasible. We are going to implement one of these algorithms. If possible, we will also do some optimization on these algorithms. After the implementation part, we get the planes in a point cloud. Thus we could use the plane data to do some work like robot path planning, or tagging the areas for better robot mapping. The plane extraction is feasible, one potential problem is the system complexity. Since the sensor we use is the FARO laser scanner, the point cloud itself contains much information. We could possibly down sampling the data, or find a way to compress data without information loss, to make the plane detection faster.


We get the FARO data from Prof. Schwertfeger. The data is xyz format. For data containing position and color information, a line in an xyz format is as the following:

  • index in the first dimension
  • index in the second dimension
  • x position
  • y position
  • z position
  • red color in uint8 type
  • green color in uint8 type
  • blue color in uint8 type

However, PCL works on pcd format. For data containing position and color information, a line in an pcd format is as the following:

  • x position
  • y position
  • z position
  • rgb color in uint32 type

Hence, we write a python code to transform xyz to pcd. In our test, for an xyz file with size 106 MB (containing information of about 2.39 million points) , the transformation can be done with in about 7 seconds.

Plane Extraction

We write a cplusplus code with PCL to extract planes from point clouds. After running the program, several pcd files of planes will be written if some planes are found.

We write a cplusplus code to show the result. It reads pcd files of planes and set different colors for points belonging to different planes. Then it shows the result with pcl_viewer.


Better Implementation

Hull extraction

The hull extraction result of points in figure 6 is shown in figure 7. The ”wave” pattern on the two sides are caused by the precision of the scanner, as there are fewer points. This work is done by the function “segment_hull” in our cpp code.


Figure 8 is the triangulation result of figure 6. This plane represents the ground, but in reality they are not on the same height. Thus we got some small triangular planes, with errors. This work is done by the function “fast_triangles” in our cpp code.


We fit the triangular planes to a single flat plane, as is shown in figure 9. This work is done by the function “B_spline” in our cpp code. The iteration runs for around hours, but the result is not bad. With better parameters, we possibly could get better results.


System Evaluation

First we have to check the correctness of our algorithm implementation as there could be some wrongly recognized plane in the output data. Once we can determine that weve got the correct planes, we optimize it.

We can compare the optimized result and the raw output of the algorithm. With the information that we have got, the infinite plane could be generated. We can compare the aligned planes map with the raw point cloud data, and see if every obvious planes have been extracted. A better evaluation method like visualization (texture mapping) or some other things could also possibly be introduced, too.


In this project we implement the plane detection in point clouds scanned by a Faro Focus 3D X330. We present a method to measure the quality of the plane detection. In the future, the result of plane detection will be used to robot navigation.