# SLAM

## **Design of Autonomous Systems**
### csci 6907/4907-Section 86
### Prof. **Sibin Mohan**

---

consider a Roomba → navigate inside a room

---

consider a Roomba → navigate inside a room

- **randomly** move around the room
- **map** out room → ensure it covers the entire room

---

consider a Roomba → navigate inside a room

- **randomly** move around the room
- **map** out room → ensure it covers the entire room

---

**map** out room → ensure it covers the entire room

---

**map** out room → ensure it covers the entire room

effective when room has → **obstacles** in it

_e.g.,_ a couch and other furniture

---

requires the robot to do,

---

requires the robot to do,

|||
|:---:|:---:|
|create a **map** of its surroundings | |
| <img src="img/slam/mapping.png"> ||

---

requires the robot to do,

|||
|:---:|:---:|
|create a **map** of its surroundings | **locate** itself within that map|
| <img src="img/slam/mapping.png"> |<img src="img/slam/truck.png"> |
||

---

### enter **simultaneous localization and mapping** (SLAM)

---

## SLAM

fundamental challenges in mobile robotics

---

## SLAM

solving **two** interconnected problems at once

---

## SLAM

solving **two** interconnected problems at once

1. building **map** → **unknown environment** while

---

## SLAM

solving **two** interconnected problems at once

1. building **map** → **unknown environment** while
2. **simultaneously tracking** → robot's position in that environment

Note:
The need for SLAM arises from a seemingly paradoxical situation: to build an accurate map, a robot needs to know its precise location, but to determine its precise location, the robot needs an accurate map. This chicken-and-egg problem is what makes SLAM challenging and fascinating.

---

SLAM → external positioning systems (GPS) are unavailable/unreliable

---

### examples

- indoor environments
- underground mines and caves
- underwater exploration
- planetary exploration
- dense urban areas with GPS signal blockage.

Note:
SLAM is more of a concept than a single algorithm. There are many approaches to implementing SLAM, varying in complexity, accuracy and computational requirements.

---

some examples of SLAM output

---

1. **grid maps** or **scans**

---

1. **grid maps** or **scans**

<br>

---

2. **landmark**-based

---

2. **landmark**-based

<br>

---

another **classification** → type of sensor (and corresponding outputs)

---

a. **visual slam** → cameras

---

a. **visual slam** → cameras

- sparse methods → match image features [PTAM, ORB_SLAM]

---

a. **visual slam** → cameras

- sparse methods → match image features [PTAM, ORB_SLAM]
- dense methods → image brightness [DTAM, LSD- SLAM, DSO, SVO]

---

a. **visual slam** → cameras

- sparse methods → match image features [PTAM, ORB_SLAM]
- dense methods → image brightness [DTAM, LSD- SLAM, DSO, SVO]

---

b. **LiDAR SLAM**

---

b. **LiDAR SLAM**

- laser point cloud → high-precision distance measurements

---

b. **LiDAR SLAM**

- laser point cloud → high-precision distance measurements
- not as finely detailed as camera images

---

b. **LiDAR SLAM**

- laser point cloud → high-precision distance measurements
- not as finely detailed as camera images
- environments with fewer obstacles → less precision

---

b. **LiDAR SLAM**

- laser point cloud → high-precision distance measurements
- not as finely detailed as camera images
- environments with fewer obstacles → less precision
- may require fusion with other sensors (e.g., GPS, odometry)

---

b. **LiDAR SLAM**

<br>

---

### SLAM Problem Definition

---

### SLAM Problem Definition

SLAM → a **state estimation problem**

---

### SLAM Problem Definition

SLAM → a **state estimation problem**

(hint, hint)

---

_given,_

---

_given,_

- robot's **control inputs** (odometry)

---

_given,_

- robot's **control inputs** (odometry)
- observations of **nearby features/landmarks**

---

_given,_

- robot's **control inputs** (odometry)
- observations of **nearby features/landmarks**
- no prior map

---

<div>

_given,_

- robot's **control inputs** (odometry)
- observations of **nearby features/landmarks**
- no prior map

</div>

<div>

_objective_ → estimate,

</div>
</div>

---

<div>

_given,_

- robot's **control inputs** (odometry)
- observations of **nearby features/landmarks**
- no prior map

</div>

<div>

_objective_ → estimate,

- the **map** of the environment

</div>
</div>

---

<div>

_given,_

- robot's **control inputs** (odometry)
- observations of **nearby features/landmarks**
- no prior map

</div>

<div>

_objective_ → estimate,

- the **map** of the environment
- the **path**/current position

</div>
</div>

---

