search

PointCloudLibrary / pcl

Point Cloud Library (PCL)
https://pointclouds.org/
Other
10.01k stars 4.61k forks source link

[sample_consensus] PR of a pcl::SampleConsensusModelEllipse3D<PointT> Class #5431

Open mnobrecastro opened 2 years ago

mnobrecastro commented 2 years ago

Feature description

Good afternoon everyone!

Here is a proposal for an Ellipse3D sac_model for the sample_consensus module that I wrote more than a year ago for a paper (soon to be submitted).

The proposed model is inspired on the pcl::SACMODEL_CIRCLE3D model that is available, which scripts were used as template. The idea behind the pcl::SACMODEL_ELLIPSE3D is to generalize the former, allowing PCL users to fit ellipses to 3D data. The model could be used, for example, to fit the elliptical data of a laser scanner "cutting" obliquely through a cylindrical object - more details below.

Context

The research project where this code was developed aimed at simulating multiple laser scans from an Intel Realsense camera, to minimize the amount of data points required to reconstruct objects that can be approximated by know 3D geometric primitives such as cylinders, spheres or cuboids. This was applied to a robotic hand prosthesis for amputees such that it helped automate the control of the prosthesis and ease the "work" done by the user. In the case of cylindrical objects, as it is illustrated in the figure below, an oblique laser scan "cut" through an upright cylindrical object (e.g., a can) produces elliptical contours. To that end, having a 3D ellipse model enables to run RANSAC fitting of those data points.

image

The steps required for computing an ellipse from the data points are described in mnobrecastro/pcl-ellipse-fitting (still needs an update soon!), which are based on a research paper . The authors propose a method that solves a generalized eigenvalue problem of the type Av=λBv that allows to retrieve the conic parameters of an ellipse. To solve that system, the GeneralizedEigenSolver of the Eigen is used, and some conflicts between Eigen and PCL were resolved before (issue #4734, PR #4742). At each RANSAC iteration, six data points are used to solve this system of equations.

There maybe certainly more efficient ways to solve this ellipse fitting problem, but I am sure that this pcl::SACMODEL_ELLIPSE3D model will help the community to have something to work on! I appreciate your understanding. A PR #5430 has been sent.

Solution usage and behavior

pcl::SACMODEL_ELLIPSE3D is used to determine 3D ellipses in a plane. The ellipse's eleven coefficients are given by its center, semi-minor/major axis lengths along its local u- and v-axis (two radii), normal and global coordinates of its local u-axis as: [center.x, center.y, center.z, semi_minor_major.u, semi_minor_major.v, normal.x, normal.y, normal.z, u.x, u.y, u.z]. 'sample_consensus/include/pcl/sample_consensus/model_types.h'.

It's usage follow the same kind of segmentation object declaration as in pcl::SACMODEL_CIRCLE3D where the setRadiusLimits(rmin, rmax) method constrains simultaneously both semi-minor/major axes length (radii) of the ellipse:

pcl::ModelCoefficients::Ptr coefs(new pcl::ModelCoefficients);
// Initialization of the 11-element Ellipse3D coefficients
coefficients->values.push_back(0.0); //0 par_cx
coefficients->values.push_back(0.0); //1 par_cy
coefficients->values.push_back(0.0); //2 par_cz
coefficients->values.push_back(0.0); //3 par_a
coefficients->values.push_back(0.0); //4 par_b
coefficients->values.push_back(0.0); //5 par_nx
coefficients->values.push_back(0.0); //6 par_ny
coefficients->values.push_back(0.0); //7 par_nz
coefficients->values.push_back(0.0); //8 par_ux
coefficients->values.push_back(0.0); //9 par_uy
coefficients->values.push_back(0.0); //10 par_uz
[...]
// Create a segmentation object
pcl::SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_ELLIPSE3D);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.001);
seg.setRadiusLimits(0.005, 0.050);
seg.setInputCloud (cloud);

A unit-test was added to 'pcl/test/sample_consensus/test_sample_consensus_quadric_models.cpp' as proposed below.

