LIDAR Odometry with ICP

Posted on July 4, 2019 by 1988kramer

This post is the second in a series of tutorials on SLAM using scanning 2D LIDAR and wheel odometry. The other posts in the series can be found in the links below. The links will be updated as work on the series progresses.

Intro To LIDAR SLAM and the IRC Dataset
Matching Scans To A Persistent Map
SLAM Using LIDAR And Wheel Odometry
Globally Consistent SLAM With LIDAR

In the previous post I introduced the Intel Research Center (IRC) Dataset and we used it in some basic visualizations. In particular, I used the robot’s odometry to get a rough estimate of its pose over time by simply concatenating the relative transformations between timesteps, $T_i$ $0 \leq i \leq n$ :

$\begin{equation*} T_n = T_1 T_2 \dots T_n \end{equation*}$

I then used the transform $T_n$ to project the laser scan at each timestep into the global coordinate frame: $p_g = T_n p_l$ where $p_g$ is the set of homogeneous scan points in the global frame, $p_l$ is the set of homogeneous points in the robot’s local frame, and $T_n$ is the estimated transform between the global and local coordinate frames. This process is visualized in VisualizeMeasurements.py in my github repo:

Visualization of the odometry and lidar measurements together.

Watching this visualization even over a short time, it’s obvious that the robot’s odometry is very noisy and collects drift very quickly. With perfect odometry, the objects measured by the LIDAR would stay static as the robot moves past them. This is clearly not the case. Interestingly, the odometry seems to be fairly reliable for translational motion, but it drifts quickly in rotation. This will be significant later.

How Do We Fix The Drift Problem?

It’s clear to us the robot’s wheel odometry isn’t sufficient to estimate its motion. We know this because we can overlay the robot’s LIDAR scans in our minds and get a sense of how the robot’s estimated motion is deviating from its true motion. If we can do this in our minds, could we tell the robot how to do it? Can the robot use its LIDAR scans to estimate its own motion?

Hopefully you’ve guessed the answer is yes, through a process called scan matching. Basically the goal is to take a new scan from the robot’s LIDAR and find the transformation that best aligns the new scan with either previous scans or some sort of abstracted map. There are many ways to implement this idea and for this tutorial I’m going to demonstrate the simplest method: using the Iterative Closest Point (ICP) algorithm to align the newest LIDAR scan with the previous scan.

Iterative Closest Point In Pictures

The ICP algorithm involves 3 steps: association, transformation, and error evaluation. These are repeated until the scans are aligned satisfactorily. I’ll first demonstrate the process pictorially with an example from the IRL dataset and delve into the math below. Let’s first imagine we want to find the transformation that aligns the two scans pictured below. The previous scan, referred to as the target, is in cyan while the new scan, also called the source is in magenta. The goal is to find the rigid transformation (rotation and translation) that best aligns the source to the target.

Our first step in estimating this transformation is to decide which points in the source scan correspond to the same physical features as points in the target scan. The simplest way to do this is through a nearest neighbor search: points in the source scan are associated to the nearest point in the target scan. In the image below I’ve found the nearest neighbors of each point in the target scan. Associated points are connected with blue lines:

We can immediately see some mistakes in the nearest neighbor search, but in general the associations pictured will pull the source points in the right direction. The next step in the process is transformation. We find the transformation that, when applied to the source points, minimizes the mean-squared distance between the associated points: $\tilde{T}=\underset{T}{\text{argmin}}\frac{1}{N}\sum_i^N \|t_i - Ts_i\|^2$ where $\tilde{T}$ is the final estimated transform and $t_i$ and $s_i$ are target points and source points, respectively. The result of this estimation is pictured below:

After this we evaluate the error in the alignment as $e=\frac{1}{N}\sum_i^N \|t_i - Ts_i\|^2$ and decide if we need to repeat the above process. In this case our scans still aren’t aligned very well, so we redo the associations with the transformed source points and repeat the process.

