1 of 36

CISC849:

Robot Vision & Learning

https://goo.gl/ektJij

Prof. Christopher Rasmussen

February 15, 2018

2 of 36

Sample "Grand" Challenges: Total ARGOS challenge (2017)

ANYmal approach

6 of 36

Point Cloud Library (PCL) (>= 1.6)

Binaries/source (with compile instructions) for Linux, Windows, Mac OS X on Github:

https://github.com/PointCloudLibrary/pcl

High-level documentation

http://www.pointclouds.org/documentation

Tutorials:

http://www.pointclouds.org/documentation/tutorials

Data

https://github.com/PointCloudLibrary/data

ROS library brief overview

7 of 36

PCL for point clouds

8 of 36

PCL Basics

Point cloud is fundamental data structure

Basically a struct with an std::vector of N-D points
Points either unorganized (C x 1) or organized (C x R -- like a range image)

9 of 36

PCL Basics

Point cloud is fundamental data structure

Basically a struct with an std::vector of N-D points
Points either unorganized (C x 1) or organized (C x R -- like a range image)

Point types

PointXYZ: x, y, z (N = 3)
PointXYZI: plus intensity (N = 4)
PointXYZRGB: plus color (N = 6)
Normal: nx, ny, nz (N = 3)
Lots more, including higher-dimensional signatures/histograms

10 of 36

Point cloud storage/display

Static scenes in .pcd files

Examples: table..., armory..., Place...

Visualization tool

pcl_viewer <filename>
Mouse

Drag to rotate, wheel to zoom
Shift-drag to translate

Various keyboard options ("h" brings up list in terminal)

+/-: Change size of points
0-9: Choose color "handler" (color by axis, show intensity, etc.)
'j': Take .png snapshot of window

11 of 36

PCL Basics

Point clouds usually templated on point type

pcl::PointCloud<pcl::PointXYZ> cloud;�std::vector<pcl::PointXYZ> data = cloud.points;

12 of 36

PCL Basics

Point clouds usually templated on point type

pcl::PointCloud<pcl::PointXYZ> cloud;�std::vector<pcl::PointXYZ> data = cloud.points;

pcl::PCLPointCloud2 is not templated, in case you don't know point type (for example, when loading from .pcd)

Most PCL functions templated on point type, but some overloaded to take pcl::PCLPointCloud2

13 of 36

PCL Basics

Point clouds usually templated on point type

pcl::PointCloud<pcl::PointXYZ> cloud;�std::vector<pcl::PointXYZ> data = cloud.points;

pcl::PCLPointCloud2 is not templated, in case you don't know point type (for example, when loading from .pcd)

Most PCL functions templated on point type, but some overloaded to take pcl::PCLPointCloud2

Be aware that some functions expect point cloud and some expect pointer to point cloud

Conversions: cloud.makeShared(), *cloudptr

14 of 36

Point cloud storage: Read, write

// Save result � pcl::PCDWriter writer;� writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered);�� return (0);�}

15 of 36

PCL filtering: Range selection on axis

Original scene

After applying Z ∈ [0, 2] range selection

courtesy of http://robotica.unileon.es/mediawiki/index.php/PCL/OpenNI_tutorial_2:_Cloud_processing_(basic)

16 of 36

PCL filtering:

Range selection with PassThrough

#include <iostream>�#include <pcl/point_types.h>�#include <pcl/filters/passthrough.h>��int� main (int argc, char** argv)�{� pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);� pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);�� // Fill in the cloud data

// Example creates random points, but could also load from .pcd

� std::cerr << "Cloud before filtering: " << std::endl;� for (size_t i = 0; i < cloud->points.size (); ++i)� std::cerr << " " << cloud->points[i].x << " " � << cloud->points[i].y << " " � << cloud->points[i].z << std::endl;�� // Create the filtering object� pcl::PassThrough<pcl::PointXYZ> pass;� pass.setInputCloud (cloud);� pass.setFilterFieldName ("z");� pass.setFilterLimits (0.0, 2.0);� //pass.setFilterLimitsNegative (true); // take points *outside* range instead of inside� pass.filter (*cloud_filtered); // cloud_filtered now contains only points that passed filter�� return (0);�}

