dynamic_obstacle_tracking
detectes and track dynamic obstacles in pointcloud
Public Member Functions | Protected Attributes | Private Member Functions | List of all members
datmo::cloud_segmentation Class Reference

#include <dynamic_obstacle_tracking.hpp>

Inheritance diagram for datmo::cloud_segmentation:
Velodyne ZR300

Public Member Functions

 cloud_segmentation ()
 
 ~cloud_segmentation ()
 
void init (ros::NodeHandle &nh, ros::NodeHandle &private_nh)
 

Protected Attributes

ros::Publisher pub
 publish point cloud after transforming it to base_link on topic /transformed_points More...
 
ros::Publisher pub_2
 Publish pointcloud with dynamic obstacles on topic /dynamic_points. More...
 
ros::Publisher pub_3
 Publish pointcloud with dynamic obstacles on topic /dynamic_points. More...
 
ros::Publisher centroids_pub
 Publish geometry_msgs::PoseArray centroids of dynamic obstacles on topic /object_centroids. More...
 
ros::Subscriber sub
 subscribes sensor_msgs::PointCloud2 from topic /cloud More...
 
tf::TransformListener listener
 A TF listener. More...
 
tf::TransformBroadcaster br
 TF brodcaster to broadcast relative transfom between current pointcloud and prev PointCloud. More...
 
tf::StampedTransform current_cloud_transform
 Container to store transform of latest pointcloud wrt /odom. More...
 
tf::StampedTransform ref_cloud_transform
 Container to store transform of reference pointcloud wrt /odom. More...
 
tf::StampedTransform prev_cloud_transform
 Container to store transform of previous pointcloud wrt /odom. More...
 
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud_transformed
 container to store current pointcloud after transforming it to /base_link frame from /sensor frame More...
 
pcl::PointCloud< pcl::PointXYZ >::Ptr obj_cloud
 container to store pointcloud having both static and dynamic objects More...
 
pcl::PointCloud< pcl::PointXYZ >::Ptr ref_obj_cloud
 container to store object cloud to be used as reference for comparing with object cloud at current time step for octree based spatial change detection More...
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr occuluded_cloud
 container to store the points at current timestep that are in the areas which were occuluded in reference object cloud. (Green in colour) More...
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr new_discovered_cloud
 Container to store points at current timestep that are in the areas which are newly discovered. (Blue in colour) More...
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr filtered_dynamic_cloud
 Container to store filtered dynamic cloud (Red in colour) = dynamic_cloud - occuluded_cloud - new_discovered_cloud. More...
 
pcl::PointCloud< pcl::PointXYZRGB >::Ptr classified_cloud
 Container to store final classified cloud = filtered_dynamic_cloud(Red) + occuluded_cloud(Green) + new_discovered_cloud(Blue) More...
 
vector< pcl::PointCloud< pcl::PointXYZ >::Ptr, Eigen::aligned_allocator< pcl::PointCloud< pcl::PointXYZ >::Ptr > > sourceClouds
 vector to store the "OCTREE_WINDOW" number of obj_cloud in the memory More...
 
vector< tf::StampedTransform > sourceTransforms
 Vector to store corresponding Transforms of each PointCloud in sourceClouds /odom to /base_link. More...
 
Eigen::Affine3d odom_transform_matrix
 Container to store relative transform between obj_cloud and ref_obj_cloud. More...
 
Eigen::Affine3f icp_transform_matrix
 
int counter
 
string frame_id
 
string base_frame_id
 
string sensor_frame_id
 
bool ENABLE_ICP
 Enable use of ICP to better match obj_cloud with ref_obj_cloud : Default it is disabled bcoz of bad results. More...
 
bool ENABLE_GROUND_REMOVAL
 Enable use of ransac plane fitting to remove ground points : Default it is disabled for ZR300 and enabled for Velodyne. More...
 
bool ENABLE_VOXELISE
 Enable voxel filter: Default it is enable for ZR300 and disabled for Velodyne. More...
 
bool ENABLE_OCCLUSION_DETECTION
 Enable Occlusion detection so as to prevent false dynamic obstacles detection due to shadow : Default it is enabled for ZR300 and enabled for Velodyne. More...
 
float VOXEL_LEAF_SIZE
 
bool ENABLE_SOR
 Enable Statistical Outlier removal filter: Default it is enable for ZR300 and disabled for Velodyne. More...
 
float SOR_MEAN_K
 number of neighbors to analyze for each point More...
 
float SOR_STD_DEV_MUL_THRESH
 distance in multiples of std dev after which point is considered an outlier More...
 
