Plane Detection Overview

Introduction

With the 3D information of the environment the ZED camera can estimate the position of the planes present in the scene.

To detect a plane the position tracking needs to be enabled with zed.enablePositionalTracking(). Also a plane can be detected only if the tracking state of the camera is OK.

Therefore the procedure for detecting a plane is as follow:

Enable the positional tracking module
Grab an image from the ZED
Check that the tracking state is OK
Estimate a plane position.

Detecting Planes

To estimate a plane position, you will need to use the findPlaneAtHit function and pass the 2D coordinates of a target pixel where you want to find its support plane. Below is an example of code that can be used to estimate the plane passing by the depth value of pixel coord = (u,v):

C++
Python
C#

sl::Plane plane;
sl::uint2 coord; // Fill it with the coordinates taken from the full size image  
while(zed.grab() == ERROR_CODE::SUCCESS) {
  tracking_state = zed.getPosition(pose); // Get the tracking state of the camera
  if(tracking_state == TRACKING_STATE::OK) {  
    // Detect the plane passing by the depth value of pixel coord
    find_plane_status = zed.findPlaneAtHit(coord, plane);
  }
}

plane = sl.Plane() # Structure that stores the estimated plane
coord = sl.uint2() # Fill it with the coordinates taken from the full size image
while zed.grab() == sl.ERROR_CODE.SUCCESS :
  tracking_state = zed.get_position(pose) # Get the tracking state of the camera
  if tracking_state == sl.TRACKING_STATE.OK) :  
    # Detect the plane passing by the depth value of pixel coord
    find_plane_status = zed.find_plane_at_hit(coord, plane)

PlaneData plane = new sl.PlaneData();
Vector2 coord = new Vector2();
sl.RuntimeParameters runtimeParameters = new sl.RuntimeParameters();
while (zed.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS){
    sl.Pose pose = new sl.Pose();
    sl.POSITIONAL_TRACKING_STATE tracking_state = zed.GetPosition(ref pose);
    if (tracking_state == sl.POSITIONAL_TRACKING_STATE.OK)
    {
       sl.ERROR_CODE e = zed.findPlaneAtHit(ref plane, coord);
    }
}

If it succeeds the function stores the detected plane in a sl::Plane object, which includes useful information such as 3D position, normal, polygon boundaries and plane type (vertical / horizontal).

Accessing Plane Data

The sl::Plane class contains all the information for defining the plane in space such as normal, center and equation. To access this information use the getter of the class. For example:

C++
Python
C#

if(find_plane_status == ERROR_CODE::SUCCESS){
  sl::float3 normal = plane.getNormal(); // Get the normal vector of the detected plane
  sl::float4 plane_equation = plane.getPlaneEquation(); // Get (a,b,c,d) where ax+by+cz=d
}

if find_plane_status == sl.ERROR_CODE.SUCCESS :
  normal = plane.get_normal() # Get the normal vector of the detected plane
  plane_equation = plane.get_plane_equation() # Get (a,b,c,d) where ax+by+cz=d

if(find_plane_status == ERROR_CODE.SUCCESS){
  Vector3 normal = plane.PlaneNormal;// Get the normal vector of the detected plane
  Vector4 plane_equation = plane.PlaneEquation;// Get (a,b,c,d) where ax+by+cz=d
}

For AR purposes it can be useful to get the Transform of the detected plane according to the global reference frame. For example to place an object close to a wall or to transform the global reference frame center to the plane center. This transform is accessible as follow:

C++
Python
C#

if(find_plane_status == ERROR_CODE::SUCCESS){
  // Get the transform of the plane according to the global reference frame
  sl::Transform plane_transform = plane.getPose();
}

if find_plane_status == sl.ERROR_CODE.SUCCESS :
  # Get the transform of the plane according to the global reference frame
  plane_transform = sl.Transform()
  plane_equation = plane.get_transform()

if(find_plane_status == ERROR_CODE.SUCCESS){
  // Get the transform of the plane according to the global reference frame
  public Quaternion rotation;
  public Vector3 translation;
  translation = plane.PlaneTransformPosition;
  rotation = plane.PlaneTransformOrientation;
}

Convert Plane to Mesh

The detected plane can be converted to a Mesh. Converting it to a mesh can be useful when highlighting a plane in the scene. To get the mesh of the plane, call:

C++
Python
C#

 sl::Mesh mesh = plane.extractMesh();

mesh = sl.Mesh()
mesh = plane.extract_mesh()

Vector3[] planeMeshVertices = new Vector3[65000];
int[] planeMeshTriangles = new int[65000];
int numVertices = 0;
int numTriangles = 0;
zed.convertFloorPlaneToMesh(planeMeshVertices, planeMeshTriangles, out numVertices, out numTriangles);

Detecting Floor Plane

With the 3D information of the environment, ZED cameras can estimate where is the ground floor in a scene.

Getting Floor Plane

The floor plane can be automatically detected by calling findFloorPlane instead of findPlaneAtHit. Apart from storing the plane in a sl::Plane, this function will also store the transform between the floor plane frame and the camera frame.

This transform can be used to reset the tracking and align it with the floor plane frame as follow:

C++
Python
C#

sl::Plane plane;
sl::Transform resetTrackingFloorFrame;
find_plane_status = zed.findFloorPlane(plane, resetTrackingFloorFrame);
if(find_plane_status == ERROR_CODE::SUCCESS){
  // Reset positional tracking to align it with the floor plane frame
  zed.resetPositionalTracking(resetTrackingFloorFrame);
}

plane = sl.Plane()
resetTrackingFloorFrame = sl.Transform()
find_plane_status = zed.find_floor_plane(plane, resetTrackingFloorFrame)
if find_plane_status == sl.ERROR_CODE.SUCCESS :
  # Reset positional tracking to align it with the floor plane frame
  zed.reset_positional_tracking(resetTrackingFloorFrame)

PlaneData plane = new PlaneData();
float playerHeight = 0;
Quaternion priorQuat = Quaternion.Identity;
Vector3 priorVec = Vector3.Zero;
ERROR_CODE find_plane_status = zedCamera.findFloorPlane(ref plane, out playerHeight, priorQuat, priorVec);
if (find_plane_status == ERROR_CODE.SUCCESS){
    // Reset positional tracking to align it with the floor plane frame
    zed.ResetPositionalTracking(plane.PlaneTransformOrientation, plane.PlaneTransformPosition);
}

This code can be simplified by using PositionalTrackingParameters::set_floor_as_origin to aligned the positional tracking reference frame on the ground floor.

C++
Python
C#

PositionalTrackingParameters positional_tracking_parameters;
positional_tracking_parameters.set_floor_as_origin = true;
zed.enablePositionalTracking(positional_tracking_parameters);

RuntimeParameters runtime_parameters;
runtime_parameters.measure3D_reference_frame = REFERENCE_FRAME::WORLD;
sl::Mat cloud;
while(zed.grab(runtime_parameters) == ERROR_CODE::SUCCESS) {
  zed.retrieveMeasure(cloud, MEASURE::XYZRGBA);
  // The point cloud is aligned on the floor plane.
  // A threshold on the height could then be used as a simple object detection method
}

positional_tracking_parameters = sl.PositionalTrackingParameters()
positional_tracking_parameters.set_floor_as_origin = True
zed.enable_positional_tracking(positional_tracking_parameters)

runtime_parameters = sl.RuntimeParameters()
runtime_parameters.measure3D_reference_frame = sl.REFERENCE_FRAME::WORLD
cloud = sl.Mat()

while zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS :
  zed.retrieveMeasure(cloud, sl.MEASURE::XYZRGBA)
  # The point cloud is aligned on the floor plane.
  # A threshold on the height could then be used as a simple object detection method

PositionalTrackingParameters trackingParams = new PositionalTrackingParameters();
trackingParams.setFloorAsOrigin = true;
zed.EnablePositionalTracking(ref trackingParams);

RuntimeParameters runtimeParameters = new RuntimeParameters();
runtimeParameters.measure3DReferenceFrame = REFERENCE_FRAME.WORLD;
Mat cloud = new Mat();
while(zed.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS)
{
    zed.RetrieveMeasure(cloud, MEASURE.XYZRGBA);
    // The point cloud is aligned on the floor plane.
    // A threshold on the height could then be used as a simple object detection method
}

The estimation of the floor plane can be refined by passing prior parameters (such as prior height or orientation) to the findFloorPlane function. For additional details on these parameters, please check out the API Reference.