17 of 36

PCL filtering: Downsampling

Original

courtesy of http://robotica.unileon.es/mediawiki/index.php/PCL/OpenNI_tutorial_2:_Cloud_processing_(basic)

18 of 36

PCL filtering: Downsampling

1 cm voxel size

courtesy of http://robotica.unileon.es/mediawiki/index.php/PCL/OpenNI_tutorial_2:_Cloud_processing_(basic)

19 of 36

PCL filtering:

Downsampling with VoxelGrid

#include <iostream>�#include <pcl/io/pcd_io.h>�#include <pcl/point_types.h>�#include <pcl/filters/voxel_grid.h>��int�main (int argc, char** argv)�{� pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());� pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());�� // Fill in the cloud data� pcl::PCDReader reader;� // Replace the path below with the path where you saved your file� reader.read ("table_scene_lms400.pcd", *cloud); // Remember to download the file first!�� // Create the filtering object� pcl::VoxelGrid<pcl::PCLPointCloud2> sor;� sor.setInputCloud (cloud);� sor.setLeafSize (0.01f, 0.01f, 0.01f); // sidelengths of each voxel "box"� sor.filter (*cloud_filtered);�� pcl::PCDWriter writer;� writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered);�� return (0);�}

20 of 36

Ground Segmentation

A standard task for robot point cloud data is segmentation--e.g., which points belong to the "bicycle", which points to the "car", which to the "tree", etc.
And what could be more basic than segmenting points into ground vs. obstacles?

21 of 36

Ground Segmentation

A standard task for robot point cloud data is segmentation--e.g., which points belong to the "bicycle", which points to the "car", which to the "tree", etc.
And what could be more basic than segmenting points into ground vs. obstacles?

With a level ladar we don't really have this problem, because it's not even supposed to "see" the ground

Based on height, everything detected is assumed an obstacle

22 of 36

Ground Segmentation

Potential obstacles have many possible shapes, but indoors and outdoors in paved areas, we can assume that the ground is planar

23 of 36

Ground Segmentation

Potential obstacles have many possible shapes, but indoors and outdoors in paved areas, we can assume that the ground is planar
If we can extract the ground plane from the point cloud, then everything left over is an obstacle (positive or negative) or "object"

"Left over" means computing the distance of each point to the ground plane and thresholding

24 of 36

One dominant plane

25 of 36

Important planes

Ground: obstacle avoidance, debris detection
Table/shelf: find drill
Door: find handle
Wall: find valve, hose reel, "surprise" plug
Stairs: distinguish treads from railing
Non-DRC examples???

26 of 36

Multiple large planes

27 of 36

Ground Segmentation

Potential obstacles have many possible shapes, but indoors and outdoors in paved areas, we can assume that the ground is planar
If we can extract the ground plane from the point cloud, then everything left over is an obstacle (positive or negative) or "object"

"Left over" means computing the distance of each point to the ground plane and thresholding

But how to estimate the plane(s) parameters?

28 of 36

Robust Plane Fitting

The 3-D plane equation is ax + by + cz + d = 0
We wish to estimate (a, b, c, d)
3 non-collinear points are the minimum solution to define a plane

With noise from our sensor, we would want to use more points for a least-squares fit to get a better result
But if we include points that don't belong to the plane (i.e., obstacle points), these outliers will bias the results

29 of 36

Robust Plane Fitting

The 3-D plane equation is ax + by + cz + d = 0
We wish to estimate (a, b, c, d)
3 non-collinear points are the minimum solution to define a plane

With noise from our sensor, we would want to use more points for a least-squares fit to get a better result
But if we include points that don't belong to the plane (i.e., obstacle points), these outliers will bias the results