float PASS_X_MIN
 minimum value in x direction More...
 
float PASS_X_MAX
 maximum value in x direction More...
 
float PASS_Y_MIN
 minimum value in y direction More...
 
float PASS_Y_MAX
 maximum value in y direction More...
 
float PASS_Z_MIN
 minimum value in z direction More...
 
float PASS_Z_MAX
 maximum value in z direction More...
 
int OCTREE_WINDOW
 This parameter defines that current pointcloud will be compared with "OCTREE_WINDOW" times previous pointcloud for spatial change detection. More...
 
float OCTREE_RESOLUTION
 smallest dimension of octree leaf node More...
 
float SEG_CLUSTER_TOLERANCE
 Maximum distance between two points to be included in same cluster. More...
 
int SEG_MIN_CLUSTER_SIZE
 Minimum number of points that should be present within a cluster. More...
 
int SEG_MAX_CLUSTER_SIZE
 Maximum number points to be included in same cluster. More...
 

Private Member Functions

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
 
void cloud_transform (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_in, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_out)
 
void cloud_filter (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_in, const string field, float min, float max)
 
void Voxel_filter (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_in, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_out, float VOXEL_LEAF_SIZE)
 
void SOR_filter (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_in, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_out, float SOR_MEAN_K, float SOR_STD_DEV_MUL_THRESH)
 
void Ransac_plane (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_in, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_out)
 
void euclideancluster (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float SEG_CLUSTER_TOLERANCE, int SEG_MIN_CLUSTER_SIZE, int SEG_MAX_CLUSTER_SIZE)
 
void detect_spatial_change (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_now, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_to_compare, const pcl::PointCloud< pcl::PointXYZ >::Ptr &dynamic_cloud, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &occuluded_cloud, float OCTREE_RESOLUTION, bool ENABLE_OCCLUSION_DETECTION)
 
bool is_in_bounding_area (const pcl::PointCloud< pcl::PointXYZ >::Ptr &coord, pcl::PointXYZRGB pt)
 
void integrateOdom_ICP (const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &obj_cloud, const pcl::PointCloud< pcl::PointXYZ >::Ptr &ref_obj_cloud)
 

Constructor & Destructor Documentation

◆ cloud_segmentation()

cloud_segmentation::cloud_segmentation ( )

◆ ~cloud_segmentation()

cloud_segmentation::~cloud_segmentation ( )

Member Function Documentation

◆ cloud_cb()

void cloud_segmentation::cloud_cb ( const sensor_msgs::PointCloud2ConstPtr &  cloud_msg)
private

Callback function get called every time a new point cloud is recieved

Parameters
[in]cloud_msgconstant pointer to sensor_msgs PointCloud2

◆ cloud_filter()

void cloud_segmentation::cloud_filter ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_in,
const string  field,
float  min,
float  max 
)
private

filters the pointcloud wiithin given dimensions and field

Parameters
[in,out]cloud_inponitcloud to be filtered
[in]fieldaxis name ("x" or "y" or "z")
[in]minminimum value of points in given field
[in]maxmaximum value of points in given field

◆ cloud_transform()

void cloud_segmentation::cloud_transform ( const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &  cloud_in,
const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_out 
)
private

Transforms the point cloud from sensor_frame_id to base_link This function contains a TF listener

Parameters
[in]cloud_in
[out]cloud_out

◆ detect_spatial_change()

void cloud_segmentation::detect_spatial_change ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_now,
const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_to_compare,
const pcl::PointCloud< pcl::PointXYZ >::Ptr &  dynamic_cloud,
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  occuluded_cloud,
float  OCTREE_RESOLUTION,
bool  ENABLE_OCCLUSION_DETECTION 
)
private

This function is core for classifying between dynamic and static objects. We used Octree based spatial change detection to find dynamic points more information follow link: http://pointclouds.org/documentation/tutorials/octree_change.php We compare the object cloud(obj_cloud) at current time step with object cloud "octree_window" times previous object cloud (ref_obj_cloud) We are trying to find the new octree leaf that are created in current obj_cloud that were not present previously.

Since the vehicle is moving Previous object cloud first has to be transformed to the frame of object cloud at current time step before pluging into this function

OCTREE_WINDOW and OCTREE_RESOLUTION parameter can be used for tweaking dynamic object detection For Example: Velodyne data is published at 10 HZ, Keeping OCTREE_WINDOW = 5 and OCTREE_RESOLUTION = 0.4 means that and object would be detected as dynamic if it has moved minimum of 0.4m distancein half sec.

