For testing of my PCL client code, I generated an artificial set of points that all lie on a plane (something like a,b,c,d = 1,1,-1,0 for the plane model a*x+b*y+c*z+d=0). After fitting the plane model I'd expect anything that is linear dependent to above stated coefficients.
The range (and size) of the point cloud is determined by a parameter n I provide to the program. And this value seems to have an influence on the result of the fit.
I do receive the expected coefficients from the RANSAC segmentation for small values of n, but going to larger values somehow an error builds up.
Example output:
$ ./a.out 240
point cloud contains 230400 points
a,b,c,d = -0.577313, -0.577376, 0.577362, 1.77026e-05,
$ ./a.out 280
point cloud contains 313600 points
a,b,c,d = -0.577325, -0.577396, 0.57733, 0.142751,
$ ./a.out 320
point cloud contains 409600 points
a,b,c,d = 0.577364, 0.577353, -0.577334, -0.202786,
$ ./a.out 1000
point cloud contains 4000000 points
a,b,c,d = -0.578332, -0.577762, 0.575954, 0.276675,
$ ./a.out 5000
point cloud contains 100000000 points
a,b,c,d = 0.35787, 0.718142, -0.596826, -162.683,
As you can see, especially the d term seems to grow with the size of the point cloud.
Why could this be? Can I do anything to fix this?
Here is my code:
#include <iostream>
#include <string>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
int main(int argc, char** argv) {
// generate point cloud
double n = 10;
if (argc > 1) {
n = std::stod(argv[1]);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
for (double i = -n; i < n; i+=1.) {
for (double j = -n; j < n; j+=1.) {
pcl::PointXYZ point;
// coefficients should be something like: a, a, -a, 0
// a*x + b*y + c*z + d = 0
point.x = i; point.y = j; point.z = i + j;
plane->points.push_back(point);
}
}
std::cout << "point cloud contains " << plane->points.size() << " points\n";
// fit plane
// as seen here:
// https://pcl.readthedocs.io/projects/tutorials/en/latest/planar_segmentation.html
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(plane);
seg.segment(*inliers, *coefficients);
// print coefficients
std::cout << "a,b,c,d = ";
for (auto c : coefficients->values) std::cout << c << ", ";
std::cout << "\n";
}
This is how I build:
$ g++ test.cc `pkg-config --cflags --libs pcl_segmentation-1.8`
This should be standard PCL from the Ubuntu 18.04 repository (libpcl1.8-dev).
Same results with Ubuntu 20.04:
$ g++ test.cc `pkg-config --cflags --libs pcl_segmentation-1.10 eigen3`
And Ubuntu 22.04:
$ g++ test.cc `pkg-config --cflags --libs pcl_segmentation-1.12 eigen3`
Despite it's "random"-ness, for a cloud containing a plane with no outliers and no noise whatsoever, plane segmentation should always succeed in this case (any 3 non-colinear points will give you the correct plane).
Story short - by setting seg.setOptimizeCoefficients(false); we get the correct coefficients for n=5000 even with seg.setMaxIterations(1);.
The problem moooeeeep encountered is related to a numerical instability issues with the computation of covariance matrix in the optional model coefficient optimization step. Please refer to Issue 3041 for more details. Clarification - the issue is not the "large" coordinates, but rather the large ratio between the cloud size (bounding box) and the resolution (distance between near points). In other words, rescaling the cloud won't help.
In the case of a noisy cloud, the coefficients you get without the final optimization might not be accurate enough. If that is the case, and you do require the coefficient optimization step, your other options is to do the calculation with double coordinates. Either:
float to double.double).