Nearest neighbor associations for the transformed source points

After five iterations our scans the algorithm finds a pretty good alignment:

Scan alignment after the fifth iteration

The Math Details

ICP is actually pretty straightforward, mathematically. The association step is pretty simple. It can just be a brute-force search for the nearest pairs of points between the source and target scans. If you’re working with large scans though, it’s a good idea to use KD Trees for this step.

The real trick to ICP is in the transformation step. Basically, we find the covariance between the two point sets, the matrix $M$ .

$\begin{equation*} M = PQ^T \end{equation*}$

Where $P$ is a matrix whose $i^\text{th}$ column is $s_i - \mu_s$ or the $i^\text{th}$ source point expressed relative to the centroid of the source point set $\mu_s$ . Similarly, $Q$ is a matrix whose $i^\text{th}$ column is $t_i - \mu_t$ . Once we have the covariance matrix $M$ , we find the rotation $R$ between the two point clouds using singular value decomposition:

$\begin{align*} M &= UWV^T \\ R &= VU^T \end{align*}$

If you’re wondering how you break the $M$ matrix down into $U$ , $W$ , and $V^T$ , know that most linear algebra packages (including matlab and numpy) have functions for SVD. That’s about as far as you need to get into it. If you’re interested though, the wikipedia page has some good details.

We’ve found the rotation $R$ between the point sets, now we just need the translation $t$ . Luckily it’s pretty simple, just the difference between the centroids of the point clouds $t = \mu_t - \mu_s$ .

Once we have our translation and rotation we evaluate the alignment error as $e=\frac{1}{N}\sum_i^N \|t_i - Rs_i + t\|^2$ . We use this to determine if we should quit or iterate again. Normally we stop the process if the error at the current step is below a threshold, if the difference between the error at the current step and the previous step’s error is below a threshold, or if we’ve reached a maximum number of iterations.

Below is a visualization of a simple ICP motion estimation algorithm. It simply aligns the newest scan to the previous scan to find the motion of the robot between scans:

Demonstration of motion estimation using ICP

Note that this method for motion estimation works pretty well sometimes. If the robot is near large sections of wall at different angles it can estimate its transformation between scans pretty reliably. This is because it has good environmental queues to its motion in all directions. On the other hand, if the robot is in a mostly straight hallway, there’s really nothing in its measurements that will tell it how it’s moving along the hallway. One alignment is as good as any other as long as the walls line up. In these cases the robot’s estimates of its translation are very poor. Similarly, if there just aren’t a lot of unique, persistent features in the scan, which happens sometimes when the robot approaches corners, there aren’t any good cues for the robot to estimate its rotation.

So, matching successive LIDAR scans via the iterative closest point algorithm can give our robot some information about its own movement. However, this on its own is not enough to provide a reliable motion estimate. Luckily, our robot has wheel odometry in addition to LIDAR. Even luckier, in fact, ICP is pretty reliable at estimating rotations but poor with translation in some cases. The wheel odometry, on the other hand, gives us very accurate translation but it is very unreliable with rotation. Next time, we’ll experiment with fusing information from these two sensors to create a more reliable motion estimate.

The Code

Below you can see an implementation of the ICP algorithm in python. You can find the full class, Align2D.py, in my github repo as well as a demonstration of its use in VisualizeICP.py.