so why is SLAM a hard problem?

---

so why is SLAM a hard problem?

real world → mapping between observations and landmarks is **unknown**

---

so why is SLAM a hard problem?

real world → mapping between observations and landmarks is **unknown**

picking **wrong associations** → **catastrophic consequences**

---

### pose

---

### pose

> the **position** and **orientation** of an object or a robot in a given space

---

### pose

**example** → $2D$ Pose (in planar systems)

- position: (x, y)

---

### pose

**example** → $2D$ Pose (in planar systems)

- position: (x, y)
- orientation: $\theta$ (rotation around the z-axis)

---

### various parts of SLAM

---

### various parts of SLAM

---

### various parts of SLAM

<div>

<br>

</div>

<div>

**Sensor Data**
    
- input to SLAM, _e.g.,_ 
    - camera <br>
    (visual SLAM, extracting features from images)
	- LiDAR/depth sensor <br>
    (distance, obstacle)

</div>
</div>

---

### various parts of SLAM

<div>

<br>

</div>

<div>

**Sensor Data**
    
- input to SLAM, _e.g.,_ 
    - camera <br>
    (visual SLAM, extracting features from images)
	- LiDAR/depth sensor <br>
    (distance, obstacle)

</div>
</div>

capture **raw data** of environment

---

### various parts of SLAM

<div>

</div>

<div>

**Front End** (sensor processing)

- process sensor information 
- extract **meaningful** features
    - motion estimation
    - obstacle location

</div>
</div>

---

### various parts of SLAM

<div>

</div>

<div>

**Front End** (sensor processing)

- process sensor information 
- extract **meaningful** features
    - motion estimation
    - obstacle location

</div>
</div>

goal → **preprocess** sensor data+reduce noise

Note:
    - motion estimation: Determines how the robot is moving by tracking sensor data over time
    - obstacle location estimation: Identifies objects and landmarks in the environment

---

### various parts of SLAM

---

### various parts of SLAM

<div>

<br>

</div>

<div>

**Back End** (sensor-independent)
	
- **global optimization** → estimated trajectory

</div>
</div>

---

### various parts of SLAM

<div>

<br>

</div>

<div>

**Back End** (sensor-independent)
	
- **global optimization** → estimated trajectory
- **sensor-independent**
    - data from different sensors

</div>
</div>

---

### various parts of SLAM

<div>

<br>

</div>

<div>

**Back End** (sensor-independent)
	
- **global optimization** → estimated trajectory
- **sensor-independent**
    - data from different sensors 
- **processed pose+landmark**
    - register pose graphs
	- graph optimization

</div>
</div>

Note:
    - **register pose graphs** → combines different pose estimates into a coherent structure
	- graph optimization → uses mathematical optimization techniques (e.g., bundle adjustment, factor graphs) to refine the trajectory and correct drift

---

### various parts of SLAM

<div>

<br>

</div>

<div>

</div>
</div>

**corrects errors** accumulating over time → **more accurate map**

---

### various parts of SLAM

<br>

there is **feedback** provided to the front end

---

### various parts of SLAM

---

### various parts of SLAM

<div>

<br>

</div>

<div>

**Pose Graph/Map**

- **final output** → **pose graph**
- map constructed → **optimized pose estimates**

</div>
</div>

Note:
- pose graph:  (a representation of the robot’s trajectory) and a map of the environment

---

### various parts of SLAM

<div>

<br>

</div>

<div>

**Pose Graph/Map**

- **final output** → **pose graph**
- map constructed → **optimized pose estimates**

</div>
</div>

map → for navigation, obstacle avoidance, autonomous decision-making

---

| component | description |
|-----------|---|
| front end (sensor-dependent) | processes raw sensor data (motion estimation, obstacle detection) |
| back end (sensor-independent) | optimizes trajectory+map |
| final output | refined pose graph+map for accurate navigation |
||

Note:

This structured approach ensures real-time and accurate localization while reducing computational complexity.

---

distill previous steps,

---

distill previous steps,

| step |description  |
|-------|--------|
| **prediction step**  | update the robot's position estimate using odometry data (motion model) |

---

distill previous steps,

| step |description  |
|-------|--------|
| **prediction step**  | update the robot's position estimate using odometry data (motion model) |
| **landmark extraction** | identify landmarks or features from sensor measurements |

---

distill previous steps,

---

distill previous steps [contd.]

| step |description  |
|-------|--------|
| **update step** | update robot position and landmark estimates based on observations |

---

distill previous steps [contd.]

| step |description  |
|-------|--------|
| **update step** | update robot position and landmark estimates based on observations |
| **map expansion** | add newly observed landmarks to the map |
||

