Introduction

Road detection plays an integral role in self-driving cars. Accuracy and reliable road detection can pave the road for good path planning. In the self-driving golf cart project, I use two methods to perform road detection. 1. I use semantic segmentation and deep learning to classify each pixel in an image. 2. I use the ZED vision system, which can construct a point cloud of the space in front of the car, to detect drivable space.

Background Information

Just in case if you are not familiar with the self-driving golf-cart project, please checkout my Github page, and the project website.

This article assumes that you have some basic knowledge of ROS, such as subscribers, publishers, topics, and callbacks. You can learn more about ROS from their official website.

Semantic segmentation from the CityScape dataset

The two road detection systems are meant to compliment each other. The semantic segmentation network is excellent under ideal conditions: no shadows and glares. Deep learning tends to be more generalization than logic based algorithms. However, the ZED camera maintains decent performance even under tricky conditions. Shadows and overcast shouldn’t effect the performance of the sensor.

ZED camera mapping

With a logic based algorithm and a deep learning model, the golf cart should be able to detect the road reliably, and then perform path planning.

What’s ZED

The ZED sensor

Unlike a simple webcam, the ZED sensor has two cameras. Similar to the human eyes’ ability to judge distance, the ZED sensor not only captures RGB photos but also distance readings.

A stereo camera is a type of camera with two or more lenses with a separate image sensor or film frame for each lens. The two-lens setup gives the camera the ability to capture three-dimensional images, a process known as stereo photography. Stereo cameras may be used for making stereoviews and 3D pictures for movies, or for range imaging. According to Wikipedia, “the distance between the lenses in a typical stereo camera (the intra-axial distance) is about the distance between one’s eyes (known as the intra-ocular distance) and is about 6.35 cm, though a longer baseline produces more extreme 3-dimensionality.”

With the ability to capture three-dimensional images, the ZED sensor can create a point cloud of the surroundings. Point clouds are critical in self-driving cars. They are essentially the real-time 3D mapping of the vehicle’s surroundings. We can perform many types of algorithms to achieve lots of tasks, by simply using point cloud data. Here is an example of a point cloud captured by ZED:

ZED point cloud, sidewalk

You might be wondering why not choose a 3D-LIDAR? Stereo cameras have a few advantages.

  1. They are pretty cheap. A ZED camera cost around 450 USD, a LIDAR cost around 7000 USD
  2. The ZED camera is versatile. It captures RGB, depth map, and IMU data.
  3. It’s relatively easy to integrate into my current ROS system.

Now we have learned more about stereo cameras, let’s dive into the self-driving car software system, and see how it can detect road with point cloud data.

Point Cloud Library (PCL)

The Point Cloud Library is an open-source library of algorithms for point cloud processing tasks and 3D geometry processing. The library contains algorithms for feature estimation, surface reconstruction, 3D registration, model fitting, and segmentation. From lidars to stereo cameras, PCL provides useful algorithms for rapid prototyping and research. If you have used OpenCV, the concept behind PCL might sound very familiar. I take advantage of the hundreds of built-in algorithms to process the point cloud data captured by the ZED camera.

Road Detection with PCL

In my road detection system, I use two features of PCL. First, I use the PCL transform and ROS tf to transform the point cloud based on the motion of the vehicle. As the vehicle moves, the point cloud’s position will stay the constant, relative to the camera. The source code of the transform node can be found here.

Second, I use IntegralImageNormalEstimation, GroundPlaneComparator, and OrganizedConnectedComponentSegmentation to detect drivable space. Here is how I initialized those global variables:

pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
pcl::GroundPlaneComparator<PointT, pcl::Normal>::Ptr road_comparator(new pcl::GroundPlaneComparator<PointT, pcl::Normal>);
pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label> road_segmentation (road_comparator);

After we have initialized the normal estimator, the road comparator, and road_segmentation, let’s take a look at the ROS point cloud call back. The code below might look daunting, don’t worry, I will try to break it down into more manageable pieces. 

