Dynamic SLAM using Landscape Theory of Aggregation

Akshit Gandhi (akgandhi)

Avinash Hemaeshwara Raju (ahemaesh)

Parv Parkhiya (pparkhiy)

Team Slam Dunk

Introduction

​

• Localization in dynamic environment a hard problem
• Limited literature available on dynamic SLAM
• Dynamic SLAM has application in self-driving cars, factory automation robots
• For our project, we addressed dynamic SLAM problem through landscape theory of aggregation as proposed in the original paper

Landscape Theory of Aggregation

• Landscape theory was first introduced in political science journal in 1993

​

• Aggregation → organization of elements of compatible elements together and less compatible elements apart.

​

• Landscape theory → alignment among nations, whose leaders are myopic in their assessments and incremental in their actions.

​

• Goal is to minimize frustration among nations based upon their pairwise propensities to align with some nations and oppose others.

​

Application in SLAM

• Frustration score in a set of only static correspondences or in a set of only dynamic correspondences will be the least.

​

• Propensity score gives the influences(weights) of one correspondence on another correspondence.

​

– “a source of conflict with a small country is not as important for determining alignments as an equivalent source of conflict with a large country”.

​

• Based on the propensity score, a classifier tries to achieve the minimum energy state by classifying correspondences into static or dynamic classes.

​

• After classification, we omit dynamic correspondences and perform �SLAM using the static correspondences.

​

​

​

Application in SLAM

​

We minimize following cost function for classification:

​

​

​

For our case, size/weight and propensity are defined as,

​

​

​

​

​

​

​

​

​

​

​

​

​

Implementation Details - Data Set

• Our initial implementation was on Wean Hall dataset

​

• We collected a dataset in NSH B Level using a SICK Lidar and Intel Realsense camera

​

• The Lidar and camera were mounted on a clearpath Husky

​

• We mimicked a dynamic environment by walking in front of a moving Husky

• Simultaneously recorded rosbags which were later parsed�To extract odometry tf-tree, laser-scan and rgb images.

Robot used for data collection

Implementation Details - Data Association

Nearest Neighbor Data Association

​

• Data association done between frames (Not with global map)

​

• Odometry data used to transform points into common reference frame

​

• Association rejected if match distance is more than threshold

Implementation Details - Optimizer

Requirement:

• Optimizer with binary variables (u_i=0 or u_i=0.001)

Proposition:

• Using ceres-solver as nonlinear least square optimization
• Additional cost to force pseudo binary state
• Additional cost to make sure not all gets assigned to same class

Results:

• Worked for toy hand crafted with 6 variables
• Extremely slow for 180 variables in real data
• Full/Dense “A” matrix

​

​

​

​

​

Cost 1:

Cost 2:

Implementation Details - Optimizer

Custom Binary Optimizer:

• Written from scratch in C++
• Variables are booleans to represent static and dynamic class
• Iterative approach to get to local minima
• A gradient step is defined as flipping or not flipping state of each variable that leads to decrease in energy/cost function
• Computing cost change (ΔE) for flipping the variable not the entire cost, resulting in performance improvement
• Convergence in 4-5 iterations usually

​

An Iteration of Optimizer

End-to-End Pipeline

Results

Fig 1: Input laserscan after data association

Fig 2: Initial guess input to the optimizer

Fig 3: Output from the optimizer

Red = Dynamic landmarks

Green = Static landmarks

For initialization,

mean(d_Ai ) < threshold : Static

mean(d_Ai ) >= threshold : Dynamic

Results - Classification

Results - Dynamic SLAM

Results - Dynamic SLAM

​

Limitation

• Limited improvement when integrated with GMapping
• GMapping (Monte Carlo) SLAM inherently handles outliers
• Integration with EKF or Graph Based should significantly improve the localization
• In scene with large dynamic objects, our system should yield significant improvement in mapping and localization
• Adds computational overhead in the pipeline of SLAM
• Currently works only with the 2D-LIDAR data

Future Work

• Testing the system on diverse dataset with ground truth for quantitative analysis

​

• Extending the idea to 3D-Lidar features as well as image based features

​

• Extending to multiclass classification where each non-static class corresponds to feature points belonging to particular dynamic object.
• would enable modeling motion of various dynamic objects in the scene