# uses the iterative closest point algorithm to find the
# transformation between the source and target point clouds
# that minimizes the sum of squared errors between nearest 
# neighbors in the two point clouds
# params:
#   max_iter: int, max number of iterations
#   min_delta_err: float, minimum change in alignment error
def AlignICP(self, max_iter, min_delta_err):

	mean_sq_error = 1.0e6 # initialize error as large number
	delta_err = 1.0e6    # change in error (used in stopping condition)
	T = self.init_T
	num_iter = 0         # number of iterations
	tf_source = self.source

	while delta_err &gt; min_delta_err and num_iter &lt; max_iter:

		# find correspondences via nearest-neighbor search
		matched_trg_pts,matched_src_pts,indices = self.FindCorrespondences(tf_source)

		# find alingment between source and corresponding target points via SVD
		# note: svd step doesn't use homogeneous points
		new_T = self.AlignSVD(matched_src_pts, matched_trg_pts)

		# update transformation between point sets
		T = np.dot(T,new_T)

		# apply transformation to the source points
		tf_source = np.dot(self.source,T.T)

		# find mean squared error between transformed source points and target points
		new_err = 0
		for i in range(len(indices)):
			if indices[i] != -1:
				diff = tf_source[i,:2] - self.target[indices[i],:2]
				new_err += np.dot(diff,diff.T)

		new_err /= float(len(matched_trg_pts))

		# update error and calculate delta error
		delta_err = abs(mean_sq_error - new_err)
		mean_sq_error = new_err

		num_iter += 1

	return T

# uses singular value decomposition to find the 
# transformation from the target to the source point cloud
# assumes source and target point clounds are ordered such that 
# corresponding points are at the same indices in each array
#
# params:
#   source: numpy array representing source pointcloud
#   target: numpy array representing target pointcloud
# returns:
#   T: transformation between the two point clouds
def AlignSVD(self, source, target):

	# first find the centroids of both point clouds
	src_centroid = self.GetCentroid(source)
	trg_centroid = self.GetCentroid(target)

	# get the point clouds in reference to their centroids
	source_centered = source - src_centroid
	target_centered = target - trg_centroid

	# get cross covariance matrix M
	M = np.dot(source_centered.T,target_centered)

	# get singular value decomposition of the cross covariance matrix
	U,W,V_t = np.linalg.svd(M)

	# get rotation between the two point clouds
	R = np.dot(V_t.T,U.T)

	# get the translation (simply the difference between the point clound centroids)
	t = trg_centroid - src_centroid

	# assemble translation and rotation into a transformation matrix
	T = np.identity(3)
	T[:2,2] = np.squeeze(t)
	T[:2,:2] = R

	return T

def GetCentroid(self, points):
	point_sum = np.sum(points,axis=0)
	return point_sum / float(len(points))

Intro To LIDAR SLAM

Posted on July 4, 2019 by 1988kramer

I’m two years in to my PhD in robotics and things are going well. I’m working on robotic perception at the NASA Jet Propulsion Laboratory over the summer and I recently had a paper accepted to the conference on Field and Service Robotics. There’s just one problem: I still haven’t won the bet that led me to return to grad school in the first place; I haven’t built a robotic system for autonomous indoor mapping. Today we’ll take a big step toward that goal by starting our development of a 2D LIDAR-based SLAM system.

In this post I’ll propose a strategy for solving the localization and mapping portion of my problem. I’ll also introduce the Intel Research Center (IRC) dataset as a means for evaluating my SLAM efforts. I’ll demonstrate my developments first on the IRC dataset, but the eventual goal will be to use them on a real robot.

The Strategy

The goal of this series is to develop LIDAR-based 2 dimensional SLAM. Of course, numerous open source packages already exist for LIDAR SLAM but, as always, my goal is to understand SLAM on a fundamental level. That’s why I’m building everything from scratch and taking a detailed look at the underlying math.

I start by introducing the dataset below. In subsequent posts I present tutorials on basic methods for LIDAR odometry via matching between LIDAR scans and then matching scans to a persistent map representation. I then develop a method for LIDAR SLAM that fuses information from scan matching and wheel odometry. Lastly I present a full SLAM method which estimates both a globally-consistent pose graph and map. This last step will require all the previously presented methods as well as methods we haven’t discussed such as place recognition and loop closure. Links to all the posts in the series can be found below (these will be populated incrementally as work on this project progresses):

