Final Report: Laser Scan Matching and Odometry

Team HexaKopterDaniel PerryPaul BartholomewSam Olson

Table of ContentsIntroduction.................................................................................................................................................3

Contour Extraction......................................................................................................................................6

Iterative Closest Point Algorithm.................................................................................................................8

Grid-Based Occupany Grids.......................................................................................................................12

Quad-Tree Data Structures........................................................................................................................16

Height Estimation......................................................................................................................................17

Function Specification Document..............................................................................................................22

Concept Generation and Selection............................................................................................................24

Introduction

The purpose of our part of the project was to be able to determine location and movement of the

unmanned aerial vehicle using the laser range finder. Our original goals were as follows:

Goals to be Completed

1) Build a contour map

2) Using a probabilistic model determine the location of the flying aircraft

3) Build a global map using a series of local contour maps

4) Refine position as predicted using aircraft orientation

In order to complete these goals, we were given a very specific set of guidelines about how we were

supposed to implement these objectives. This implementation is as follows:

Block Diagram of Proposed Solution

Information

Data Out

Design Criteria

Read in output data from the laser—Since the hexacopter cannot see, the only means it has to know

where obstacles lie is through the input of this external device.

Convert the data into Cartesian coordinates from polar—Later on in this project a global map will be

required. Our customer has specified that they would like the output to be a 3-dimensional diagram

Heap/Tree data structure

Line segments

Contour Extraction

Log-Likelihood Map

Global map

Hexacopter Location

which includes the features of a room. We along with the customer feel the best implementation of this

design is with a Cartesian map.

Connect data points with each nearest neighbor to create line segments—Connection of individual data

points will create the useable line segments to later form a map.

Create contours which represent surfaces and slopes—The use of contours is necessary as hundreds of

data points are input from each laser scan. These individual data points after connected represent

surfaces and features of the room. The connection of the line segments to a contour can make the

process efficient enough to run at the required rate as a contour describe hundreds of points with a

single line by two points and a slope.

Create a Log-Likelihood map that includes contour segments—The Log-Likelihood map is a map that we

use to account for the error in the laser reading caused by bobbing of the laser range finder and noise in

the readings. We will do this by sliding a Gaussian along each of the contours.

Use Log-Likelihood map to refine Hexacopter’s location—This will show the hexacopter where it is able

to fly without bumping into obstacles. This will also be used to refine the hexacopter’s pose.

Use multiple scans to create a 3-dimensional local map—The vehicle will be flying through a 3D world. It

needs to understand where obstacles lie and the probabilities of each of the features of the wall being

at the point where they have been measured.

Integrate the local map into a 3-dimensional global map—The robot needs to know where it has been

and be able to know if it has already been in a particular location.

Contour Extraction

We were assigned to create MatLab code that would build maps and determine the location of

the aircraft at any point in time. The graduate students originally assigned us to work on the localization

problem. We were asked to read in a scan, take all the points from the scan and convert them to

contour lines. Then using these scans, we were asked to slide a Gaussian probability over each contour

and compare the probability of each line to a global map to determine where in the known space that

the flying aircraft was located. We wanted the code to be able to leave out noisy data. We generated

our own set of data with an outlier that was left out and used our code. Using actual data from the laser

scanner, we used our data to extract the following contour as seen in figure 1. The code to generate this

is found on the wiki page associated with our project under the Contour Mapping Section.

The code worked fairly well but the graduate students overseeing us decided that they wanted us to

work on different parts of the project and split us off into our own individual groups.

After a few weeks, the graduate students determined that this project would be beyond our

knowledge to solve, so we were all assigned different tasks. Daniel was assigned to work on the

Figure 1: Laser data that we plotted using our code.

Iterative closest point algorithm, Paul was assigned to building grid-based occupancy maps, and Sam

was assigned to build a quad-tree data structure.

Iterative Closest Point Algorithm

We have worked on implementing an iterative closest point algorithum (ICP) for use in

odometry for a UAV. Specifically I have been reading in successive laser scans to determine

how the vehicle moved. Unfortunately, we have decided that this implementation has too

much error in it to be an effective method of determining position.

Test 1

We started by doing a known rotation and translation on a set of data. We ran it through an

ICP algorithum and were able to obtain the exact same arbitrary rotation and translation vector

that we originally used to modify the known data. This test can be found in our code test.m.

The output is seen in figure 2.

