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

viewer-lite-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

2.1 GroundPlaneDetection.cpp

#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.2 GroundPlaneDetection.cpp explained

As we saw above, 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 to begin replaying data, you will need to ensure the network is setup by setting the PolySync IP address to the loopback interface. Next, you will open the SDF Configurator to make sure the network configuration is valid. If a mismatch is detected, the SDF Configurator will open a host setup wizard .

$ polysync-core-manager -s 127.0.0.1
$ polysync-core-sdf-configurator

With a valid host configuration, you can instruct the PolySync manager to spawn all nodes that are defined on this host and enabled in the SDF.

$ polysync-core-manager -n -w

Now is the time to start PolySync Studio in order to visualize the runtime status of our nodes, and eventually to visualize the LiDAR point data. The System Hierarchy plugin can be opened from the plugin launcher on the right-hand panel to ensure all nodes are in the “OK” state.

$ polysync-core-studio

Next we select the replay tab from the right-hand panel, and select a replay session for playback by double-clicking. The default session is 1000.

The play button becomes solid black when nodes are ready for playback.

5. Visualizing data

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

Open the 3D View plugin from the plugin launcher.

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

Conclusion

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