zr300 data is published at 30 Hz , Keeping OCTREE_WINDOW = 15 and OCTREE_RESOLUTION = 0.4 means that and object would be detected as dynamic if it has moved minimum of 0.4m distancein half sec.

Parameters
cloud_now[description]
cloud_to_compare[description]
dynamic_cloud[description]
occuluded_cloud[description]
OCTREE_RESOLUTION[description]
ENABLE_OCCLUSION_DETECTIONEnables occlusion detection

◆ euclideancluster()

void cloud_segmentation::euclideancluster ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud,
float  SEG_CLUSTER_TOLERANCE,
int  SEG_MIN_CLUSTER_SIZE,
int  SEG_MAX_CLUSTER_SIZE 
)
private

This function uses euclidean clustering algorithm to remove outliers. i.e remove random points or very small set of clusters. for more information please follow the link:http://pointclouds.org/documentation/tutorials/cluster_extraction.php

Parameters
[in,out]cloudThe input PointCloud is replaced by filtered PointCloud
[in]SEG_CLUSTER_TOLERANCE
[in]SEG_MIN_CLUSTER_SIZE
[in]SEG_MAX_CLUSTER_SIZE

◆ init()

void cloud_segmentation::init ( ros::NodeHandle &  nh,
ros::NodeHandle &  private_nh 
)

◆ integrateOdom_ICP()

void cloud_segmentation::integrateOdom_ICP ( const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &  obj_cloud,
const pcl::PointCloud< pcl::PointXYZ >::Ptr &  ref_obj_cloud 
)
private

◆ is_in_bounding_area()

bool cloud_segmentation::is_in_bounding_area ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  coord,
pcl::PointXYZRGB  pt 
)
private

Since our vehicle is moving we keep exploring new areas. The points those detected in these new areas are classified as dynamic by Spatial change detection algorithm because these points did not existed in previous time step i.e in reference cloud.

Lets say coordinates of our bounding area is (PASS_X_MIN, PASS_Y_MAX) (PASS_X_MAX, PASS_Y_MAX) (PASS_X_MAX, PASS_Y_MIN) (PASS_X_MIN, PASS_Y_MIN) as the vehicle moves forward we tranform this bounding area backwards accoridingly.

Now every point that is detected as dynamic is checked if it lies in the bounding area. If not then it is classified as new detected point.

To check whether the point lies within the polygon a ray casting algorithim is used.

for more information to know about the algorithm follow link: http://www.geeksforgeeks.org/how-to-check-if-a-given-point-lies-inside-a-polygon/

Parameters
[in]coordPointCloud containing 4 points extreme co-ordinates of bounding area.
[in]ptpoint to be checked if it lies within bounding area
Returns
true/false

◆ Ransac_plane()

void cloud_segmentation::Ransac_plane ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_in,
const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_out 
)
private

This function uses Ransac plane fitting algorithm to find all the points within a point cloud that support a plane model in our case we aim to find a Ground plane and remove all the points belonging to Ground so that we are only left with points belonging to obstacles. For more information follow link: http://pointclouds.org/documentation/tutorials/planar_segmentation.php

In this function we filter out the point cloud above certain height so that the most dominating plane to be fit by Ransac is Ground Plane. Since the we have large number of data points using Ransac to fit all of them would be computationally expensive so we only try fit plane to point in surrounding area of our vehicle here x(0m to 30m) y(-15m to 15m) and z(less than 0.5m). Accuracy and computational time can be changed by tweaking these parameters.

Also for further reducing computational time we randomly choose 500 points and fit Ransac plane to them only....!

For ZR300 point cloud generated do not contain much points on ground hence Ransac_plane fitting is not neccessary. Putting a simple cap on height and neglecting all the point below that suffice our purpose. So Ground plane estimation is disabled by default.

For Pointclouds generated by Velodyne Lidars significant number of point s fall on ground plane and hence properly removing them is neccessary hence Ground Plane estimation is enabled by default.

Parameters
cloud_inPointCloud in which ground plane is to be estimated
cloud_outPointCloud containing only obstacles (after removing ground points from original PointCloud)

distance from ransac plane

◆ SOR_filter()

void cloud_segmentation::SOR_filter ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_in,
const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_out,
float  SOR_MEAN_K,
float  SOR_STD_DEV_MUL_THRESH 
)
private

StatisticalOutlierRemoval filter is used to remove sparse points generated due to noise for more information follow the link: http://pointclouds.org/documentation/tutorials/statistical_outlier.php SOR filter is required for ZR300 pointcloud data and is disables for Velodyne pointcloud

