Ground Plane Detection Tutorial

This tutorial will introduce you to a common process for implementing perception algorithms, as well as using logfile data as an algorithm development acceleration tool.

1. Ground plane detection overview

The ground plane detection node is an example of a PolySync perception algorithm.

This node depends on a LiDAR point cloud as an input, so it subscribes to the ps_lidar_points_msg to receive a copy of every LiDAR message that’s seen on the PolySync bus .

For each received message, this node will access the point cloud and ignore any non-ground points using simple filtering .

LiDAR ground points are packaged into a separate ps_lidar_points_msg message, which is re-published to the bus for all other nodes to optionally consume.

studio-ground-plane-visualization

2. Locate the C++ code

You can find the C++ Ground Plane Detection code as part of our public C++ examples repo here.

PolySync-Core-CPP-Examples/GroundPlaneDetection
#include < PolySyncNode.hpp >
#include < PolySyncDataModel.hpp >

using namespace std;

/class GroundPlaneDetection : public polysync::Node
{
private:
    ps_msg_type _messageType;

    std::vector< polysync::datamodel::LidarPoint > _groundPlanePoints;

public:
    / void initStateEvent() override
    {
        _messageType = getMessageTypeByName( "ps_lidar_points_msg" );
        registerListener( _messageType );
    }
    void okStateEvent() override
    {
        polysync::datamodel::LidarPointsMessage groundPlaneMessage ( *this );
        groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() );
        groundPlaneMessage.setPoints( _groundPlanePoints );
        groundPlaneMessage.publish();
        usleep(50);
    }
    / virtual void messageEvent( std::shared_ptr< polysync::Message > message )
    {
        using namespace polysync::datamodel;
        if( std::shared_ptr <LidarPointsMessage > lidarPointsMessage = getSubclass< LidarPointsMessage >( message ) )
        {
           if( lidarPointsMessage->getHeaderSrcGuid() != getGUID() )
            {
                _groundPlanePoints.clear();
                groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() );
                std::vector< polysync::datamodel::LidarPoint > lidarPoints = lidarPointsMessage->getPoints();
                std::vector< polysync::datamodel::LidarPoint > groundPlanePoints;

                std::array< float, 3 > position;

                for( polysync::datamodel::LidarPoint point : lidarPoints )
                {
                    position = point.getPosition();
                    {
                        _groundPlanePoints.push_back( point );
                    }
                }
                //groundPlaneMessage.setPoints( _groundPlanePoints );

              // groundPlaneMessage.publish();

                groundPlanePoints.clear();
                lidarPoints.clear();
            }
        }
    }

    bool pointIsNearTheGround( const std::array< float, 3 > & point )
    {

        return point[0] >= 2.5 and      // x is 2.5+ meters from the vehicle origin
               point[0] < 35 and        // x is less than 35 meters from the vehicle origin
               point[1] > -12 and       // y is greater than -12 meters from the vehicle origin (towards the passenger side)
               point[1] < 12 and        // y is less than 12 meters from the vehicle origin (towards the driver side)
               point[2] > -0.35 and     // z is greater than -0.35 meters from the vehicle origin (towards the ground),

               point[2] < 0.25;         // z is less than 0.25 meters from the vehicle origin
    }

};

/int main()
{
    return 0;
}

2.1 GroundPlaneDetection.cpp explained

The _messageType is used to hold the integer value for the ps_lidar_points_msg.

_groundPlanePoints is used to hold the subset of LiDAR points that represent the ground plane.

private:
    ps_msg_type _messageType;

    std::vector< polysync::datamodel::LidarPoint > _groundPlanePoints;

You will call the following functions─almost always in this order─by a node that’s publishing messages to the bus. After creating an instance of the ps_lidar_points_msg, the payload is set. In this instance the ground plane points vector.

Please note that the message header timestamp is set immediately before the publish message function.

void okStateEvent() override
{
    polysync::datamodel::LidarPointsMessage groundPlaneMessage( *this );

    groundPlaneMessage.setPoints( _groundPlanePoints );

    groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() );

    groundPlaneMessage.publish();

    usleep(50);
}

Within the messageEvent, you will promote the base class message to a LidarPointsMessage. It’s important that the node filters out any message that it publishes to the bus.

Once the node receives a valid, new LiDAR points message it can clear out the existing vector of LiDAR ground points.