Simple LIDAR Odometry Via Scan Matching
Matching Scans To A Persistent Map
SLAM Using LIDAR And Wheel Odometry
Globally Consistent SLAM With LIDAR

Why LIDAR?

LIDAR is an interesting and versatile sensor. In many ways 2D LIDAR measurements are very similar to the measurements we used in the UTIAS dataset in my EKF SLAM tutorial. As with the UTIAS dataset, the measurement model is simply the range and bearing to the measured landmark or obstacle. No complicated projection or distortion models are required to extract this information, so LIDAR is a pretty gentle introduction to the use of raw sensor data.

Unlike the UTIAS dataset, however, our sensor measurements are not associated with particular landmarks in the environment. Instead a LIDAR measurement is associated with whatever surface happened to get in the way of the laser. This makes data association more difficult. Luckily LIDAR makes up for this ambiguity by giving us much more information per timestep. At a given timestep in the UTIAS dataset we get just a few range and bearing measurements to nearby landmarks. With LIDAR, on the other hand, we get range and bearing measurements to everything in the robot’s vicinity. So instead of just estimating the locations of discrete landmarks we can estimate a continuous map of the whole environment. This is exactly what we need for an indoor mapping robot.

A SICK LMS sensor, similar to the one used to create the IRC Dataset.

Lastly, LIDAR sensors have a (mostly deserved) reputation for being extremely expensive and therefore out of reach for the robotics hobbyist. For instance, the 2D scanning LIDAR used to create the IRC dataset costs thousands of US dollars. How many basement tinkerers have that kind of money to spend on their hobby?

rplidar A3, an inexpensive scanning 2D lidar sensor

Luckily for us, cheaper LIDAR sensors, such as the Rplidar A3, have recently been introduced. These have lower range and lower precision than their more expensive cousins. They are totally sufficient for hobby projects though, and their cost is in the hundreds of dollars rather than the thousands.

Of course, publicly available datasets like the IRC dataset make it possible to do robotics work without spending any money. So let’s start there!

The IRC Dataset

The IRC dataset is an indoor localization and mapping dataset recorded at the Intel Research Center in Seattle in 2003. It is available on the MPRT website but I’d recommend getting it from my github repo instead. The raw data is the same in either case, but my repo has a few helpful scripts for loading, aligning, and visualizing the data. The original dataset has some ambiguities. For instance, it doesn’t specify the angular spacing between the lidar beams or the transformation between the odometry coordinate frame and the lidar coordinate frame. My DataLoader.py script resolves these problems for you!

The DataLoader reads the dataset from the txt files. It converts the odometry measurements into transformation matrices. It also converts the raw LIDAR measurements into cartesian points in the robot’s coordinate frame. Finally, it packages these measurements in Measurement objects. This is a much easier way to interact with the data than reading the txt files line by line.

You can see the LIDAR data in the robot’s frame of reference by running my script, VisializeLaser.py. A short bit of that script’s visualization is shown below:

Raw LIDAR measurements shown in the robot’s frame of reference.

Also, to make sure I got the sensor coordinate frame transformation correct, I made VisualizeMeasurements.py. It plots the pose of the robot in the global coordinate frame calculated by integrating the odometry measurements. We can also see the LIDAR measurements transformed into the global frame. As is shown below, the robot’s moves while the LIDAR measurements stay (more or less) static. This indicates we have the robot-to-LIDAR transformation correct. The visualization also shows the LIDAR measurements don’t stay completely static. This indicates the odometry is pretty noisy and accumulates drift quickly. We also notice the odometry noise is biased pretty heavily in rotation. This will be significant when we try to use it later.

Beyond EKF SLAM

Posted on February 3, 2019 by 1988kramer

My series of tutorials on SLAM with the extended Kalman filter is finally done. At this point it’s a good idea to reflect on where we’re trying to go and what motivates us. My goal is to make a mobile robot that can autonomously map indoor spaces. I’m doing this mainly because it’s fascinating, but also to win a bet. Learning state estimation with EKFs was a great step toward that goal; it gave us an initial introduction to probabilistic state estimation. But we have a long way to go. We need to develop a SLAM system that’s more robust and better suited to our problem.

Mapping Methods

My eventual goal is to develop a SLAM system capable of accurately mapping an indoor environment. This gives us a couple of requirements. First, its map representation must be human readable and metrically accurate. A sparse collection of point landmarks, like we used in our EKF, is not sufficient for this. Some sort of dense map representation like an occupancy grid or signed distance function is required.

Example of an occupancy grid map build with RTAB-map. Black cells indicate occupied areas, white cells indicate unoccupied, and gray cells indicate uncertain areas.

Sensor Selection

Second, we need to select sensors suited to the task. Our sensors must be able to give us a metrically accurate picture of the local environment’s structure, not just the locations of discrete landmarks as in the UTIAS dataset. A 2D scanning LIDAR is ideal for taking dense measurements of the surrounding environment and they can be found relatively cheaply. Wheel encoders will also provide useful information on the robot’s motion between LIDAR measurements. Also, because we’re trying to build a globally consistent map, we need a topological understanding of the whole environment. That is, we need to understand how different areas in the environment are connected. This means we must be able to detect when the robot revisits a previously-visited location. This can be done using LIDAR data alone, but cameras using visual bag-of-words or deep place recognition could also be helpful here.

Visual depiction of loop closure using ORB-SLAM. Map on left is prior to loop closure with place association indicated by blue line. Map on right is after loop closure and reoptimization of the map and robot trajectory.

SLAM Formulation

Lastly, we need an estimation framework that can solve our problem quickly and efficiently. We want to estimate a global map of the environment and the robot’s full path through the environment. Filtering approaches are ill-suited to this task because they do not allow for re-estimation of previous poses. This makes it difficult to constrain drift in the robot’s state estimate. One would normally use loop closures to do this, but this is difficult with filtering since ideally the full map and robot trajectory should be re-estimated when a loop closure is detected. That is why we will be using a factor graph approach. A factor graph is a representation of the SLAM problem that visualizes the quantities to be estimated (the robot trajectory, map, etc) as nodes and sensor measurements as constraints between those nodes. The graph model can be translated into a single cost function that can be solved through nonlinear optimization. Best of all, this framework allows for the use of an arbitrary combination of sensors as long as we’re able to correctly incorporate the sensor models into the cost function.

SLAM factor graph example: Blue circles denote robot poses at consecutive time steps (x1, x2 , . . .), green circles denote landmark positions (l1 , l2 , . . .), red circle denotes the variable associated with the intrinsic calibration parameters (K). Factors are shown as black squares: the label “u” marks factors corresponding to odometry constraints, “v” marks factors corresponding to camera observations, “c” denotes loop closures, and “p” denotes prior factors. Taken from Cadena et al, 2016.

Moving Forward

So the plan is to design a graph SLAM system for indoor mapping that uses 2D scanning LIDAR and wheel encoders. It should be able to accurately map and estimate its position within its local environment, as well as perform reliable place recognition and loop closure. It should ideally be able to run in real time on a Raspberry Pi or similar single board computer, so careful attention must be paid to the sparsity of the problem’s factor graph. The graph should be as sparse as possible for efficient optimization. This means I will need to include mechanisms to avoid adding redundant information and to sparsify the problem as it proceeds.

I will first be testing my software on a publicly available dataset. The IRC dataset includes sensor streams from odometry and a 2D scanning LIDAR on a wheeled mobile robot as it traverses the Intel Research Center. It will allow us to test all the requirements on our algorithm including accurate mapping and place recognition. After we’re getting good results on the IRC dataset we can implement it on Colin!

Bot Blog

building my robot army one sketch at a time

Tag Archives: LIDAR