Parameters
[in]cloud_in
[out]cloud_out
[in]SOR_MEAN_K
[in]SOR_STD_DEV_MUL_THRESH

◆ Voxel_filter()

void cloud_segmentation::Voxel_filter ( const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_in,
const pcl::PointCloud< pcl::PointXYZ >::Ptr &  cloud_out,
float  VOXEL_LEAF_SIZE 
)
private

This function downsamples a PointCloud that is, reduce the number of points – a point cloud dataset, using a voxelized grid approach For more information follow the link http://pointclouds.org/documentation/tutorials/voxel_grid.php Also it removes any nan values in pointcloud This filter is neccessary for the dense PointCloud generated by stereo camera like ZED stereo camera and RGBD camera like ZR300 PointCloud generated by 3D Lidars like Velodyne and Quanergy are very sparse and hence is disable for them by default.

Parameters
[in]cloud_in
[out]cloud_out
[in]VOXEL_LEAF_SIZE

Member Data Documentation

◆ base_frame_id

string datmo::cloud_segmentation::base_frame_id
protected

◆ br

tf::TransformBroadcaster datmo::cloud_segmentation::br
protected

TF brodcaster to broadcast relative transfom between current pointcloud and prev PointCloud.

◆ centroids_pub

ros::Publisher datmo::cloud_segmentation::centroids_pub
protected

Publish geometry_msgs::PoseArray centroids of dynamic obstacles on topic /object_centroids.

◆ classified_cloud

pcl::PointCloud<pcl::PointXYZRGB>::Ptr datmo::cloud_segmentation::classified_cloud
protected

Container to store final classified cloud = filtered_dynamic_cloud(Red) + occuluded_cloud(Green) + new_discovered_cloud(Blue)

◆ cloud_transformed

pcl::PointCloud<pcl::PointXYZ>:: Ptr datmo::cloud_segmentation::cloud_transformed
protected

container to store current pointcloud after transforming it to /base_link frame from /sensor frame

◆ counter

int datmo::cloud_segmentation::counter
protected

◆ current_cloud_transform

tf::StampedTransform datmo::cloud_segmentation::current_cloud_transform
protected

Container to store transform of latest pointcloud wrt /odom.

◆ ENABLE_GROUND_REMOVAL

bool datmo::cloud_segmentation::ENABLE_GROUND_REMOVAL
protected

Enable use of ransac plane fitting to remove ground points : Default it is disabled for ZR300 and enabled for Velodyne.

◆ ENABLE_ICP

bool datmo::cloud_segmentation::ENABLE_ICP
protected

Enable use of ICP to better match obj_cloud with ref_obj_cloud : Default it is disabled bcoz of bad results.

◆ ENABLE_OCCLUSION_DETECTION

bool datmo::cloud_segmentation::ENABLE_OCCLUSION_DETECTION
protected

Enable Occlusion detection so as to prevent false dynamic obstacles detection due to shadow : Default it is enabled for ZR300 and enabled for Velodyne.

◆ ENABLE_SOR

bool datmo::cloud_segmentation::ENABLE_SOR
protected

Enable Statistical Outlier removal filter: Default it is enable for ZR300 and disabled for Velodyne.

◆ ENABLE_VOXELISE

bool datmo::cloud_segmentation::ENABLE_VOXELISE
protected

Enable voxel filter: Default it is enable for ZR300 and disabled for Velodyne.

◆ filtered_dynamic_cloud

pcl::PointCloud<pcl::PointXYZRGB>::Ptr datmo::cloud_segmentation::filtered_dynamic_cloud
protected

Container to store filtered dynamic cloud (Red in colour) = dynamic_cloud - occuluded_cloud - new_discovered_cloud.

◆ frame_id

string datmo::cloud_segmentation::frame_id
protected

◆ icp_transform_matrix

Eigen::Affine3f datmo::cloud_segmentation::icp_transform_matrix
protected

◆ listener

tf::TransformListener datmo::cloud_segmentation::listener
protected

A TF listener.

◆ new_discovered_cloud

pcl::PointCloud<pcl::PointXYZRGB>::Ptr datmo::cloud_segmentation::new_discovered_cloud
protected

Container to store points at current timestep that are in the areas which are newly discovered. (Blue in colour)

◆ obj_cloud

pcl::PointCloud<pcl::PointXYZ>::Ptr datmo::cloud_segmentation::obj_cloud
protected

container to store pointcloud having both static and dynamic objects

◆ occuluded_cloud

