SACSegmentation fails when the pointcloud is too far from origin

[nolemocius]

Environment

• Operating System and version: Xubuntu and Ubuntu 16.04
• Compiler: system default
• PCL Version: 1.7.2-14 (libpcl-dev installed from apt-get)

Context

When I try to segment ground plane from a lidar frame already transformed to global coordinates, the result is erroneous and erratical when the sensor is far from map origins.

Steps to reproduce:

• get a pointcloud with a clear near-horizontal dominant plane, such as a frame from a 3D lidar sensor on a vehicle;
• move it so that it is positioned several its sizes far from the frame origin. Say, if the size of pointcloud is 70x70x10, let its center be at (1000, 500, 400)
• perform SACSegmentation or SACSegmentationFromNormals on the cloud with SACModel: Plane and DistanceThreshold of 0.1-0.3
• watch the resulting plane coefficients and inliers

Expected Behavior

Expected to obtain coefficients for the biggest planar segment in the pointcloud

Current Behavior

Results oscillate wildly when tested on many similar pointclouds. The planes are often found to be vertical, inliers being a narrow strip of points cutting along the real principal plane.

Way to fix

Do a coordinates transform so that coordinates are relative to some point in the vicinity of the pointcloud. If the pointcloud undergoes a reference frame translation to an origin somewhere near the cloud points, the segmentation works as expected, correctly identifying the ground plane.

Possible Solution

Include a routine in the segmentation procedure that recalculates the coordinates of input point cloud relative to its min/max/center point before the segmentation. After the segmentation, recalculate the coefficients to the original coordinates.

Remarks

Seeing this issue: #1842 I can suppose that other functions and routines in PCL may work incorrectly when the pointcloud is far from the coordinates system origin. This might depend on the actual algos used.

[SergioRAgostinho]

Read into #2403 for an extensive discussion on the topic. The solution proposed is in line with what you suggested. We just need someone to actually implement it, as the maintainer efforts are now focused on doing the c++14 transition for the next release.

[taketwo]

As long as the point coordinates can be stored in float without loss of significant digits, I think there is nothing in the basic SAC plane estimation that is affected by the shift from the origin. What is affected though is the (optional) model coefficient optimization. It involves covariance matrix computation and thus the issue Sergio referenced kicks in. So I would say one additional quick fix option for you would be to disable optimization.

Sidenote: @SergioRAgostinho I have a student who will implement the fix discussed in #2403. I expect to start a more specific discussion in next couple of weeks to figure out exact location and/or interface changes required for the fix.

[stale]

Marking this as stale due to 30 days of inactivity. It will be closed in 7 days if no further activity occurs.

[kunaltyagi]

Related to #3169 #1407

[mvieth]

Fixed by pull request #4983