void cloud_callback_new (const sensor_msgs::PointCloud2ConstPtr& input){

    // Create a container for the data. --------------------------------------------------------------------------------
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*input, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2, *cloud);

    // Compute the normals
    pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
    ne.setInputCloud (cloud);
    ne.compute (*normal_cloud);

    // Set up the groundplane comparator
    road_comparator->setInputCloud (cloud);
    road_comparator->setInputNormals (normal_cloud);

    // Run segmentation
    pcl::PointCloud<pcl::Label> labels;
    std::vector<pcl::PointIndices> region_indices;
    road_segmentation.setInputCloud (cloud);
    road_segmentation.segment (labels, region_indices);

    // Draw the segmentation result
    pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr ground_image (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr label_image (new pcl::PointCloud<pcl::PointXYZRGB>);
    *ground_image = *cloud;
    *label_image = *cloud;

    Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
    Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
    Eigen::Matrix3f clust_cov;
    pcl::ModelCoefficients model;
    model.values.resize (4);

    std::vector<pcl::ModelCoefficients> model_coefficients;
    std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
    std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
    std::vector<pcl::PointIndices> inlier_indices;

    for (const auto &region_index : region_indices){

        if (region_index.indices.size () > 1000){

            for (size_t j = 0; j < region_index.indices.size (); j++){

                pcl::PointXYZ ground_pt (cloud->points[region_index.indices[j]].x,
                                         cloud->points[region_index.indices[j]].y,
                                         cloud->points[region_index.indices[j]].z);
                ground_cloud->points.push_back (ground_pt);
                ground_image->points[region_index.indices[j]].g = static_cast<pcl::uint8_t> ((cloud->points[region_index.indices[j]].g + 255) / 2);
                label_image->points[region_index.indices[j]].r = 0;
                label_image->points[region_index.indices[j]].g = 255;
                label_image->points[region_index.indices[j]].b = 0;
            }

            // Compute plane info
            pcl::computeMeanAndCovarianceMatrix (*cloud, region_index.indices, clust_cov, clust_centroid);
            Eigen::Vector4f plane_params;

            EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
            EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
            pcl::eigen33 (clust_cov, eigen_value, eigen_vector);
            plane_params[0] = eigen_vector[0];
            plane_params[1] = eigen_vector[1];
            plane_params[2] = eigen_vector[2];
            plane_params[3] = 0;
            plane_params[3] = -1 * plane_params.dot (clust_centroid);

            vp -= clust_centroid;
            float cos_theta = vp.dot (plane_params);
            if (cos_theta < 0){

                plane_params *= -1;
                plane_params[3] = 0;
                plane_params[3] = -1 * plane_params.dot (clust_centroid);
            }

            model.values[0] = plane_params[0];
            model.values[1] = plane_params[1];
            model.values[2] = plane_params[2];
            model.values[3] = plane_params[3];
            model_coefficients.push_back (model);
            inlier_indices.push_back (region_index);
            centroids.push_back (clust_centroid);
            covariances.push_back (clust_cov);
        }
    }

    //Refinement
    std::vector<bool> grow_labels;
    std::vector<int> label_to_model;
    grow_labels.resize (region_indices.size (), false);
    label_to_model.resize (region_indices.size (), 0);

    for (size_t i = 0; i < model_coefficients.size (); i++){

        int model_label = (labels)[inlier_indices[i].indices[0]].label;
        label_to_model[model_label] = static_cast<int> (i);
        grow_labels[model_label] = true;
    }

    boost::shared_ptr<pcl::PointCloud<pcl::Label> > labels_ptr (new pcl::PointCloud<pcl::Label>());
    *labels_ptr = labels;
    pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
    pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>());
    refinement_compare->setInputCloud (cloud);
    refinement_compare->setDistanceThreshold (0.15f);
    refinement_compare->setLabels (labels_ptr);
    refinement_compare->setModelCoefficients (model_coefficients);
    refinement_compare->setRefineLabels (grow_labels);
    refinement_compare->setLabelToModel (label_to_model);
    mps.setRefinementComparator (refinement_compare);
    mps.setMinInliers (500);
    mps.setAngularThreshold (pcl::deg2rad (5.0));
    mps.setDistanceThreshold (0.01);
    mps.setInputCloud (cloud);
    mps.setInputNormals (normal_cloud);
    mps.refine (model_coefficients, inlier_indices, centroids, covariances, labels_ptr, region_indices);

    //Note the regions that have been extended
    pcl::PointCloud<PointT> extended_ground_cloud;
    for (const auto &region_index : region_indices){

        if (region_index.indices.size () > 1000){

            for (size_t j = 0; j < region_index.indices.size (); j++){

                // Check to see if it has already been labeled
                if (ground_image->points[region_index.indices[j]].g == ground_image->points[region_index.indices[j]].b){

                    pcl::PointXYZ ground_pt (cloud->points[region_index.indices[j]].x,
                                             cloud->points[region_index.indices[j]].y,
                                             cloud->points[region_index.indices[j]].z);
                    ground_cloud->points.push_back (ground_pt);
                    ground_image->points[region_index.indices[j]].r = static_cast<pcl::uint8_t> ((cloud->points[region_index.indices[j]].r + 255) / 2);
                    ground_image->points[region_index.indices[j]].g = static_cast<pcl::uint8_t> ((cloud->points[region_index.indices[j]].g + 255) / 2);
                    label_image->points[region_index.indices[j]].r = 128;
                    label_image->points[region_index.indices[j]].g = 128;
                    label_image->points[region_index.indices[j]].b = 0;
                }

            }
        }
    }

    // publish filtered cloud data
    sensor_msgs::PointCloud2 cloud_publish;
    pcl::toROSMsg(*label_image, cloud_publish);
    cloud_publish.header = input->header;

    pub.publish(cloud_publish);
}