pcl::PointCloud<pcl::PointXYZRGB>::Ptr datmo::cloud_segmentation::occuluded_cloud
protected

container to store the points at current timestep that are in the areas which were occuluded in reference object cloud. (Green in colour)

◆ OCTREE_RESOLUTION

float datmo::cloud_segmentation::OCTREE_RESOLUTION
protected

smallest dimension of octree leaf node

◆ OCTREE_WINDOW

int datmo::cloud_segmentation::OCTREE_WINDOW
protected

This parameter defines that current pointcloud will be compared with "OCTREE_WINDOW" times previous pointcloud for spatial change detection.

◆ odom_transform_matrix

Eigen::Affine3d datmo::cloud_segmentation::odom_transform_matrix
protected

Container to store relative transform between obj_cloud and ref_obj_cloud.

◆ PASS_X_MAX

float datmo::cloud_segmentation::PASS_X_MAX
protected

maximum value in x direction

◆ PASS_X_MIN

float datmo::cloud_segmentation::PASS_X_MIN
protected

minimum value in x direction

◆ PASS_Y_MAX

float datmo::cloud_segmentation::PASS_Y_MAX
protected

maximum value in y direction

◆ PASS_Y_MIN

float datmo::cloud_segmentation::PASS_Y_MIN
protected

minimum value in y direction

◆ PASS_Z_MAX

float datmo::cloud_segmentation::PASS_Z_MAX
protected

maximum value in z direction

◆ PASS_Z_MIN

float datmo::cloud_segmentation::PASS_Z_MIN
protected

minimum value in z direction

◆ prev_cloud_transform

tf::StampedTransform datmo::cloud_segmentation::prev_cloud_transform
protected

Container to store transform of previous pointcloud wrt /odom.

◆ pub

ros::Publisher datmo::cloud_segmentation::pub
protected

publish point cloud after transforming it to base_link on topic /transformed_points

◆ pub_2

ros::Publisher datmo::cloud_segmentation::pub_2
protected

Publish pointcloud with dynamic obstacles on topic /dynamic_points.

◆ pub_3

ros::Publisher datmo::cloud_segmentation::pub_3
protected

Publish pointcloud with dynamic obstacles on topic /dynamic_points.

◆ ref_cloud_transform

tf::StampedTransform datmo::cloud_segmentation::ref_cloud_transform
protected

Container to store transform of reference pointcloud wrt /odom.

◆ ref_obj_cloud

pcl::PointCloud<pcl::PointXYZ>:: Ptr datmo::cloud_segmentation::ref_obj_cloud
protected

container to store object cloud to be used as reference for comparing with object cloud at current time step for octree based spatial change detection

◆ SEG_CLUSTER_TOLERANCE

float datmo::cloud_segmentation::SEG_CLUSTER_TOLERANCE
protected

Maximum distance between two points to be included in same cluster.

◆ SEG_MAX_CLUSTER_SIZE

int datmo::cloud_segmentation::SEG_MAX_CLUSTER_SIZE
protected

Maximum number points to be included in same cluster.

◆ SEG_MIN_CLUSTER_SIZE

int datmo::cloud_segmentation::SEG_MIN_CLUSTER_SIZE
protected

Minimum number of points that should be present within a cluster.

◆ sensor_frame_id

string datmo::cloud_segmentation::sensor_frame_id
protected

◆ SOR_MEAN_K

float datmo::cloud_segmentation::SOR_MEAN_K
protected

number of neighbors to analyze for each point

◆ SOR_STD_DEV_MUL_THRESH

float datmo::cloud_segmentation::SOR_STD_DEV_MUL_THRESH
protected

distance in multiples of std dev after which point is considered an outlier

◆ sourceClouds

vector< pcl::PointCloud<pcl::PointXYZ>::Ptr, Eigen::aligned_allocator <pcl::PointCloud<pcl::PointXYZ>::Ptr > > datmo::cloud_segmentation::sourceClouds
protected

vector to store the "OCTREE_WINDOW" number of obj_cloud in the memory

◆ sourceTransforms

vector<tf::StampedTransform> datmo::cloud_segmentation::sourceTransforms
protected

Vector to store corresponding Transforms of each PointCloud in sourceClouds /odom to /base_link.

◆ sub

ros::Subscriber datmo::cloud_segmentation::sub
protected

subscribes sensor_msgs::PointCloud2 from topic /cloud

◆ VOXEL_LEAF_SIZE

float datmo::cloud_segmentation::VOXEL_LEAF_SIZE
protected

The documentation for this class was generated from the following files: