Add computeCentroidAndOBB function #5690

mynickmynick · 2023-04-25T14:44:39Z

/** \brief Compute centroid, OBB, PCA axes of a given set of points.

OBB is oriented like the three axes (major, middle and minor) with
major_axis = obb_rotational_matrix.col(0)
middle_axis = obb_rotational_matrix.col(1)
minor_axis = obb_rotational_matrix.col(2)
one way to visualize OBB when Scalar is float:
Eigen::Vector3f position(obb_position(0), obb_position(1), obb_position(2));
Eigen::Quaternionf quat(obb_rotational_matrix);
viewer->addCube(position, quat, obb_dimensions(0), obb_dimensions(1), obb_dimensions(2), .....);
\param[in] cloud the input point cloud
\param[out] centroid the centroid of the set of points in the cloud
\param[out] obb_center position of the centroid of the OBB (Oriented Bounding Box)
\param[out] obb_dimensions (width, height and depth) of the OBB (Oriented Bounding Box)
\param[out] obb_rotational_matrix rotational matrix of the OBB (Oriented Bounding Box)
\return number of valid points used to determine the output.
In case of dense point clouds, this is the same as the size of the input cloud.
\ingroup common
*/

light connection with issue #5659 which can be closed after this merge

mynickmynick · 2023-04-26T16:13:59Z

@mvieth please let me know what you think about the two points left from the previous pull request (enforcing right hand rule and Eigen::Affine ) that I proposed to postpone. please see the comments there

mvieth

@mvieth please let me know what you think about the two points left from the previous pull request (enforcing right hand rule and Eigen::Affine ) that I proposed to postpone. please see the comments there

If the cross product is used to enforce the right hand rule, I am fine with it, but I would prefer using Eigen's vec1.cross(vec2) function to not "reinvent the wheel" and add a comment why this is done.
Regarding Eigen::Affine: yes I think it would be a good idea to try that since it would lead to shorter and potentially faster code (Eigen may use SIMD instructions).

common/include/pcl/common/impl/centroid.hpp

mynickmynick · 2023-04-28T14:31:29Z

Regarding Eigen::Affine: yes I think it would be a good idea to try that since it would lead to shorter and potentially faster code (Eigen may use SIMD instructions).

could you please suggest a link to read about this SIMD (to possibly study also how to trigger it)?
could you please exemplify how you were thinking to define that Eigen::Affine call ?
it would be a Eigen::Transform<Scalar,3,Affine> defined with rotate() and translation() but how to execute it exactly in this case?

i conceived this
Scalar x = xd * major_axis(0) + yd * major_axis(1) + zd * major_axis(2);
Scalar y = xd * middle_axis(0) + yd * middle_axis(1) + zd * middle_axis(2);
Scalar z = xd * minor_axis(0) + yd * minor_axis(1) + zd * minor_axis(2);

as the projection of the vector v=[xd,yd,zd] on the 3 axes major...minor to give the 3 Cartesian components
so
more easily
as 3 scalar products <major_axis, v> ...<minor_axis,v> (as implemented here)
or as a product between the obb_rotational_matrix and v

which is defined below but can be defined before as
obb_rotational_matrix <<
major_axis(0), middle_axis(0), minor_axis(0),
major_axis(1), middle_axis(1), minor_axis(1),
major_axis(2), middle_axis(2), minor_axis(2);

that seems to me conceptually more simple and straight forward then an affine transform of coordinates, which I am not sure how to implement in this case with an Eigen call
and substituting 3 lines of codes (with few * and +) with function calls, unless we are really sure it is inline and parallelizable, doesn't seem to optimize the processing time.
I have very little time now to study this , implement it and test it, that's why I thought it was considerable a later optimization

probably it might get convenient make a global affine transformed copy of all the cloud before the for cycle, instead of on the single point inside the for, probably that is what you meant

that would allow also to reduce the executed < and > inside the for() which I saw cost time
(

because you can
initialize min and max with the first finite value and
then substitute

if ( < min )
..
if ( > max )
...

with
if ( < min )
..
else if ( > max )
...

)

I know for instance soemthing like this

Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
....
transform_2.translation() << ....
pcl::transformPointCloud(*cloud, *transformed, transform_2);