---

there are some challenges that need to be overcome...

---

### challenges

---

### challenges

| step |description  | challenge  |
|-------|--------|--------|
| **prediction step**  | update the robot's position estimate using odometry data (motion model) | **correlated errors** in position estimation affect the map |
| **landmark extraction** | identify landmarks or features from sensor measurements | **environmental dynamics** may cause landmarks to change |

---

### challenges [contd.]

---

### challenges [contd.]

| step |description  | challenge  |
|-------|--------|--------|
| **data association** | match observed landmarks with previously mapped landmarks | **incorrect associations** lead to mapping errors |
| **update step** | update robot position and landmark estimates based on observations | **computational complexity** increases with more data |

---

### challenges [contd.]

| step |description  | challenge  |
|-------|--------|--------|
| **map expansion** | add newly observed landmarks to the map | loop closure* is required to recognize **revisited** places |
||

---

### challenges [contd.]

| step |description  | challenge  |
|-------|--------|--------|
| **map expansion** | add newly observed landmarks to the map | loop closure* is required to recognize **revisited** places |
||

[* recognizing when a robot/vehicle is at **previously visited location**]

---

## Mathematical Formulations

---

let's define some terms,

| symbol   | meaning  |
|----------|-----------|
| $x_{1:t}$ | robot path |
| $m$ | map |
| $z_{1:t}$ | sensor observations |
| $u_{1:t}$ | control inputs|
||

---

**two** types of SLAM:

---

**two** types of SLAM:

1. **full** SLAM

---

**two** types of SLAM:

1. **full** SLAM
2. **online** SLAM

---

### 1. **full** SLAM

estimates the **entire** robot path and map

---

### 1. **full** SLAM

estimates the **entire** robot path and map

---

### 1. **full** SLAM

<div>

</div>

<div>

model estimates.

- **entire trajectory** $x_{1:t}$ 
- map, $m$

</div>
</div>

---

### 1. **full** SLAM

<div>

</div>

<div>

model estimates.

- **entire trajectory** $x_{1:t}$ 
- map, $m$
$$
p(x_{1:t}, m | z_{1:t}, u_{1:t})
$$

</div>
</div>

---

### 1. **full** SLAM

<div>

</div>

<div>

model estimates.

- **entire trajectory**, $x_{1:t}$ 
- map, $m$
$$
p(x_{1:t}, m | z_{1:t}, u_{1:t})
$$ 
- joint probability

</div>
</div>

---

### 1. **full** SLAM

<div>

</div>

<div>

model estimates.

- **entire trajectory**, $x_{1:t}$ 
- map $m$
$$
p(x_{1:t}, m | z_{1:t}, u_{1:t})
$$ 
- joint probability
- all past and current poses

</div>
</div>

---

### 1. **full** SLAM

<div>

</div>

<div>

model estimates.

- **entire trajectory**, $x_{1:t}$ 
- map, $m$
$$
p(x_{1:t}, m | z_{1:t}, u_{1:t})
$$ 
- joint probability
- all past and current poses

</div>
</div>

<br>

**smoothing** → refine past estimates when new observations received

Note:
- gray-shaded area (from the figure) covers all past poses $x_{1:t}$, indicating that they are explicitly maintained in the estimation

---

### 2. **online** SLAM

estimates only **most recent** robot pose and map

---

### 2. **online** SLAM

estimates only **most recent** robot pose and map

---

### 2. **online** SLAM

<div>

</div>

<div>

model estimates,

- only **most recent pose**, $x_t$ 
- map, $m$
</div>
</div>

---

### 2. **online** SLAM

<div>

</div>

<div>

model estimates,

- only **most recent pose**, $x_t$ 
- map, $m$
</div>
</div>

$$
p(x_t, m \mid z_{1:t}, u_{1:t}) = \int\int...\int p(x_{1:t}, m \mid z_{1:t}, u_{1:t}) dx_1 dx_2...dx_{t-1}
$$

---

### 2. **online** SLAM

<div>

</div>

<div>

model estimates,

- only **most recent pose**, $x_t$ 
- map, $m$
- past poses, $x_{1:t-1}$ → marginalized out
</div>
</div>

$$
p(x_t, m \mid z_{1:t}, u_{1:t}) = \int\int...\int p(x_{1:t}, m \mid z_{1:t}, u_{1:t}) dx_1 dx_2...dx_{t-1}
$$

---

### 2. **online** SLAM

<div>

</div>

<div>

model estimates,