Figure 2 The output of the ICP algorithum with a known rotation and translation

Attempted Implementation

This data showed that the ICP algorithum could directly find the rotation and translation

matricies if all the points lined up. We then tried to implement the code with real data that we

took from consecutive scans. We read in a series of scans and then changed the translation and

rotation matrices so show how the UAV moved. The output of the data did not represent how

the UAV moved. Figure 3 shows the output from running the code myICP.m.

Figure 3: The ICP algorithm’s projection of position

When this output was compared to cortex data showing the position of the UAV, the actual

position did not match up with what the ICP algorithm’s projection was. We originally thought

it was the implementation of the rotation and translation matrices until we tried another test.

Test 2

We decided to invert the translation and rotation matrices and continuously add them together

to track total rotation and translation. We then applied this new matrix to the different scans

that were taken. In theory if the ICP algorithm worked, each scan would plot on top of the

previous scan. When we implemented this it worked really well for the first 5 scans as seen in

figure 4.

Figure 4: The first 5 sets of data plotted on top of each other

As we added more and more scans to the code, error started to accumulate and the new sets of

data did not plot on top of previous ones but started to drift significantly. The output from all

of the scans is shown in figure 5.

Figure 5: Output of myICP2.m. It shows all of the successive data points mapped on top of each other.

Summary

The ICP implementation has no way of accounting for error in the system. This causes error to

be amplified through every rendition of the code. Because of these problems we have decided

to use a different implementation to determine the location of the UAV. Later after talking to

Dr. Beard, he suggested that the ICP might still work if we implemented it with a Kalman filter. I

talked to our grad students about this and they wanted me to focus on our new project of

height estimation and not return to the ICP algorithm.

Figure 6. Rotation of Laser Scans as Plotted in Matlab

Grid-Based Occupany Grids

Paul was assigned to work with Everett Bryan to create local maps that contain information from

several scans returned from the laser range finder. This information will later be used to create a global

map and determine where in space the flying aircraft is most likely located.

This problem was solved by reading in scans and rotating the points from each scan based on

values returned from the Cortex Room and plotting them on from Matlab to make sure everything was

changed correctly as seen in Figure 6.

After this was accomplished, we created a matrix and began to assign the points to specific cells

based on their return location. This created an occupancy grid map as shown in Figure 7.

The code to build the occupancy grids is as follows:

% Build a world map of the cortex data. hold off; clear all; clc; clf; figure(1);%% Import cortex psi angles% The data is imported as a Struct. P contains two parts: data and text% data. The data of interest is the psi angle which is contained in% "P.data".first_good_data_point = 45;P = importdata('cortexdata.txt'); % Import datapsi = P.data; % Extract psix = 1000.*str2double(P.textdata(:,2)) + 4000; % Extract x from cell arrayy = 1000.*str2double(P.textdata(:,4)) - 5000; % Extract Y from cell arraytheta = linspace(-120,120,682)*pi/180;hold on;res = 75; %millimeters resolution

%% Import data scan and convert range data to <x,y> values% Iterate through each of the scans creating a new data file for each data% scan. for current_scan_index = 0:length(psi)-1 % Build the name of the file index_num = num2str(current_scan_index); % Convert index val to string scan_number = strcat('scan',index_num,'.txt'); % Concatonate the number and the string name % Import and prepare the scan data

Figure 7. Grid-Based Occupancy Map with 7.5cm Resolution