TEST(SampleConsensusModelEllipse3D, RANSAC)
{
  srand(0);

  // Using a custom point cloud on a tilted plane
  PointCloud<PointXYZ> cloud;
  cloud.resize(22);

  cloud[ 0].getVector3fMap() << 0.9996, 5.0000, 2.9996;
  cloud[ 1].getVector3fMap() << 0.6916, 5.0000, 2.9022;
  cloud[ 2].getVector3fMap() << 0.4124, 5.0000, 2.6181;
  cloud[ 3].getVector3fMap() << 0.1909, 5.0000, 2.1757;
  cloud[ 4].getVector3fMap() << 0.0485, 5.0000, 1.6177;
  cloud[ 5].getVector3fMap() << -0.0007, 5.0000, 0.9998;
  cloud[ 6].getVector3fMap() << 0.0485, 5.0000, 0.3818;
  cloud[ 7].getVector3fMap() << 0.1903, 5.0000, -0.1746;
  cloud[ 8].getVector3fMap() << 0.4118, 5.0000, -0.6189;
  cloud[ 9].getVector3fMap() << 0.6902, 5.0000, -0.9023;
  cloud[10].getVector3fMap() << 1.0007, 5.0000, -0.9994;
  cloud[11].getVector3fMap() << 1.3100, 5.0000, -0.9019;
  cloud[12].getVector3fMap() << 1.5872, 5.0000, -0.6184;
  cloud[13].getVector3fMap() << 1.8094, 5.0000, -0.1748;
  cloud[14].getVector3fMap() << 1.9501, 5.0000, 0.3812;
  cloud[15].getVector3fMap() << 2.0002, 5.0000, 0.9996;
  cloud[16].getVector3fMap() << 1.9514, 5.0000, 1.6188;
  cloud[17].getVector3fMap() << 1.8081, 5.0000, 2.1759;
  cloud[18].getVector3fMap() << 1.5868, 5.0000, 2.6184;
  cloud[19].getVector3fMap() << 1.3083, 5.0000, 2.9021;

  cloud[20].getVector3fMap() << 0.85000002f, 4.8499999f, -3.1500001f;
  cloud[21].getVector3fMap() << 1.15000000f, 5.1500001f, -2.8499999f;

  // Create a shared 3d circle model pointer directly
  SampleConsensusModelEllipse3DPtr model(
      new SampleConsensusModelEllipse3D<PointXYZ>(cloud.makeShared()));

  // Create the RANSAC object
  RandomSampleConsensus<PointXYZ> sac(model, 0.03);

  // Algorithm tests
  bool result = sac.computeModel();
  ASSERT_TRUE(result);

  std::vector<int> sample;
  sac.getModel(sample);
  EXPECT_EQ(6, sample.size());

  std::vector<int> inliers;
  sac.getInliers(inliers);
  EXPECT_EQ(20, inliers.size());

  Eigen::VectorXf coeff;
  sac.getModelCoefficients(coeff);
  EXPECT_EQ(11, coeff.size());
  EXPECT_NEAR(1.0, coeff[0], 1e-3);
  EXPECT_NEAR(5.0, coeff[1], 1e-3);
  EXPECT_NEAR(1.0, coeff[2], 1e-3);

  EXPECT_NEAR(1.0, coeff[3], 1e-3);
  EXPECT_NEAR(2.0, coeff[4], 1e-3);

  EXPECT_NEAR(0.0, coeff[5], 1e-3);
  EXPECT_NEAR(1.0, coeff[6], 1e-3);
  EXPECT_NEAR(0.0, coeff[7], 1e-3);

  EXPECT_NEAR(0.999, coeff[8], 1e-3); // ~=1.0
  EXPECT_NEAR(0.0, coeff[9], 1e-3);
  EXPECT_NEAR(-0.0284, coeff[10], 1e-3); // ~=0.0

  Eigen::VectorXf coeff_refined;
  model->optimizeModelCoefficients(inliers, coeff, coeff_refined);
  EXPECT_EQ(11, coeff_refined.size());
  EXPECT_NEAR(1.0, coeff_refined[0], 1e-3);
  EXPECT_NEAR(5.0, coeff_refined[1], 1e-3);
  EXPECT_NEAR(1.0, coeff_refined[2], 1e-3);

  EXPECT_NEAR(1.0, coeff_refined[3], 1e-3);
  EXPECT_NEAR(2.0, coeff_refined[4], 1e-3);

  EXPECT_NEAR(0.0, coeff_refined[5], 1e-3);
  EXPECT_NEAR(1.0, coeff_refined[6], 1e-3);
  EXPECT_NEAR(0.0, coeff_refined[7], 1e-3);

  EXPECT_NEAR(0.999, coeff_refined[8], 1e-3); // ~=1.0
  EXPECT_NEAR(0.0, coeff_refined[9], 1e-3);
  EXPECT_NEAR(-0.0284, coeff_refined[10], 1e-3); // ~=0.0
}

Environment:

Additional info

As I was pulling today the up-to-date remote, before merging my code, I noticed that something has changed since the last PCL 1.11.1 version. The following error appeared:

C:\git\pcl\sample_consensus\eigen.h(41,81): error C4430: missing type specifier - int assumed. Note: C++ does not support default-int
C:\Program Files (x86)\Microsoft Visual Studio\2019\Community\VC\Tools\MSVC\14.27.29110\include\vcruntime_new.h(17,8): error C2448: 'PCL_DEPRECATED_HEADER': function-style initializer appears to be a function definition