- only **most recent pose**, $x_t$ 
- map, $m$
- past poses, $x_{1:t-1}$ → marginalized out
- leaving current pose and map
</div>
</div>

$$
p(x_t, m \mid z_{1:t}, u_{1:t}) = \int\int...\int p(x_{1:t}, m \mid z_{1:t}, u_{1:t}) dx_1 dx_2...dx_{t-1}
$$

---

### 2. **online** SLAM

<div>

</div>

<div>

model estimates,

- only **most recent pose**, $x_t$ 
- map, $m$
- past poses, $x_{1:t-1}$ → marginalized out
- leaving current pose and map
</div>
</div>

**computationally efficient** → suitable for real-time applications

Note:
- gray-shaded area in the graphical model highlights that only the current pose is maintained explicitly

---

### full SLAM vs online SLAM

---

### full SLAM vs online SLAM

| feature | full slam | online slam |
|-----------------|-----------|------------|
| **estimates**   | **full** path, $x_{1:t}$ & map, $m$ | **current** pose, $x_t$ & map, $m$ |
---

### full SLAM vs online SLAM

| feature | full slam | online slam |
|-----------------|-----------|------------|
| **estimates**   | **full** path, $x_{1:t}$ & map, $m$ | **current** pose, $x_t$ & map, $m$ |
| **past poses**  | explicitly maintained | marginalized out |
| **computation** | higher (batch proc) | lower (real-time feasible) |
| **approach**    | smoothing-based <br> (e.g., graph-based slam) | filtering-based<br>  (e.g., ekf-slam, fastslam) |
---

### full SLAM vs online SLAM

---

## SLAM Hardware

---

## SLAM Hardware

main hardare for SLAM consists of,

|||
|:------|:------|
| **mobile** robot||
| range measurement device||
||

---

## SLAM Hardware

main hardare for SLAM consists of,

|||
|:------|:------|
| **mobile** robot| <ul> <li>wheel encoders →odometry</li>  <li>processor → calculations</li> <li>appropriate motion capabilities</li> </ul>|
| range measurement device||
||

---

## SLAM Hardware

main hardare for SLAM consists of,

|||
|:------|:------|
| **mobile** robot| <ul> <li>wheel encoders →odometry</li>  <li>processor → calculations</li> <li>appropriate motion capabilities</li> </ul>|
| range measurement device| laser scanners, sonar, vision (camera) |
||

---

## Landmark Identification

---

## Landmark Identification

> a feature that can be **re-observed** and **distinguished from environment**

---

## Landmark Identification

> a feature that can be **re-observed** and **distinguished from environment**

<br>

- used by robot to find out where it is → localize itself

---

## Landmark Identification

> a feature that can be **re-observed** and **distinguished from environment**

<br>

- used by robot to find out where it is → localize itself
- types of landmarks depend on the environment

---

properties of **good** landmarks

---

properties of **good** landmarks

- easily **re-observable** from different positions and angles

---

properties of **good** landmarks

- easily **re-observable** from different positions and angles
- **distinguishable** from other landmarks

---

properties of **good** landmarks

- easily **re-observable** from different positions and angles
- **distinguishable** from other landmarks
- **stationary** (not moving objects)

---

properties of **good** landmarks

- easily **re-observable** from different positions and angles
- **distinguishable** from other landmarks
- **stationary** (not moving objects)
- **plentiful** in the environment

---

**indoor landmarks**,

- lots of **straight lines**
- well defined corners

---

_common_ methods to find/distinguish **landmarks**:

1. [spikes](#landmarks--spikes)
2. [RANSAC](#ransac-random-sample-consensus)
3. [scan matching](#landmarks--scan-matching)

---

### Landmarks | Spikes

---

### Landmarks | Spikes

detect **significant changes** in range measurements

---

### Landmarks | Spikes

detect **significant changes** in range measurements

indicates **distinctive** features

---

### Landmarks | Spikes

- uses **extrema** to find landmarks

---

### Landmarks | Spikes

- uses **extrema** to find landmarks
    - if two values differ > certain amount [e.g. <scb>0.5</scb>]

---

### Landmarks | Spikes

- uses **extrema** to find landmarks
    - if two values differ > certain amount [e.g. <scb>0.5</scb>]
- detects **big changes**

---

### Landmarks | Spikes

- uses **extrema** to find landmarks
    - if two values differ > certain amount [e.g. <scb>0.5</scb>]
- detects **big changes**
- some beams reflect from walls, others don’t

---

### Landmarks | Spikes

---

### Landmarks | **RANSAC** (Random Sample Consensus)

---

### Landmarks | **RANSAC** (Random Sample Consensus)

extracting **lines** → from laser scans

---

### Landmarks | **RANSAC** (Random Sample Consensus)

extracting **lines** → from laser scans

robust method → useful for detecting **walls** in indoor environments

---

### Landmarks | **RANSAC** (Random Sample Consensus)

robust **iterative** method

---

### Landmarks | **RANSAC** (Random Sample Consensus)

robust **iterative** method

- estimating mathematical model parameters

---

### Landmarks | **RANSAC** (Random Sample Consensus)

robust **iterative** method

- estimating mathematical model parameters 
- from a set of observed data → contains outliers

Note:
Developed by Fischler and Bolles in 1981, it's widely used in computer vision and image processing tasks.

---

### RANSAC | **Insight**

---

### RANSAC | **Insight**

- estimation techniques → fitting model to data containing **only inliers**

---

### RANSAC | **Insight**

- estimation techniques → fitting model to data containing **only inliers** 
- break down → when **outliers** present

---

### RANSAC | **solution**

---

### RANSAC | **solution**

1. **randomly sampling** → **minimum** number of points

Note:
- minimum points required to determine model parameters

---

### RANSAC | **solution**

1. **randomly sampling** → **minimum** number of points 
2. **fitting** a model to those points

---

### RANSAC | **solution**

1. **randomly sampling** → **minimum** number of points 
2. **fitting** a model to those points
3. checking which other data points → **consistent** with model

---

### RANSAC | **solution**

1. **randomly sampling** → **minimum** number of points 
2. **fitting** a model to those points
3. checking which other data points → **consistent** with model
4. keep model → if it has **sufficient support** (enough inliers)

---

### RANSAC | **solution**

---

### RANSAC | **example**

---

### RANSAC | **example**

how many readings lie close to best fit line?

Note:
- could we have created a better line if we picked other points?

---

### RANSAC | **Mathematical Formulation**

---

### RANSAC | **Mathematical Formulation**

for a dataset with,

|||
|----|:-----|
| $n$ | total points |

---

### RANSAC | **Mathematical Formulation**

for a dataset with,

|||
|----|:-----|
| $n$ | total points |
| $\epsilon$ | hypothesized probability of point being **inlier** |

---

### RANSAC | **Mathematical Formulation**

for a dataset with,

|||
|----|:-----|
| $n$ | total points |
| $\epsilon$ | hypothesized probability of point being **inlier** |
| $m$ | **minimum** number of points → fit the model |

---

### RANSAC | **Mathematical Formulation**

for a dataset with,

|||
|----|:-----|
| $n$ | total points |
| $\epsilon$ | hypothesized probability of point being **inlier** |
| $m$ | **minimum** number of points → fit the model |
| $p$ | desired probability → finding at **least one good sample** |
||

---

### RANSAC | **Mathematical Formulation**

for a dataset with,

[number of iterations](https://dl.acm.org/doi/pdf/10.1145/358669.358692) needed, $k$,

$$
k = \frac{log(1 - p)}{log(1 - (1 - \epsilon)^m)}
$$

---

### RANSAC | **Key Parameters**

---

### RANSAC | **Key Parameters**

| **parameter** | description |
|:--------------|:---------------|
| **inlier threshold** | max distance point can be from model → inlier|

---

### RANSAC | **Key Parameters**

---

### RANSAC | **Key Parameters**

| **parameter** | description |
|:--------------|:---------------|
| **inlier threshold** | max distance point can be from model → inlier|
| **iteration count** | maximum number of iterations to attempt |
| **consensus threshold**  | minimum number of inliers → accept a model|
---

### RANSAC | **Key Parameters**

---

### RANSAC | **LiDAR example**

---

### RANSAC | **LiDAR example**

---

let's define a few parameters,

---

let's define a few parameters,

| symbol | description |
|--------|-------------|
| **N** | max number of attempts |

---

let's define a few parameters,

| symbol | description |
|--------|-------------|
| **N** | max number of attempts |
| **S** | number of samples to compute initial line |

---

let's define a few parameters,

| symbol | description |
|--------|-------------|
| **N** | max number of attempts |
| **S** | number of samples to compute initial line |
| **D** | degrees from initial reading to sample from |

---

let's define a few parameters,

| symbol | description |
|--------|-------------|
| **N** | max number of attempts |
| **S** | number of samples to compute initial line |
| **D** | degrees from initial reading to sample from |
| **X** | max distance reading may be from line |

---

let's define a few parameters,

---

### RANSAC | **pseudocode**

---

### RANSAC | **pseudocode**

```[1|3|4|5|6|8-9|10|12|14|16|17-18|20|22-23|3-6]
function RANSAC( N, S, D, X, C )

while 
   there are still unassociated laser readings
   number of readings > consensus C
   completed < N trials

do
   select random laser data reading, R
   random sample S data readings within D degrees of R

calculate least_squares_best_fit_line(S,R)

how many readings within X cm of best fit line

if num of readings on best fit line > consensus C
      calculate new least squares best fit line
          based on all readings that lie on old line

add new best fit line to lines extracted so far

remove number of readings lying on this line
          from total set of unassociated readings
```

---

RANSAC → produces reasonable results

---

RANSAC → produces reasonable results

even with a **significant percentage of outliers**

---

### RANSAC | **visual example**

---

### RANSAC | **visual example**

<div>

</div>

<div>

- <b><span style="color:green">green</span></b> → inliers ("true model")
- <b><span style="color:red">red</span></b> → outliers

</div>
</div>

---

### RANSAC | **visual example**

<div>

</div>

<div>

- <b><span style="color:green">green</span></b> → inliers ("true model")
- <b><span style="color:red">red</span></b> → outliers

</div>
</div>

challenge of RANSAC → finding **correct model** in presence of outliers

---

### RANSAC | **visual example**

<div>

</div>

<div>

- <b><span style="color:blue">blue</span></b> → random points
- <b><span style="color:orange">orange</span></b> line → **poor model**

</div>
</div>

---

### RANSAC | **visual example**

<div>

</div>

<div>

- <b><span style="color:blue">blue</span></b> → random points
- <b><span style="color:orange">orange</span></b> line → **poor model**

</div>
</div>

demonstrates why random sampling alone isn't enough

---

### RANSAC | **visual example**
<div class="multicolumn">

<div>

</div>

<div>

- <b><span style="color:blue">blue</span></b> → better random selection (**inliers**!)
- <b><span style="color:green">green</span></b> line → **good** model fit

</div>
</div>

---

### RANSAC | **visual example**
<div class="multicolumn">

<div>

</div>

<div>

- <b><span style="color:blue">blue</span></b> → better random selection (**inliers**!)
- <b><span style="color:green">green</span></b> line → **good** model fit

</div>
</div>

illustrates RANSAC can find good models → multiple **random** attempts

---

### RANSAC | **visual example**

<div>

</div>

<div>

- <b><span style="color:green">green</span></b> → identified inliers (**consensus set**)
- <b><span style="color:gray">gray</span></b> lines → **error** threshold boundaries
- <b><span style="color:#ff6f69">faded red</span></b> → **rejected** outliers

</div>
</div>

---

### RANSAC | **visual example**

---

### RANSAC | **visual example**

---

### RANSAC | **visual example**

<div>

</div>

<div>

key **insights**

- strength → **iterative** approach

</div>
</div>

---

### RANSAC | **visual example**

<div>

</div>

<div>

key **insights**

- strength → **iterative** approach
- if random sample has outliers
    - **future** iterations can fix it
</div>
</div>

---

### RANSAC | **visual example**

<div>

</div>

<div>

key **insights**

- strength → **iterative** approach
- if random sample has outliers
    - **future** iterations can fix it
- **error threshold** → crucial
    - determines → outliers

</div>
</div>

---

### RANSAC | **visual example**

<div>

</div>

<div>

key **insights**

- strength → **iterative** approach
- if random sample has outliers
    - **future** iterations can fix it
- **error threshold** → crucial
    - determines → outliers
- consensus → adds **robustness**
    - how many points → model

</div>
</div>

---

RANSAC → can **extrapolate lines as dots**

---

RANSAC → can **extrapolate lines as dots**

makes for **easier calculations**

---

RANSAC is **robust against people**!

---

### RANSAC | **Limitations**

|||
|:-----|:----|
| **non-deterministic** | each run → different results |
| **parameter sensitivity** | performance → tuning threshold parameters |
| **computation** | **many iterations** → complex models/high outlier ratios |
| **no guarantee** | can fail to find optimal solution <br> (especially with poor parameter selection) |
||

---

various updates/variations of RANSAC → [see textbook](https://autonomy-course.github.io/textbook/autonomy-textbook.html#landmarks-ransac-random-sample-consensus)

---

### Landmarks | Scan Matching

---

### Landmarks | Scan Matching

**align** current sensor readings → with previous scans

---

### Landmarks | Scan Matching

**align** current sensor readings → with previous scans

determine **position changes**

---

### Landmarks | Scan Matching

see textbook for:

- [top 5 SCAN matching algorithms](https://autonomy-course.github.io/textbook/autonomy-textbook.html#landmarks-scan-matching) 
- [Spike vs RANSAC vs Scan Comparison](https://autonomy-course.github.io/textbook/autonomy-textbook.html#comparison-of-spike-ransac-and-scan-matching)

---

## Data Association

---

**matching** observed landmark from **different** scans is important!

---

consider a chair

---

- leave the room and then

---

- leave the room and then 
- (at some later point) return to the room

---

<div>

- same chair?
- different?

</div>

---

- same chair?
- **associated** this chair → old chair

---

simple?

---

data association is **hard** to do **well**

---

consider → room had **two identical chairs**

---

<div>

- same two chairs?
- which is which?

</div>

Note:
When we subsequently return to the room we might not be able to distinguish accurately which of the chairs were which of the chairs we originally saw (as they all look the same!). Our best bet is to say that the one to the left must be the one we previously saw to the left, and the one to the right must be the one we previously saw on the right.

---

need to **distinguish** previously seen landmarks vs. new ones

---

### Data Association

---

### Data Association

we may encounter following issues:

---

### Data Association

we may encounter following issues:

- you might not re-observe landmarks →  every time step

---

### Data Association

we may encounter following issues:

- you might not re-observe landmarks →  every time step
- you might observe landmark → fail to ever see it again

---

### Data Association

we may encounter following issues:

- you might not re-observe landmarks →  every time step
- you might observe landmark → fail to ever see it again
- you might wrongly associate landmark → previously seen landmark

---

recall → should be **easy** to re-observe landmarks

---
- you might not re-observe landmarks →  every time step
- you might observe landmark → fail to ever see it again
- you might wrongly associate landmark → previously seen landmark

first two cases **not acceptable** → rather they're **bad landmarks**

---

even very good landmark extraction algorithm may fail!

---

even very good landmark extraction algorithm may fail!

define a suitable **data-association policy**

---

- you might wrongly associate landmark → previously seen landmark

---

- you might wrongly associate landmark → previously seen landmark

**really** problematic → robot thinks it is at different location!

---

high level **SLAM data association policy**

---

high level **SLAM data association policy**

- assume **database of previously seen landmarks** → initially empty

---

high level **SLAM data association policy**

- assume **database of previously seen landmarks** → initially empty
- don’t consider a landmark → unless seen **N** times

---

**nearest neighbor approach** → landmark association

---

**nearest neighbor approach** → landmark association

```[0|1|2|3|5|6-7|8]
   landmark_extraction(...) to extract all visible landmarks
   associate each extracted landmark to closest landmark 
       seen > N times in database

each association --> validation_gate(extracted, seen in database)
       if validation_gate(...) passes --> same landmark
           increment number in database
       if validation_gate(...) fails --> add as new landmark in database
```

---

**simplest** calculation for "nearest landmark" → **[euclidean distance](https://hlab.stanford.edu/brian/euclidean_distance_in.html)**

---

### euclidean distance

<div>

$$
d(p,q) = \sqrt{(p-q)^2}
$$

</div>

---

other methods → **[Mahalanobis distance](https://www.mathworks.com/help/stats/mahal.html#mw_c4c6cf50-a523-47ef-adaf-085c19dff68d)**

---

other methods → **[Mahalanobis distance](https://www.mathworks.com/help/stats/mahal.html#mw_c4c6cf50-a523-47ef-adaf-085c19dff68d)**

better but more complicated

---

but what is → **validation gate**?

---

**validation gate**

check if landmark lies → **within area of uncertainty from EKF**

---

**validation gate**

- EKF →  bound on uncertainty of landmark observation

---

**validation gate**

- EKF →  bound on uncertainty of landmark observation
- is "observed" landmark in database → within area of uncertainty?

---

if we set a **constant threshold**, $\lambda$,

---

if we set a **constant threshold**, $\lambda$,

observed landmark is associated to real landmark if,

---

observed landmark is associated to real landmark if,

$$v_i^T S_i^{-1} v_i \leq \lambda$$

---

observed landmark is associated to real landmark if,

$$v_i^T S_i^{-1} v_i \leq \lambda$$

**validation gate** condition

---

<div>

$$v_i^T S_i^{-1} v_i \leq \lambda$$

**validation gate** condition

</div>

<div>

| symbol | description |
|--------|-------------|
| $v_i$  | **innovation** <br> (difference observed and predicted) |
| $S_i$  | innovation **covariance** matrix |
| $\lambda$ | constant **threshold** value |
||

</div>

---

the mathematical expression using Mahalanobis distance test

---

## EKF-SLAM

---

## EKF-SLAM

- estimate state (position) of robot

---

## EKF-SLAM

- estimate state (position) of robot 
- using odometry+landmarks

---

### normal EKF vs EKF-SLAM

---

### normal EKF vs EKF-SLAM

- regular EKF robot → given a "_perfect map_"

---

### normal EKF vs EKF-SLAM

- regular EKF robot → given a "_perfect map_"
- SLAM → needs a **map update** step

Note:
 since the robot is figuring out the map as it goes along.

---

## EKF-SLAM

mostly standard EKF → once matrices are set up

Note:
So the trick lies in setting up the right equations.

---

see [textbook](https://autonomy-course.github.io/textbook/autonomy-textbook.html#ekf-slam) for detailed example → extracted from [SLAM for Dummies](https://dspace.mit.edu/bitstream/handle/1721.1/36832/16-412JSpring2004/NR/rdonlyres/Aeronautics-and-Astronautics/16-412JSpring2004/A3C5517F-C092-4554-AA43-232DC74609B3/0/1Aslam_blas_report.pdf)

---

**properties** of EKF-SLAM

---

**properties** of EKF-SLAM

1. computational complexity → **quadratic** in num. of landmarks: $O(n^2)$

---

**properties** of EKF-SLAM

---

**properties** of EKF-SLAM

1. computational complexity → **quadratic** in num. of landmarks: $O(n^2)$
2. determinant of any sub-matrix of the map covariance matrix 
    - **decreases monotonically** as observations are made
    - increasing **similarity**
3. at the limit → landmark estimates become **fully correlated**

---

**properties** of EKF-SLAM

---

## SLAM Implementations

---

## SLAM Implementations

one of the most studied areas in robotics and autonomous systems

---

## SLAM Implementations

1. **[Scan matching](https://link.springer.com/chapter/10.1007/978-3-642-13408-1_14)**

Note:
can also be used a generic SLAM method. It attempts to align consecutive laser scans to determine robot displacement. This approach is computationally efficient but may accumulate errors over time.

---

## SLAM Implementations

1. **[Scan matching](https://link.springer.com/chapter/10.1007/978-3-642-13408-1_14)**

2. **[Submaps Approach](https://www.atlantis-press.com/proceedings/icaic-24/126003458)**

Note:
to manage computational complexity, large environments can be **divided into smaller submaps**. Each submap is built using standard SLAM techniques and then the submaps are connected. This divide-and-conquer approach reduces the computational burden and has been successfully applied in large environments.

---

## SLAM Implementations

1. **[Scan matching](https://link.springer.com/chapter/10.1007/978-3-642-13408-1_14)**

2. **[Submaps Approach](https://www.atlantis-press.com/proceedings/icaic-24/126003458)**

3. **[Sparse Extended Information Filters](http://robots.stanford.edu/papers/thrun.seif.pdf)** (SEIF)

Note:
SEIF is the **information form** of EKF, which maintains the information matrix (inverse of covariance matrix). The information matrix naturally becomes sparse in SLAM, allowing for efficient algorithms.

---

## SLAM Implementations

1. **[Scan matching](https://link.springer.com/chapter/10.1007/978-3-642-13408-1_14)**

2. **[Submaps Approach](https://www.atlantis-press.com/proceedings/icaic-24/126003458)**

3. **[Sparse Extended Information Filters](http://robots.stanford.edu/papers/thrun.seif.pdf)** (SEIF)

4. **[FastSLAM](http://robots.stanford.edu/papers/Thrun03g.pdf)**

Note:
uses **particle filters** to represent the robot's path and maintains separate EKFs for each landmark. This factorization takes advantage of conditional independence properties. This approach scales better than standard EKF-SLAM, with complexity $O(M\cdot log N)$ where $M$ is the number of particles and $N$ is number of landmarks.

---

so far we've completed,

---

so far we've completed,

|||
|:----|:----|
| state estimation | understand itself |

---

so far we've completed,

|||
|:----|:----|
| state estimation | understand itself |
| sensor fusion | make sense of surroundings |

---

so far we've completed,

|||
|:----|:----|
| state estimation | understand itself |
| sensor fusion | make sense of surroundings |
| localization and mapping | current location |

---

so far we've completed,

|||
|:----|:----|
| state estimation | understand itself |
| sensor fusion | make sense of surroundings |
| localization and mapping | current location |
| control and actuation | how to move |
||

---

so far we've completed,

|||
|:----|:----|
| state estimation | understand itself |
| sensor fusion | make sense of surroundings |
| localization and mapping | current location |
| control and actuation | how to move |
||

important question → **where** to move?

---

enter **path planning**