A robust plane-fitting technique will work to identify and reject outliers and only do the least-squares fit on the inliers

32 of 36

RANSAC plane fitting

RANSAC (RANdomized SAmple Consensus) is a commonly-used approach to robust fitting
It is iterative, in that multiple hypotheses are considered

Each hypothesis is a randomly-chosen sample of the points which minimally solves the problem

For line-fitting, a sample would be 2 points
For plane-fitting, a sample is 3 points

33 of 36

RANSAC plane fitting

RANSAC (RANdomized SAmple Consensus) is a commonly-used approach to robust fitting
It is iterative, in that multiple hypotheses are considered

Each hypothesis is a randomly-chosen sample of the points which minimally solves the problem

For line-fitting, a sample would be 2 points
For plane-fitting, a sample is 3 points

The solution for a given sample is scored by counting the number of inliers (points < threshold distance away)

34 of 36

RANSAC plane fitting

RANSAC (RANdomized SAmple Consensus) is a commonly-used approach to robust fitting
It is iterative, in that multiple hypotheses are considered

Each hypothesis is a randomly-chosen sample of the points which minimally solves the problem

For line-fitting, a sample would be 2 points
For plane-fitting, a sample is 3 points

The solution for a given sample is scored by counting the number of inliers (points < threshold distance away)

The sample with the most inliers (after a certain number of tries) is considered the "best", and a least-squares solution is computed from it

35 of 36

RANSAC in PCL

PCL has a sample_consensus module which implements RANSAC and has options for fitting planes as well as other shapes

Constraints can also be imposed, such as looking for horizontal planes vs. vertical planes

This tutorial and this one will get you started

36 of 36

RANSAC in PCL

#include <iostream>�#include <pcl/ModelCoefficients.h>�#include <pcl/io/pcd_io.h>�#include <pcl/point_types.h>�#include <pcl/sample_consensus/method_types.h>�#include <pcl/sample_consensus/model_types.h>�#include <pcl/segmentation/sac_segmentation.h>��int main (int argc, char** argv)�{� pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);�� // Fill in the cloud data…

// ???�� // Try to do robust plane fit� pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);� pcl::PointIndices::Ptr inliers (new pcl::PointIndices);� pcl::SACSegmentation<pcl::PointXYZ> seg; // Create segmentation object�� seg.setOptimizeCoefficients (true); // optional

seg.setModelType (pcl::SACMODEL_PLANE); // mandatory� seg.setMethodType (pcl::SAC_RANSAC);� seg.setDistanceThreshold (0.01);�� seg.setInputCloud (cloud);� seg.segment (*inliers, *coefficients);�

� // did it work?

� if (inliers->indices.size () == 0) {� PCL_ERROR ("Could not estimate a planar model for the given dataset.");� return (-1);� }�

// report results

std::cerr << "Model coefficients: "

<< coefficients->values[0] << " " � << coefficients->values[1] << " "� << coefficients->values[2] << " " � << coefficients->values[3] << std::endl;�� std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;� for (size_t i = 0; i < inliers->indices.size (); ++i)� std::cerr << inliers->indices[i] << " "

<< cloud->points[inliers->indices[i]].x << " "� << cloud->points[inliers->indices[i]].y << " "� << cloud->points[inliers->indices[i]].z <<

std::endl;

return (0);�}

1 of 36

2 of 36

3 of 36

4 of 36

5 of 36

6 of 36

7 of 36

8 of 36

9 of 36

10 of 36

11 of 36

12 of 36

13 of 36

14 of 36

15 of 36

16 of 36

17 of 36

18 of 36

19 of 36

20 of 36

21 of 36

22 of 36

23 of 36

24 of 36

25 of 36

26 of 36

27 of 36

28 of 36

29 of 36

30 of 36

31 of 36

32 of 36

33 of 36

34 of 36

35 of 36

36 of 36