current_laser_scan_data = importdata(scan_number)'; current_laser_scan_data = current_laser_scan_data(first_good_data_point:end); % Remove the preceeding (-1)s from data set current_laser_scan_data = [current_laser_scan_data.*cos(theta') current_laser_scan_data.*sin(theta')]; j = length(current_laser_scan_data); for i = 1:j if (current_laser_scan_data(j-i+1,1) < 20 && current_laser_scan_data(j-i+1,2) < 20) current_laser_scan_data(j-i+1,:) = []; end end

if current_scan_index == 0 updateOccupancyGrid( x(current_scan_index+1), y(current_scan_index+1), psi(current_scan_index+1),current_laser_scan_data, 12000/res, 9000/res , res ); else updateOccupancyGrid( x(current_scan_index+1), y(current_scan_index+1), psi(current_scan_index+1),current_laser_scan_data ); end

%eval(['scan' num2str(current_scan_index) '= current_laser_scan_data;']); %pause(.0125);end

The following function updates the local grid from current laser scans.

function occupancy_grid = updateOccupancyGrid( x, y, psi, current_laser_scan_data, new_x, new_y , res )% This function takes in a single current scan and adds the information to% a occupancy grid.% -------------------------------------------------------------------------% Inputs:% x = current x position of laser scanner% y = current y position of laser scanner% psi = current heading of the laser scanner% scan_data = the current scan ranges returned by the laser scanner% new_x = if a new grid is created then this will be the new x% dimension of the occupancy grid% new_y = if a new grid is created then this will be the new y% dimension of the occupancy grid% -------------------------------------------------------------------------persistent grid;persistent resolution;

if (nargin == 7) grid = ones( new_x, new_y); resolution = res;endif (nargin == 4 || nargin == 7) % Rotation matrix R = [cos(psi) -sin(psi); sin(psi) cos(psi)];

% Rotate the data points current_laser_scan_data(:,1:2) = ((R^-1)*(current_laser_scan_data(:,1:2))')'; % Translate by cortex data current_laser_scan_data(:,1) = current_laser_scan_data(:,1) + x; current_laser_scan_data(:,2) = current_laser_scan_data(:,2) - y; current_laser_scan_data = floor( current_laser_scan_data./resolution ); for i = 1:length(current_laser_scan_data)

grid(current_laser_scan_data(i,1),current_laser_scan_data(i,2))=0; end imshow(grid, 'InitialMagnification' , 'FIT'); % plot(current_laser_scan_data(:,1),current_laser_scan_data(:,2),'.'); pause(.0125); else error('Wrong number of input arguments.');endoccupancy_grid = grid;end

This code works well and when it is converted to C code, it should run fast enough to be run in

real time on the aircraft hardware.

Quad-Tree Data Structures

Sam began by researching efficient methods to store the laser scan data in a quad tree

structure. (Sam put all of your information here and update all figure numbers below. Your figures

should start at number 8. Also, when you are done go to the table of contents page at the beginning,

click on it and let it auto update.)

Height Estimation

After the completion of previous sections of out project, Daniel and Paul were assigned to work

on the project to determine the height for the aircraft to fly. We were given instructions to make sure

that in flight that the aircraft never had a significant jump or drop in altitude. We decided that in order

to accomplish this we would create an algorithm that could average the heights that were previously

returned, and by using a dampening coefficient we could control the speed at which the aircraft could

change in altitude.

We created the following code that would read in data from the cortex room and use the data

returned from the laser range finder to determine the distance that the aircraft actually was from an

object below.

%%Height Estimation Using Cortex Data%Using data from the cortex room and the Laser range finder with mounted mirrors,%we are able to calculate the distance above the floor that the air craft%is flying. This algorithm averages the laser scans 23-30 after the first%45 scans were removed since they have been defaulted to return -1. The%variable "flying_height_above_ground" can be adjusted to determine how%hight one desires that the aircraft flies above the ground. Also the%variable "damping" can be adjusted to a larger number to make the aircraft%more responsive to height changes.

%these variables are the same as the build_world_map_from_cortex.m these%files may be combined to run simulateous one daycortex = importdata('cortexdata.txt'); % Import Cortex position Datapsi = cortex.data; % Extract psix = 1000.*str2double(cortex.textdata(:,2)); % Extract x from cell array convert to mmy = 1000.*str2double(cortex.textdata(:,4)); % Extract y from cell array convert to mmz = 1000.*str2double(cortex.textdata(:,6)); % Extract z from cell array convert to mmbegin_good_data = 45; % number of scans that return -1 at the beginning of each scan filenumAvg = 45; % number of previous data points to average for hexakopter heightdamping = .05; % how damped you want the system to beflying_height_above_ground = 1000; % Determine height flighing distance above obstacles

figure(4) %figure for visualization of heightclfhold on;

for scan_index = 0:length(psi)-1 % Build the name of the file to be used as a variable index_num = num2str(scan_index); % Convert index val to string file_name = strcat('scan',index_num,'.txt'); % Concatonate the file number to the string name

file_var = genvarname(['scan' num2str(scan_index)]); % Create a variable to store the imported file into

eval([file_var ' = importdata(file_name)']); % Import the data into the varible from its file eval([file_var ' = ' file_var '(begin_good_data:end)' ]); % remove bad points at beginning of each input data file the 45 -1 values at the beginning of each scan file currentHeight = getCurrentHeight(eval(file_var)); %assign the current height from the laser scan data by averageing the best scans from the mirror reflection %in order to plot the data from the scanner we needed to invert it. %This part of the code adds a set amount to the ground to set it to %zero if scan_index == 0; offset = currentHeight; end setHeight = flying_height_above_ground+(-1*currentHeight+offset); %tell the flying aircraft the altitude that it should be at after flying over an obstacle %calculates vehicle height based on a set number of previous heights % this is done by averaging the height of the flying aircraft over the % last hexHeight = getHeightAverage(setHeight, scan_index , numAvg); %this block of code moves the hexicopter height to the setHeight it should %be flying at by the last "numAvg" scans if hexHeight<setHeight; hexHeight = hexHeight+damping*abs((setHeight-hexHeight)); end if hexHeight>setHeight; hexHeight = hexHeight-damping*abs((setHeight-hexHeight)); end plot(scan_index+1,(-1)*currentHeight+offset,'*b') plot(scan_index+1, hexHeight, 'r^') plot(scan_index+1, setHeight, 'k.') pause(.01)end

hold off;xlabel('distance traveled');ylabel('height');legend('ground', 'hexicopter height', 'desired location');axis([0 140 -50 2000]);

We created the following functions to calculate the current distance that the aircraft was flying

above objects below.

function myHeight = getCurrentHeight(scan) firstHeight = 23; % value that I determined is fairly robust to accept as the first point to measure for height. First real point is 21 but this is a little further into the mirror lastHeight = 30; % this value is also on the mirror, althought the real last value is 33, I decided to use an earlier scan for robustness

height = scan; %import a scan file to be used to calculate the height thisHeight = 0; for j = firstHeight : lastHeight % Average the heights thisHeight = thisHeight + height(j)/(lastHeight-firstHeight + 1); % by adding them all together and dividing each individual sum by the number of terms end myHeight = thisHeight;end

We created the following functions to calculate the average height that the aircraft had been

flying.

function averageHeight = getHeightAverage(currentHeight, scan_index, number_scans_to_average)

persistent heights_to_ave; %if this is the first scan of the flight, then we need to reinitialize %the flying height. if(scan_index == 0) heights_to_ave = []; end

%for the first scans, until we achieve the number we are trying to average % just average the heights that have been returned, %else, add a new scan, remove the oldest scan and average all the % values that have been returned within the specified number to average if(scan_index < number_scans_to_average) heights_to_ave = [heights_to_ave currentHeight]; averageHeight = sum(heights_to_ave)/length(heights_to_ave); else heights_to_ave = [heights_to_ave(2:number_scans_to_average) currentHeight]; averageHeight = sum(heights_to_ave)/number_scans_to_average; end end

The output of this top-level code as well the functions gave us the height estimation that we

were looking to achieve. The output is shown in Figure 3. After we determined that this code worked

we decided to simulate flying up and down staircases as shown in Figures 4 and 5

Figure 3. Height Estimation using Laser Range Finder

Figure 4. Simulation of Flying Up a Staircase

Figure 5. Simulation of Flying Down a Staircase

Function Specification Document

DescriptionWe have a laser range finder. Our project requires us to create a 3D global map using data retrieved

from the laser range finder. MIT has developed the technology to build map 2D contour maps. Our

project will require us to take this technology and improve it to include the height of the HexaKopter

and create a 3-dimensional map.

Customer Needs1) Build a contour map

2) Using a probabilistic model determine the location of the flying aircraft

