To implement a C++ object-oriented program to solve a Geomatics Engineering problem, namely 3D plane extraction from point clouds for reconstructing the 3D shape of a room. To complete the geometric reconstruction and plotting in MATLAB.
Although it is recommended to complete this lab individually, you may complete it in groups of two students. If completed in pairs: make sure to submit exact same copies to both students’ Dropbox folder, make sure that both people’s names are on the lab report and remember that both people will receive the same grade.
This lab combines all of the C++ skills you have learned in ENGO 333 (so far) to solve a real Geomatics Engineering problem, namely processing 3D data.
Point cloud is simply a set of 3D points (points with X,Y,Z coordinates) that can be collected in different ways. One of the ways to collect point clouds of objects and environment is 3D laser scanning. A laser scanner measures distance from the scanner to the objects; each distance is associated with a vertical and a horizontal angle as well. Using these three measurements, we can calculate the 3D coordinates of a point.
In this assignment, you are provided with a text file “Points.txt” that contains the labels (IDs) and the 3D coordinates of a point cloud collected from an empty room and a human standing in the middle of the room as shown in Figure 1.
Due to several reasons, the point clouds may contain “outliers”. Outliers mean points that are not consistent with their surrounding points in geometric sense. For instance, points that are, supposedly, collected from a flat wall but are located outside the planar surface of the wall, as shown in Figure 2, are outliers.
If the geometric shape of an object is known, then detecting the outliers is simple. For instance, in the above example, if one can find the model of the 3D plane, then the points that don’t lie on that plane can be recognized as outliers.
Once the planes are determined, their intersection will result in edge lines of the room. Figure 3 shows a sample edge line which is the result of interesting the floor plane with the right wall.
Once the lines are determined, their intersection will result in the corner points. For instance, in Figure 4, the intersection of the two floor edge lines (red and blue) results in a corner point. If a corner point can be determined as the intersection of more than 2 lines (e.g. the red, blue and yellow lines), then one can find the corner point from any of the two lines and then take the average of the results as the final coordinates of the corner pint.
The process explained above is called 3D feature extraction from point clouds, where the features of interest are planes, edge lines and corner points. Feature extraction is a very important topic in digital terrain modelling, object recognition, and visualization.
What we will practice in this assignment is to simultaneously extract the walls of the room (planes) from the point cloud of Figure 1 and to detect/remove the outliers as well.
Code to accomplish plane extraction from point clouds is available extensively online but avoid temptation to download a canned answer. Most of the algorithms available online are not the same as the algorithm you will be implementing here. They may be more efficient or more generalized but are also very difficult to understand for a beginner. Instead, follow the instructions below and develop your own feature-extraction software. Also, please keep in mind that we a rigorous system of plagiarism detection from codes; this includes codes from class 2017!
A 3D plane can be defined using four parameters as in Equation 1. Parameters nx, ny, nz are showing the direction of the normal vector to the plane and the parameter d shows the distance of the plane from the origin of the coordinate system (See Figure 5). X, Y and Z are the coordinates of any 3D point that lies on this plane.
Give a point, like point E in Figure 6, with coordinates Xi, Yi and Zi , the distance D from the plane can be calculated as in Equation 2.
This lab contains a lot of different elements and requires you to write all of the code from scratch; so, start early.
Make sure that you thoroughly test each part of your work before proceeding to the next step (compile and debug your code frequently and after every change). Some hints are provided in the Appendix 1 as how to test that your algorithms are implemented correctly. Be sure to describe how you tested each step in your lab report. In you program, you MUST use the Eigen library to work with matrices.
You will start this lab assignment from scratch by creating an empty Visual Studio solution. This solution must be self-contained, in which all additional libraries (e.g. Eigen Folder) must be correctly included. Follow these instructions to create a self-contained Visual Studio solution:
- Create an empty Visual C++ project solution. Name the solution and project directories accordingly.
- Download the following files from D2L: “Eigen.zip”, “Points.txt” and “Seeds1.txt” to “Seeds6.txt”.
- “Eigen.zip” contains a folder named “Eigen” and it has all Eigen Matrix library files. Extract this folder, “Eigen” and copy it beside the solution folder. For instance, if your solution folder is located on your Desktop, then the folder Eigen must be on the Desktop as well.
- Add include path for Eigen library into the project properties (additional include directories). If you follow these instructions, you just need to add ..\..\Eigen\.
- Put all downloaded *.txt files into project directory as well (physically inside the project folder).
The file “Points.txt” contains a list of points. The first column of the file contains the label (ID) of each point, followed by the X, Y and Z coordinates of the points in next columns. You may want to plot the points using MATLAB first; you should see a figure similar to Figure 1. The files “Seeds[n].txt” contain the IDs of the points (containing both inliers and outliers) that are, supposedly, on plane [n]. For instance, “Seeds1.txt” contains the IDs of some of the points that lie on the floor plane (red points in Figure 7). Figure 7 shows the seed points of different planes plotted in different colors.
Define a structure* called Point that should have member variables for the ID and the X, Y and Z coordinates of a point.
- If you’d like, you can create a class Point instead of a structure.
Create a class called Plane containing the followings:
- a) four private data members, for storing the four parameters of a plane model (nx, ny, nz, d)
- b) a default constructor that sets the private data members to zeros
- c) an overloaded constructor that receives the values of all the private data members and sets them
- d) a member function that receives a point object (with type Point) and measures the distance of the plane to this point and returns the calculated distance
- e) a setter member function that sets the values of the private data members
- f) a getter member function that retrieves the values of the private data members
- g) a mutator member function that receives a vector of 3D coordinates of points (either a vector[Point] or a matrix with n rows and 3 columns or a matrix with n rows and 4 columns (including the ID) [you can chose either way]) and uses the Algorithm 1 to fit a plane to these points, and calculates and updates the parameters of the plane.
This algorithm uses the concepts of leas-squares adjustment, about which you will learn a lot next semester. This algorithm uses very simple linear and non-linear least-squares solutions to make the implementation easy for you; but, you will learn their sophisticated versions later.
Write the prototype and definition of a function using Algorithm 2. Make this function a friend of class Plane.
Note that there are various versions of RANSAC, and the one you implement in this assignment is the simplest form of RANSAC. To read more about this algorithm, you can search on WikiPedia.
Add the definitions and prototypes of function Read_Mat() and Write_Mat() from Assignment 8 to your program.
Perform the following tasks in the main() function.
- a) Read the data in file “Points.txt” into a matrix. Suppose that the size of the matrix is n-by-4, where n is the number of points in this file.
- b) Create an empty vector of Planes
- c) Create a matrix I of size n-by-1, and fill it with zeros.
- d) In a loop (from 1 to 6):
- Read each of the files “Seeds[n].txt”. Given the IDs of the points in the file, find their 3D coordinates from the original matrix of point coordinates (the one you read in Task 6.a).
- Use the function developed in Task 4 and calculate the correct parameters of a plane that fits to these seed points
- Add the plane to the vector of Planes (the one created in Task 6.b)
- For each point in the original matrix of point coordinates (the one read in Task 6.a)
- Calculate the distance of the point from the plane
- if the distance is smaller than 0.01
- set the corresponding element in I (the matrix created in Task 6.c) equal to the plane number (a number between 1 to 6, depending on which iteration of loop 6.d you are in; this shows to which plane this point belongs)
- e) Create a new matrix and concatenate the matrix of original points (from Task 6.a) and the matrix I f) Write the matrix created in Task 6.e (a n-by5 matrix) to a file called “Plane_Points.txt”
- g) Write the parameters of the planes from the vector of Planes to a file called “Planes.txt”
Create a new MATLAB script. Write a code to load the data from “Plane_Points.txt”. Use six different colors to show the points on different planes (in one figure only). So, your figure should be painted in 6 different colors.
Please submit the followings via D2L:
- The report file, which MUST be in PDF format.
From the Word Application, go to File Save as Adobe PDF.
Your lab report must describe what you did (and your developed codes) for all the above tasks and must include all the relevant output/results/figures/plots, as well as answers to any other questions asked in the above tasks. Make sure the content of the file “Planes.txt” is also in your report.
- The zipped solution folder, which MUST contain all the completed source files as well as the input and output text files. Please follow the instructions that we have always given in previous labs to submit the zipped solution folder.
- The MATLAB script created for Task 7 (and possibly for the Bonus Task).