or also something like this (defining the Homogenous matrix 4x4)

Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
transform_1=....
pcl::transformPointCloud(*cloud, *transformed, transform_1);
but it's far from optimal

what's the best way you think to use Eigen instead of pcl in the last step ( pcl::transformPointCloud ) ?

mynickmynick · 2023-04-28T14:35:42Z

If the cross product is used to enforce the right hand rule, I am fine with it, but I would prefer using Eigen's vec1.cross(vec2) function to not "reinvent the wheel" and add a comment why this is done.

no problem

mvieth · 2023-05-04T12:10:32Z

could you please suggest a link to read about this SIMD (to possibly study also how to trigger it)?

If you have never heard of SIMD before, the Wikipedia page could be a good starting point. This page discusses some Eigen internals. This FAQ page is also very informative. The basic idea is that an operation is done on multiple values (usually 4, 8, or 16) at the same time, which can be much faster. Eigen will use SIMD operations if possible, for example when multiplying with a 4x4 matrix (or equivalently when using an affine transform).

probably it might get convenient make a global affine transformed copy of all the cloud before the for cycle, instead of on the single point inside the for, probably that is what you meant

That would also be possible, but not quite what I meant. We don't need to store all the transformed points, we can just transform each point, compare, and "forget" the point.
What you describe as a projection on the Eigen vectors can also be thought of as a rotation. My idea was to create an Eigen affine transform before the for-loop from the translation and projection/rotation (see also here: https://eigen.tuxfamily.org/dox/group__TutorialGeometry.html)

I have very little time now to study this , implement it and test it, that's why I thought it was considerable a later optimization

If the function works but is not fully optimized performance-wise, that is also fine by me. Just say what you are willing to do/how much time you have.

mynickmynick · 2023-05-04T19:54:58Z

@mvieth

If you have never heard of SIMD before,

of course I have heard of it I just wanted to know how and when it is applied by eigen in this case.

What you describe as a projection on the Eigen vectors can also be thought of as a rotation.

of course the change of reference is also considerable as an homogeneous transform

I have implemented it. I had to solve a stupid bug I had done
The results are equivalent.
The processing is slower

float: 6 -> 7 msec
double : 7 -> 8 msec

How to assess if eigen is optimizing?

i pushed the code, please let me know what you think about it and please let's close this point soon ;))

…tion

…tion + minor fix

mynickmynick · 2023-05-05T07:16:38Z

@mvieth
your patch

centroid = centroid4.head<3>();
instead of
centroid(0) = centroid4(0);
centroid(1) = centroid4(1);
centroid(2) = centroid4(2);

compiles on my PC but does not compile on azure pipeline so I had to restore it

…tion + minor fix

mvieth

How to assess if eigen is optimizing?

Eigen says: https://eigen.tuxfamily.org/index.php?title=FAQ#How_can_I_check_that_vectorization_is_actually_being_used.3F

Please also add a note to https://github.com/PointCloudLibrary/pcl/blob/master/doc/tutorials/content/moment_of_inertia.rst specifying that users who only need the obb or aabb, but not the descriptors, should use computeCentroidAndOBB or getMinMax3D (faster)

common/include/pcl/common/impl/centroid.hpp

…ion + ref in moment of inertia tutorial

mynickmynick · 2023-05-08T14:08:55Z

@mvieth it's all set up

mvieth · 2023-05-17T13:28:48Z

common/include/pcl/common/impl/centroid.hpp

+
+    for (; i<cloud.points.size();++i)
+    {
+      auto point = cloud.points[i];


Here, an isFinite check is missing?

i will check it later but yes sorry!! seems a silly bug

Ohh, and don't use .points - use the operators on the pointcloud directly.

I am sorry I don't think I understand in detail what you mean, can you exemplify in more detail please?

And if it is mvieth - I could imagine something like:

If(!pcl::isInfinite(point)) continue;

mvieth · 2023-05-17T13:34:16Z

common/include/pcl/common/impl/centroid.hpp

+
+    for (; i<indices.size();++i)
+    {
+      auto point = cloud.points[indices[i]];


isFinite check also missing here?

mvieth · 2023-05-17T13:40:09Z

common/include/pcl/common/centroid.h

+  * \param[out] centroid the centroid of the set of points in the cloud
+  * \param[out] obb_center position of the centroid of the OBB (Oriented Bounding Box)


Please describe better what the difference between centroid and obb_center is.
For the description of obb_center, I would strictly use the word "center" instead of "centroid" to avoid confusion.
For centroid, I would mention that this is the mean of all points, and perhaps that obb_center and centroid are very different if the cloud is highly skewed or has outliers.

ok, the two are the same if the cloud is centrally symmetric.
the obb centre is the centroid of the 8 OBB vertices= centre of symmetry of the OBB (so I called it centroid, but I agree it might be misleading and needs better clarification as you wrote)

larshg · 2023-05-19T10:16:38Z

common/include/pcl/common/impl/centroid.hpp

+
+    for (; i<cloud.points.size();++i)
+    {
+      auto point = cloud.points[i];


Suggested change

auto point = cloud.points[i];

auto point = cloud[i];

mynickmynick · 2023-05-20T20:30:27Z

i pushed the corrections

larshg · 2023-05-20T21:16:34Z

common/include/pcl/common/impl/centroid.hpp

+
+  if (cloud.is_dense)
+  {
+    auto point = cloud[0];


Why do you process this first point outside the for loop?

Else you could have a for-range loop, ie:

for(const auto& point : cloud) { ... }

couldn't you?

anyways, you missed a few .points in the for loops 😄

Same for the non-dense branch.

for the sane reason I have split the non dense for loop in two parts: to set min max finite so to have

if (P(0) < obb_min_pointx) obb_min_pointx = P(0); else if (P(0) > obb_max_pointx) obb_max_pointx = P(0);

instead of

if (P(0) < obb_min_pointx) obb_min_pointx = P(0); if (P(0) > obb_max_pointx) obb_max_pointx = P(0);

one might avoid also
obb_min_pointx = obb_min_pointy = obb_min_pointz = std::numeric_limits::max();
obb_max_pointx = obb_max_pointy = obb_max_pointz = std::numeric_limits::min();
but then the compiler doesn't like it cause it sees a possible non initialization (if the cloud is non dense and all non finite or if it is of size 0)

do you mean cloud.points.size() ??
that is a different thing
I checked there is an inline efficient operator[]
but for this ?

Ahh. Make sense.

Yes, I pretty sure you can and should use cloud.size(). The .points should have been a private member, as it exposes the internal container type. Which makes it unchangeable atm, but eventually one could.

I guess you save lots of compares if the first point is in the right "side" and then the next just grows or shrink. Worst case you would have equally as many compares btween the two solutions?

one could make difficult statistical analysis on this based on the statistical cloud distribution. for a uniform density the probability would inolv some sort of geometric progression. the best case you save more is when the cloud is a surface with indexes decreasing progressively in x,y,z, the worst the opposite. one could think of a dynamical object that changes the code of comparison based on the cloud progression but let's leave it. I just measured with a counter with one of my main samples and it gives this on 11 segments

saved 0.000258977
saved 0.00359994
saved 0.00205254
saved 0.00217213
saved 0.0025008
saved 0.0554903
saved 0.00217482
saved 0.00121175
saved 0.0190972
saved 0.00453767
saved 0.00445811

Loaded cloud width*height = 2205157 w,h= 2205157 , 1
cloud is_dense= 1
cloud isOrganized= 0

so you can see from 0.02% to 5.5% comparisons avoided, 0.9% in average on this cloud, it can change a lot from cloud to cloud

larshg · 2023-05-20T21:22:57Z

common/include/pcl/common/impl/centroid.hpp

+    major_axis(0), middle_axis(0), minor_axis(0),
+    major_axis(1), middle_axis(1), minor_axis(1),
+    major_axis(2), middle_axis(2), minor_axis(2);
+  //obb_rotational_matrix.col(0)==major_axis


I consider this comment necessary

larshg · 2023-05-20T21:23:09Z

common/include/pcl/common/impl/centroid.hpp

+    major_axis(0), middle_axis(0), minor_axis(0),
+    major_axis(1), middle_axis(1), minor_axis(1),
+    major_axis(2), middle_axis(2), minor_axis(2);
+  //obb_rotational_matrix.col(0)==major_axis


mynickmynick · 2023-05-21T09:23:19Z

the tests have failed due to kfpcs_ia, has the master erros now?
Ubuntu 20 Clang - run Unit Tests

larshg · 2023-05-21T09:24:49Z

the tests have failed due to kfpcs_ia, has the master erros now? Ubuntu 20 Clang - run Unit Tests

No, its somehow a flaky test.

larshg · 2023-05-22T11:35:25Z

common/include/pcl/common/impl/centroid.hpp

+{
+  Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
+  Eigen::Matrix<Scalar, 4, 1> centroid4;
+  unsigned int point_count= computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);


Suggested change

unsigned int point_count= computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);

const auto point_count = computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);

larshg · 2023-05-22T11:37:52Z

common/include/pcl/common/impl/centroid.hpp

+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 3, 3>> evd(covariance_matrix);
+
+  Eigen::Matrix<Scalar, 3, 3> eigenvectors_ = evd.eigenvectors();
+  Eigen::Matrix<Scalar, 3, 1> major_axis;


const Eigen::Matrix<Scalar, 3, 1> minor_axis = eigenvectors_.col(0); //the eigenvectors do not need to be normalized (they are already) const Eigen::Matrix<Scalar, 3, 1> middle_axis = eigenvectors_.col(1); // Enforce right hand rule const Eigen::Matrix<Scalar, 3, 1> major_axis = middle_axis.cross(minor_axis);

larshg · 2023-05-22T11:38:17Z

common/include/pcl/common/impl/centroid.hpp

+
+  if (cloud.is_dense)
+  {
+    auto point = cloud[0];


Suggested change

auto point = cloud[0];

const auto& point = cloud[0];

Get a reference instead of making a copy? Can be applied several places.

larshg · 2023-05-22T11:39:05Z

common/include/pcl/common/impl/centroid.hpp

+
+  obb_center = centroid+ obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud
+
+  return ( point_count);


Suggested change

return ( point_count);

return (point_count);

larshg · 2023-05-22T11:39:13Z

common/include/pcl/common/impl/centroid.hpp

+  obb_dimensions(1) = obb_max_pointy - obb_min_pointy;
+  obb_dimensions(2) = obb_max_pointz - obb_min_pointz;
+
+  obb_center = centroid+ obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud


Suggested change

obb_center = centroid+ obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud

obb_center = centroid + obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud

larshg · 2023-05-22T11:39:33Z

common/include/pcl/common/impl/centroid.hpp

@@ -52,6 +53,7 @@
 namespace pcl
 {

+


larshg · 2023-05-22T11:47:43Z

common/include/pcl/common/impl/centroid.hpp

+
+  }
+
+  Eigen::Matrix<Scalar, 3, 1>  //shift between point cloud centroid and OBB center (position of the OBB center relative to (p.c.centroid, major_axis, middle_axis, minor_axis))


const infront - can be applied to several of the Eigen::Matrix's.

larshg · 2023-05-26T13:13:32Z

common/include/pcl/common/impl/centroid.hpp

+{
+  Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
+  Eigen::Matrix<Scalar, 4, 1> centroid4;
+  const auto point_count= computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);


Suggested change

const auto point_count= computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);

const auto point_count = computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);

larshg · 2023-05-26T13:14:03Z

common/include/pcl/common/impl/centroid.hpp

+
+  if (cloud.is_dense)
+  {
+    const auto & point = cloud[0];


Suggested change

const auto & point = cloud[0];

const auto& point = cloud[0];

There are multiple places for this

larshg · 2023-05-26T13:15:35Z

Minor formatting errors, but else it looks good!

mvieth

Thank you!

function computeCentroidAndOBB

c012d57

mvieth reviewed Apr 28, 2023

View reviewed changes

common/include/pcl/common/impl/centroid.hpp Outdated Show resolved Hide resolved

common/include/pcl/common/impl/centroid.hpp Outdated Show resolved Hide resolved

mynickmynick mentioned this pull request May 4, 2023

[ConvexHull] ConvexHull::reconstruct (PointCloud &points) pretends points to be empty #5693

Closed

mynickmynick added 2 commits May 5, 2023 08:56

function computeCentroidAndOBB with more eigen use for the implementa…

3bb7c7d

…tion

function computeCentroidAndOBB with more eigen use for the implementa…

8adbdb1

…tion + minor fix

mynickmynick added 2 commits May 5, 2023 09:38

function computeCentroidAndOBB with more eigen use for the implementa…

f5fa08b

…tion + minor fix

function computeCentroidAndOBB with more eigen use for the implementa…

394df20

…tion + minor fix

mvieth reviewed May 5, 2023

View reviewed changes

common/include/pcl/common/impl/centroid.hpp Outdated Show resolved Hide resolved

common/include/pcl/common/impl/centroid.hpp Outdated Show resolved Hide resolved

function computeCentroidAndOBB with more eigen use in the implementat…

9c260f6

…ion + ref in moment of inertia tutorial

larshg added changelog: enhancement Meta-information for changelog generation module: common labels May 10, 2023

larshg added this to the pcl-1.14.0 milestone May 10, 2023

mvieth reviewed May 17, 2023

View reviewed changes

larshg reviewed May 19, 2023

View reviewed changes

bug fix

cca5351

larshg reviewed May 20, 2023

View reviewed changes

minor refinements

85173b0

larshg reviewed May 22, 2023

View reviewed changes

common/include/pcl/common/impl/centroid.hpp Outdated

@@ -52,6 +53,7 @@

namespace pcl

{

Copy link

Contributor

larshg May 22, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove

larshg reviewed May 22, 2023

View reviewed changes

added const to some variables

7f830d3

larshg reviewed May 26, 2023

View reviewed changes

larshg previously approved these changes May 26, 2023

View reviewed changes

minor formatting refinements

9d26ba1

mynickmynick dismissed larshg’s stale review via 9d26ba1 May 30, 2023 17:17

mvieth approved these changes Jun 1, 2023

View reviewed changes

mvieth merged commit 68904fc into PointCloudLibrary:master Jun 1, 2023

mynickmynick mentioned this pull request Jun 1, 2023

[moment_of_inertia] "Planar and Linear Regression as a lighter much faster (200 times) but still powerful alternative to Moments of Inertia Estimation " #5659

Closed

mvieth changed the title ~~function computeCentroidAndOBB~~ Add computeCentroidAndOBB function Aug 7, 2023

		* \param[out] centroid the centroid of the set of points in the cloud
		* \param[out] obb_center position of the centroid of the OBB (Oriented Bounding Box)

	unsigned int point_count= computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);
	const auto point_count = computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);


		obb_center = centroid+ obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud

		return ( point_count);

	obb_center = centroid+ obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud
	obb_center = centroid + obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud


		}

		Eigen::Matrix<Scalar, 3, 1> //shift between point cloud centroid and OBB center (position of the OBB center relative to (p.c.centroid, major_axis, middle_axis, minor_axis))

	const auto point_count= computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);
	const auto point_count = computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);

Add computeCentroidAndOBB function #5690

Add computeCentroidAndOBB function #5690

Conversation

mynickmynick commented Apr 25, 2023

mynickmynick commented Apr 26, 2023

mvieth left a comment

Choose a reason for hiding this comment

mynickmynick commented Apr 28, 2023 • edited Loading

mynickmynick commented Apr 28, 2023

mvieth commented May 4, 2023

mynickmynick commented May 4, 2023 • edited Loading

mynickmynick commented May 5, 2023 • edited Loading

mvieth left a comment

Choose a reason for hiding this comment

mynickmynick commented May 8, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mynickmynick commented May 20, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mynickmynick May 20, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mynickmynick commented May 21, 2023

larshg commented May 21, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

larshg commented May 26, 2023

mvieth left a comment

Choose a reason for hiding this comment

mynickmynick commented Apr 28, 2023 •

edited

Loading

mynickmynick commented May 4, 2023 •

edited

Loading

mynickmynick commented May 5, 2023 •

edited

Loading

mynickmynick May 20, 2023 •

edited

Loading