3) Build a global map using a series of local contour maps

4) Refine position as predicted using aircraft orientation

Our customers have explained to us the need for software that can run in real time to build contours

and create a global map from a laser range finder mounted to a flying aircraft.

Weight C++ MatLAB Java

Ease of Implementation 5 2 4 3

Accuracy of Measurement 3 5 5 5

Processing Speed 2 6 3 3

Totals: 10 37 41 36

Based on the result of our table of values we have decided to begin our project by creating an initial

implementation of our costumers needs in MatLAB. We feel like we have the greatest grasp on this

programming language and will be able to most quickly create the results required by this project.

Linking Project Specifications and Project Needs

Ease of ImplementationWe can write the necessary code in less than 10 hours of work a week from each of the team members.

The project implementation of building contours needs to be understandable and reachable in the

MatLAB coding language by March 1st, 2011. We will then focus our time to building 3-dimensional maps

rather than creating boundaries and contours from the laser range finder.

Accuracy of MeasurementWe have been given the necessary measurement of a one cm of error per scan. We will need to use

probability measurements to accurately predict our location and change in orientation using the laser

data and our movement.

Processing SpeedWe will need to be able to build a contour map and add it to our global map in 100ms as the laser range

scanner takes 10 scans per second. So as to be able to create a map in real time we will need to build