if( std::shared_ptr <LidarPointsMessage > lidarPointsMessage = getSubclass< LidarPointsMessage >( message ) )
{
    // Filter out this nodes own messages
    if( lidarPointsMessage->getHeaderSrcGuid() != getGUID() )
    {
        _groundPlanePoints.clear();

Each time the node receives a LiDAR point message, it creates another instance of the LiDAR point message to hold the ground plane points. The original message and ground plane points message will have the same message start/end scan timestamps, which allows other node(s) to correlate the original and ground plane messages for further processing.

This code block shows how to create local containers for the incoming message, and iterate over the message payload in the for loop.

LidarPointsMessage groundPlaneMessage ( *this );

groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() );

// Get the entire LiDAR point cloud from the incoming message
std::vector< polysync::datamodel::LidarPoint > lidarPoints = lidarPointsMessage->getPoints();

// Create a container that will hold all ground plane points that are found in the nodes processing
std::vector< polysync::datamodel::LidarPoint > groundPlanePoints;

// Create a container to hold a single point as the node iterates over the full point cloud
std::array< float, 3 > position;

for( polysync::datamodel::LidarPoint point : lidarPoints )
{
    // Get the x/y/z position for this point in the point cloud
    position = point.getPosition();


    if( pointIsNearTheGround( position ) )
    {
        // This point is close the ground, place it in our point vector
        _groundPlanePoints.push_back( point );
    }
}

Since the incoming data has been transformed to a vehicle centered reference coordinate frame by default, we can use the following simple filtering techniques to determine which points in the LiDAR cloud are ground points.

bool pointIsNearTheGround( const std::array< float, 3 > & point )
{
    // The vehicle origin is at the center of the rear axle, on the ground
    // Incoming LiDAR point messages have been corrected for sensor mount position already


    return point[0] >= 2.5 and      // x is 2.5+ meters from the vehicle origin
           point[0] < 35 and        // x is less than 35 meters from the vehicle origin
           point[1] > -12 and       // y is greater than -12 meters from the vehicle origin (towards the passenger side)
           point[1] < 12 and        // y is less than 12 meters from the vehicle origin (towards the driver side)
           point[2] > -0.35 and     // z is greater than -0.35 meters from the vehicle origin (towards the ground),
                                    // this compensates for vehicle pitch as the vehicle drives
           point[2] < 0.25;         // z is less than 0.25 meters from the vehicle origin
}

3. Build and run ground plane node

You can find the C++ Data Generation code as part of our public C++ examples repo here. You will build the node using cmake and make.

$ cd PolySync-Core-CPP-Examples/GroundPlaneDetection`
$ cmake . && make
$ ./polysync-ground-plane-detection-cpp

The node will quietly wait until LiDAR data is received in the messageEvent.

Since this node requires LiDAR data as an input, it won’t do anything unless LiDAR data─ps_lidar_points_msg─is being published to the bus by another node.

The PolySync development lifecycle uses the powerful replay tools to recreate real-world scenarios on the bench.

Using PolySync Studio, which leverages the Record & Replay API, you can command active nodes to replay data from a logfile and publish high-level messages to the bus. This allows for the development of algorithms that subscribe to messages and process the data contained within the messages in real-time.

4. Replaying data

In order for the ground plane tutorial to act on a data set, you need to replay data either from the command line or through Studio. You can also connect to sensor hardware and run the application against live data.

5. Visualizing data

The data can be visualized in Studio’s 3D View and the OpenGL based Viewer Lite example node.

To view in studio, open the 3D View plugin from the Studio plugin launcher. Once open if you are replaying data, LiDAR points should appear in the 3D View. The ground plane LiDAR points will appear in a different color in the 3D view, highlighting their difference from the other LiDAR points.

studio-ground-plane-visualization

5.1 Viewer Lite

Viewer Lite is an OpenGL 2D visualizer node that allows us to see primitive data types (LiDAR points, objects, and RADAR targets) being transmitted using the PolySync bus and messages.

Viewer Lite is an example written in C.

In another terminal download, build, and start the viewer lite application.

$ git clone git@github.com:PolySync/PolySync-Core-C-Examples.git
$ cd PolySync-Core-C-Examples/viewer_lite
$ sudo apt-get install libglib2.0-dev freeglut3-dev  # this is required for this example to build, documented in the examples README.md file
$ make
$ ./bin/polysync-viewer-lite

Once launched, when data is replayed from the command line or from Studio, data will appear in the Viewer Lite application.

viewer-lite-ground-plane-visualization

Conclusion

Congratulations! You have successfully implemented perception algorithms and walked through using logfile data as an algorithm development acceleration tool.