Upon inspection of the newer 'pcl/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp' file PCL 1.12.1, I noticed some '#include' changes as presented below and changed my 'sac_model_ellipse3d.hpp'. Even though the code built smoothly and passed the tests, I would appreciate if someone double checks these changes. 

// PCL 1.11.1 (lines 42-44)
#include <cfloat> // for DBL_MAX
#include <pcl/sample_consensus/eigen.h>
// PCL 1.12.1 (lines 42-44)
#include <limits>
#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt

Further developments

Before merging my comments, I removed the files corresponding to the unfinished 2D version of this, pcl::SACMODEL_ELLIPSE2D. Please let me know if this is something of your interest. I can spend some more time trying to finish them. Lastly, I have not managed to find time to render the these 3D ellipses using VTK, but for anyone interested, this can be achieved using an EllipticalCylinder with minimal height.

That's all from me, please let me know if this issue needs further improvement. Thank you for your time!

mvieth commented 2 years ago

Thank you! This is really interesting. Regarding your research project (out of interest): since you have 4 scans in 45 degree turns, why did you decide to segment 3d ellipses instead of a cylinder? My intuition would be that working with a cylinder directly might be more stable?

I noticed some '#include' changes

Yes, we are deprecating and removing pcl/sample_consensus/eigen.h and similar headers. The files that used them should include the headers from Eigen directly. So including the NonLinearOptimization header looks like the right solution here.

pcl::SACMODEL_ELLIPSE2D. Please let me know if this is something of your interest. I can spend some more time trying to finish them

Ultimately, you will have to decide whether you want to spend the time of course :slightly_smiling_face: I would say, let's first work on the 3D ellipse, then decide how we want to continue.

I will have a look at your PR soon!

mnobrecastro commented 2 years ago

Dear @mvieth, thank you very much for your quick reply!

We have done that already - feel free to have a look at it, it's OA (https://www.frontiersin.org/articles/10.3389/fnbot.2022.814973/full). The challenge here was precisely to use less data and have similar performance. So those rendered 3D cylinders you see at the bottom of the figure, were estimated solely based on a simple algorithm which uses those 4 scans, namely the models of four fitted Ellipse3D or two Ellipse3D + one Circle3D + one Line.

Regarding the Ellipse2D, of course I would not mind working on it! But has you suggested, let's take it one step at a time.

Best regards, Miguel

mnobrecastro commented 2 years ago

Dear @mvieth, thank you for accepting and merging my PR. I will update you with another PR regarding the 2D version, asap.

mvieth commented 2 years ago

Dear @mvieth, thank you for accepting and merging my PR. I will update you with another PR regarding the 2D version, asap.

I thought a bit about whether to add a 2D version or not. So on the one hand, it would be nice to have such a model of course, but on the other hand, it would mean more code to maintain and adding a bit of compilation time. Additionally, we can't estimate yet whether such a 2D ellipse would be used (especially since the 3D ellipse has just been merged). Can you say whether the 3D ellipse model can also be used in the 2D case (when all points are in one plane)? Would there be drawbacks to this? (accuracy, speed) Which parts of the 3D ellipse model would have to be adapted and how difficult would this be?

mnobrecastro commented 1 year ago

Dear @mvieth, thank you for accepting and merging my PR. I will update you with another PR regarding the 2D version, asap.

I thought a bit about whether to add a 2D version or not. So on the one hand, it would be nice to have such a model of course, but on the other hand, it would mean more code to maintain and adding a bit of compilation time. Additionally, we can't estimate yet whether such a 2D ellipse would be used (especially since the 3D ellipse has just been merged). Can you say whether the 3D ellipse model can also be used in the 2D case (when all points are in one plane)? Would there be drawbacks to this? (accuracy, speed) Which parts of the 3D ellipse model would have to be adapted and how difficult would this be?

You made a good point. A 2D ellipse fitting can be accomplished using the existing 3D ellipse (in the end it would be just a matter of adding a z=0 coordinate to the 2D points, thus converting it to a 3D problem). There won't be much overhead in terms of computation beyond the simple calculation of the plane normal. If you recall, the 3D ellipse fitting procedure works at the level of local coordinates of the points w.r.t. to the plane that was found. So, the 3D problem is actually solved as a 2D problem once the points are converted to local coordinates (and only x_local and y_local coordinates are used). In sum, adapting the 3D ellipse model to its 2D counterpart will be very simple and straight forward, but if you have concerns about the compilation time, that is something to take in consideration (especially after changing all Eigen functions to const).