maps in real time.

Concept Generation and Selection

Introduction and Process DescriptionThe creation of this document was provided by the necessary goals as set forth by our customer and the

deadlines for each of the parts that they require of us to create. We broke up each of their milestones

into incremental steps that we believe we will be able to accomplish in the given time frame.

Body of Facts and AssumptionsWe have a laser range finder that will output information regarding the distance and bearing to the

nearest obstacle. This measurement is given in polar coordinates. We are basing our design off the MIT

Bachrach thesis. We assume this design to be functional and understandable to a point that we can

implement it into our 3-dimensional contour map builds. We will be required to implement this design

using MATLAB and/or C++. We assume that we will be able to write this code to be to function and

process the information in the given time frame.

Block Diagram of Proposed Solution

Information

Data Out

Our customers have requested us to use this implementation and as such an alternative architecture will

not be acceptable.

Design CriteriaRead in output data from the laser—

Since the HexaKopter cannot see, the only means it has to know where obstacles lie is through

the input of this external device.

Convert the data into Cartesian coordinates from polar—

Heap/Tree data

structure

Line

segments

Contour

Extraction

Log-Likelihood MapGlobal

map

HexaKopter

Location

Later on in this project a global map will be required. Our customer has specified that they

would like the output to be a 3-dimensional diagram that includes the features of a room. We along

with the customer feel the best implementation of this design is with a Cartesian map.

Connect data points with each nearest neighbor to create line segments—

Connection of individual data points will create the useable line segments to later form a map.

Create contours that represent surfaces and slopes—

The use of contours is necessary as hundreds of data points are input from each laser scan.

These individual data points after connected represent surfaces and features of the room. The

connection of the line segments to a contour can make the process efficient enough to run at the

required rate as a contour describe hundreds of points with a single line by two points and a slope.

Create a Log-Likelihood map that includes contour segments—

The Log-Likelihood map is a map that we use to account for the error in the laser reading caused

by bobbing of the laser range finder and noise in the readings. We will do this by sliding a Gaussian along

each of the contours.

Use Log-Likelihood map to refine HexaKopter’s location—

This will show the HexaKopter where it is able to fly without bumping into obstacles. This will

also be used to refine the HexaKopter’s pose.

Use multiple scans to create a 3-dimensional local map—

The vehicle will be flying through a 3D world. It needs to understand where obstacles lie and the

probabilities of each of the features of the wall being at the point where they have been measured.

Integrate the local map into a 3-dimensional global map—

The robot needs to know where it has been and be able to know if it has already been in a

particular location.

AlternativesWe have the option to code up the implementation of the laser range finder using any code we like. The

different coding languages we know all have different advantages and disadvantages. In order to decide

which coding language we should use, we created a decision matrix.

Weight C++ MatLAB Java

Ease of Implementation 5 2 4 3

Accuracy of Measurement 3 5 5 5

Processing Speed 2 6 3 4

Totals: 10 37 41 38

Summary and SelectionEase of Implementation – We chose to give this half of the weight because of the limited time frame

that we have to implement our code. Our time would be better spent coding in a language that we

know and getting a functional product than learning a new programming language. A majority of our

group felt the most comfortable in MATLAB, then Java, and then C++ so that is why MATLAB has the

biggest weight, followed by Java, and then C++.

Accuracy of Measurement – We thought this should have a large percent of the weight because an

inaccurate measurement would make all of our data and computation meaningless. We felt that all

three languages would provide accurate results assuming our implementation method and coding were

correct. All three languages received an equal weight of 5.

Processing Speed – We decided to give processing speed the lowest weight because we can always port

code over to a faster version if we get a working copy. There are also ways to improve processing speed

inside each individual language. C++ is the fastest language so it received a 6, Java is slower getting a 4,

and MATLAB is the slowest receiving a 3.

ResultThe majority of our project specifications have been pre-determined by our customer. Our main goal is

to be able to implement their design to their criteria by the deadlines they have stated on the

milestones website. Our decision matrix showed us that we should use MATLAB to implement our code

because its score was 3 points higher than Java, which came in second.