First, we need to convert the ROS PointCloud2 message to a PCL point cloud. And then, let’s run a normal vector estimation algorithm, and then pass the results into the segmentation object. 

// convert the PointCloud2 msg to PCL.
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*input, pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2, *cloud);

// Compute the normals
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (cloud);
ne.compute (*normal_cloud);

// Set up the groundplane comparator
road_comparator->setInputCloud (cloud);
road_comparator->setInputNormals (normal_cloud);

// Run segmentation
pcl::PointCloud<pcl::Label> labels;
std::vector<pcl::PointIndices> region_indices;
road_segmentation.setInputCloud (cloud);
road_segmentation.segment (labels, region_indices);

The next segment of code is for refinement and visualization purposes. This piece of code uses OrganizedMultiPlaneSegmentation. The MPS takes in multiple parameter and refines the segmentation results. 

    boost::shared_ptr<pcl::PointCloud<pcl::Label> > labels_ptr (new pcl::PointCloud<pcl::Label>());
    *labels_ptr = labels;
    pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
    pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>());
    refinement_compare->setInputCloud (cloud);
    refinement_compare->setDistanceThreshold (0.15f);
    refinement_compare->setLabels (labels_ptr);
    refinement_compare->setModelCoefficients (model_coefficients);
    refinement_compare->setRefineLabels (grow_labels);
    refinement_compare->setLabelToModel (label_to_model);
    mps.setRefinementComparator (refinement_compare);
    mps.setMinInliers (500);
    mps.setAngularThreshold (pcl::deg2rad (5.0));
    mps.setDistanceThreshold (0.01);
    mps.setInputCloud (cloud);
    mps.setInputNormals (normal_cloud);
    mps.refine (model_coefficients, inlier_indices, centroids, covariances, labels_ptr, region_indices);

Last but not least, I publish the ground detection result using a ROS publisher. 

    // publish filtered cloud data
    sensor_msgs::PointCloud2 cloud_publish;
    pcl::toROSMsg(*label_image, cloud_publish);
    cloud_publish.header = input->header;

    pub.publish(cloud_publish);

Conclusion

When I run the ROS node described in this article, here is the result.

This is the point cloud before detection.
The black part of the point cloud is marked as void. The yellow segment is considered likely ground, and the green segment is most likely ground.
Viewing the point cloud from another angle.

So that’s it!

This is an overview of my ground segmentation system with PCL and ROS. In the next blog, I will discuss now I segmented and labeled a point cloud using the RGB semantic segmentation results. If you have any questions, comments, or concerns, feel free to reach out to me at contact@neilnie.com. As always, thanks for checking out by blog.

If you are interested in learning more about the self-driving golf cart project, you might enjoy the following posts.

  1. Deep learning steering prediction
    1. Visualizing the Steering Model with Attention Maps
    2. Successfully Tested the Autonomous Steering System for the Self-Driving Golf Cart
    3. Predicting Steering Angle with Deep Learning — Part 2
    4. Predicting Steering Angle with Deep Learning — Part 1
  2. Semantic segmentation
    1. The Robustness of the Semantic Segmentation Network
    2. Autonomous Cruise Control System
    3. Understanding the World Through Semantic Segmentation
  3. Robot Operating System
    1. Hello, ROS
    2. Open Street Map with ROS
    3. Self-Driving Software + Carla Simulator
    4. GPS Localization with ROS, rviz, and OSM
Posted by:NeilNie

Student at Columbia University, School of Engineering and Applied Sciences. Prev. software engineer intern at Apple. More than six years of experience developing iOS and macOS applications. Experienced in electrical engineering/microcontrollers. From publishing several apps to presented a TEDx talk in machine learning. Striving to use my knowledge, skills, and passion to positively impact the world.

One thought on “Road Detection with Point Cloud Library and ZED Stereoscopic Vision System”

  1. Hi Neil,
    I have a task for a project to detect the road with the PCL, and we have a dataset from a Lidar. I’m just wondering if it would be possible to use this piece of code to do this.
    And also if you have your dataset, that I can check step by step how this code works, would help me a lot 🙂

    Thank You very much in advance.

    Sincerely,
    Vedran

    Like

    Reply

Leave a comment

This site uses Akismet to reduce spam. Learn how your comment data is processed.