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

[mynickmynick]

.

In an application I faced before, I needed a faster alternative to Moment Of Inertia Estimation.

Context

In an application I faced I first used the very powerful Moment Of Inertia Estimation to calculate the position of items (previously segmented via Euclidean clustering) but it had the issue of being too slow for that application so I implemented a much faster algorithm based on Statistical Regression (basically center of mass + mean square minimized error plane + search of major and middle vector on that plane). What I cmpute is center of mass, major, middle and minor axis vectors, AABB and OBB boxes. The algorithm is lighter meaning that: (1) it is much faster (n points complexity) with equivalent accuracy in the calculation of the center of mass and the calculation of the best fit plane (where major and middle vector are) and the calculation of the minor vector normal to the best fit plane (where middle and major vector are positioned) (2) the major and middle vector are less accurately calculated (but I am working on it): it is very correct the plane position of the two, they are correct in the planar position but when the segment is something like a square (not eccentric on the plane) they might be calculated in a position different from expected (sometimes along the diagonals more than along the axes of the item), rotated on the plane in a position different from the one provided by Moment of inertia method. The main reason is how the values are calculated in just one step (n complexity on the number of points). But when the item is quite eccentric (an eccentric rectangle or similar) the result is relatively good. I might decide to add an iteration with a minimum angle step to better calculate the correct roation of the two vectors (depending on the application). For that application I didn't need that accuracy. (3) it calculates less features (I am adding some of those calculated by Moment Of Inertia which are not yet supported)

I have implemented this outside of the PCL but I am porting it in a class inside the forked PCL and I could make a push/merge request if you appreciate it. It would be a honour. Again: it's very fast. This algorithm is about 200 times faster

I am working on it to improve the calculation of middle and major vector which are not invariant with rotation right now. I have implemented also the calculation of other features like the two kinds of bounding boxes etc. I could also add the staystical (not bounding) boxes

please let me know if this class (which I called RegressionEstimation) is interesting (I think it really is), thank you so much

I can also provide detailed mathematical explanations of the implementation I performed

Regarding point (1) please let me be more specific: The algorithm is based on the minimization of error epsilon between the actual cloud and the best fit plane in a formula of the kind z=z(x,y)=ax+by+c+epsilon(x,y) So of course it is expected to have a behaviour (even only in the more limited number of features mentioned at point (1)) worse than moment of inertia for : (1.1) surfaces that have a plane parallel to axis z as a best fit plane (so having the minor moment axis perpendicular to z axis). For this it will be easy to extend the algorithm with the optional calculation also of the best fit planes of the kind x=x(y,z)=dy+ez+f+epsilon(y,z) and y=y(x,z)=gx+hz+l+epsilon(x,z) and then select the best fit of the three planes found. The processing time should NOT increase *3 because much of the calculation is common to the triple plane calculation. I have not tested this yet because in my application all stuff is acquired as surfaces that have mostly 0 to 45 deg inclination between the local normals and the z axis (1.2) volumes (like those coming from a MRI for instance) or "very volumetric/curved" segment surfaces. This is based on best fit plane so volumes and "very volumetric/curved" surfaces are probably best covered by the moments of inertia method. Again: this is not the case for the applications i faced before so I didn't need it.

[mynickmynick]

By the way I noticed that inside the compute method of moment of inertia there is a double nested loop of the kind:

`while (theta <= 90.0f) { float phi = 0.0f;

while (phi <= 360.0f)
{

................. phi += step; } theta += step; }`

That's why it's so slow. Is it for what purpouse? To calculate eccentricity and moment of inertia in a dense span of directions? Cannot it be somehow avoided or made optional? The moments of inertia could be calculated only at request with a specific method for a single axis using the inertia matrix, they don't need to be precalculated in the main compute() method. Cannot you just calculate at default the eccentricity only along the three main axes? Or at least make the default behaviour lighter on this and make deeper calculations just on request. I might have a look at that and propose an implementation. I am reading in the tutorial that that cycle is performed to obtain rotation invariance but I don't understand because unless the cloud has special simmetry the 3 eigen vectors (obtained by the inertia matrix of which the covariance is a component) should be already invariant, don't they?

[mvieth]

Hi! I may misunderstand your idea, or perhaps you misunderstand what MomentOfInertiaEstimation is intended for. :slightly_smiling_face: If I interpret it correctly, the main purpose of MomentOfInertiaEstimation is to compute the global descriptors based on the moments of inertia and eccentricity. These could e.g. be used to compare two point clouds to find out whether they show the same object. The bounding boxes, mass center, etc are more a by-product. If you, for example, just want the AABB or the OBB, then MomentOfInertiaEstimation is not what you would want to use because it does much more than you need and is accordingly slow.

For reference: Moment of Inertia tutorial

[mynickmynick]

thank you for the reply. In my application I need to calculate where to go and suck and pick up different items. For that center of mass, minor vector (hence best fit plane) and OBB are perfect. How could I calculate that better than with my regression method or by the moment of inertia method? A suggested third alternative is welcome. The point and direction where to put the sucker is the intersection of the line passing by the center of mass directed like the minor vector and intersecting the OBB.

Plus I still don't understand why making such a long cycle inside compute() when you can calculate the inertia matrix once and for all to get moments (axis) any time with a simple vMv linear algebra product. Also the eccentricity should be calculated only along the 3 eigen vectors and not along so many directions. If the eigen vectors are not invariant it should mean the cloud is symmetric (so no need to rotate). But may be I am wrong. I will further investigate.

[mynickmynick]

Thanks for taking this into discussion but I disagree with "bounding boxes, mass center, etc are more a by-product". Moment of inertia and all its attached features are very general and wide application detections. Mass center, either one or three of the eigen vectors and the OBB alone for instance, above all if computed fast, are useful for finding position and orientation of items in situations where you have to process many different kinds of items of several different irregular shapes and very variable positions, you would not be able to teach and recognize with pattern matching algorithms becase you would have too many models. For that kind of application eccentricity and moments of inertia are of little use, while very useful instead are the previously mentioned features. I am doing some tests increasing from the default 10.0 to 45.0 the angle step but the long processing time of moment of inertia seems unaffected, so the reason must reside somewhere else (instead of the double loop I mentioned before). I am not sure, I will further investigate. What I propose here is a lighter, less powerful (with less features) but much faster class RegressionEstimation. I am working anyway to implement an optional more rich (less light) version triggered by the input parameter essential=false to provide more invariant middle and major vector and probably other features now missing, like measures of quadratic deviations along the three pseudoeigen vectors that might mimic or be similar to moments and eccentricity.

[larshg]

Can't you use PCA to find the mean (gripping point) and then use the eigenvectors, to estimate the orientation?

[mvieth]

Regarding computing the OBB: I am not sure if PCL currently has a function to conveniently do this (apart from what MomentOfInertiaEstimation offers). This could be something worth adding to PCL (a free function to compute the OBB).

How I would approach your problem: first compute the center and subtract that from the cloud (compute3DCentroid and demeanPointCloud). Then compute the covariance matrix and find out the direction of least variance by Eigenvalue decomposition (computeCovarianceMatrix and eigen33). Finally compute the range of your cloud along this direction. Or use the PCA class, as Lars suggested.

but I disagree with "bounding boxes, mass center, etc are more a by-product". [...] Mass center, either one or three of the eigen vectors and the OBB alone for instance, above all if computed fast, are useful for [...]

My point was not that the AABB, the OBB, the mass center, and the covariance matrix eigen vectors are not useful! I agree that they are very useful in many situations. My point was: if you want AABB, OBB, mass center, and/or eigen vectors, but you do not need the moments-of-inertia descriptor and the eccentricity descriptor (the ones stored as std::vector<float>), then MomentsOfInertiaEstimation is not what you should use. You should use other functions or classes which compute only what you need and are accordingly faster (for example the functions and classes Lars and I suggested).

[mynickmynick]

Thanks for the suggestion. I made a benchmark. point cloud of 1.4 million points. Clustered in about 10 clusters via euclidean conditional method. On each cluster I apply the feature extraction. PCA providing only mean point and 3 eigen vectors: 53 msec Moment of Inertia providing center of mass, 3 eigen vectors, OBB (and the rest not used): 7600msec Regression providing only center of mass and 3 pseudoeigen vectors ( setEssential(true)): 38msec Regression providing center of mass, 3 pseudoeigen vectors, OBB (setEssential(false)): 58msec

Regression providing only center of mass and 3 pseudoeigen vectors is about 40% faster than PCA but PCA is perfectly invariant with rotations while regression has 2 pseudoeigen vectors out of 3 (the less important ones for my application: major and middle) that are variant with rotation along z axis (but I am studying to fix that)

[mynickmynick]

thanks mvieth I quite agree, please let me further study the matter in the next days (I don't have much time now unfortunately) and I'll get back to you. For sure PCA works fine for my purpouse, still slower though, I have to better check how much I can reach invariance of major and middle vectors without increasing the processing time up to PCA values.

[mvieth]

While benchmarking the PCA class, do you set the basis_only option to true in the constructor? Otherwise you pay for something you don't need (since you don't use getCoefficients())

[mynickmynick]

Good point with basis_only true PCA decreased from 53 to 44msec Great I substituted the linear regression on the plane with a 2D PCA. So before I had Planar Regression (to determine center of mass and minor vector) + Linear Regression on the found plane (the plane defined by center of mass , minor vector) to determine major and middle vector. Now I have Planar Regression (to determine center of mass and minor vector) + 2D PCA on the found plane (the plane defined by center of mass , minor vector) to determine major and middle vector. The situation improved with all 3 vectors invariance (so also OBB invariance!) and I also managed to speed up a little due to minor optimizations. Now I wonder if a full substitution with PCA (3D) done more at point cloud level and less at Eigen lib level will further speed up. Actual situation: point cloud of 1.4 million points. Clustered in about 10 clusters via euclidean conditional method. On each cluster I apply the feature extraction. PCA providing only mean point and 3 eigen vectors: 44 msec Regression (+2D PCA) providing only center of mass and 3 pseudoeigen vectors ( setEssential(true)): 36msec Regression (+2D PCA) providing center of mass, 3 pseudoeigen vectors, OBB (setEssential(false)): 48msec (improved) Regression providing only center of mass and 3 pseudoeigen vectors seems about 20% faster than PCA (might be some other internal difference??) and has now reached invariance of all 3 vectors thanks to 2D PCA.

[mynickmynick]

the figures i gave are not net, they are gross values due to the fact that some post clustering is mixed in cycle on each cluster together with feature extraction. i will see if I can optimize and separate that to get better net time measure. Yes I apologize now with net values the benchmark is even more strict: PCA providing only mean point and 3 eigen vectors: 21 msec Regression (+2D PCA) providing only center of mass and 3 pseudoeigen vectors ( setEssential(true)): 13msec

If I cut loose the filtering and cluster euclidean thres. so to have a big cluster the time goes like: PCA providing only mean point and 3 eigen vectors: 36 msec Regression (+2D PCA) providing only center of mass and 3 pseudoeigen vectors ( setEssential(true)): 21msec

so now the difference has increased to about 60-70% (it's normal as I subtracted a common term to both by separating the "post clustering")

[mynickmynick]

I surrender ;) !

I have totally substituted my regression algorithm with PCA done more at PCL level (for instance the product M*M.transposed) calling Eigen lib only for the EigenSolver ` // Compute eigen vectors and values Eigen::SelfAdjointEigenSolver evd(alpha); // Eigen::Vector3f eigenvalues; Eigen::Matrix3f eigenvectors;

eigenvectors_ = evd.eigenvectors();`

and the time i get is the same of Regression Method (12-13 msec), which is 60-70% lower than that of PCA (21msec) which in principle does the same thing but using more the Eigen lib

I have installed on my PC the eigen lib via vcpkg

[mynickmynick]

PCA injected in RegressionEstimation class (which now is a mutant): (the paste of code gets formatting errors because of the ><) `

void InertiaMatrixEtc_PCLExtended( //INPUT pcl::PointCloud::Ptr cloud, //OUTPUT _InertiaMatrixEtc& inertiaMatrixEtc

) { pcl::RegressionEstimation feature_extractor;

feature_extractor.setInputCloud(cloud);
feature_extractor.setEssential(true);
feature_extractor.computeByPCA();

feature_extractor.getAABB(inertiaMatrixEtc.min_point_AABB, inertiaMatrixEtc.max_point_AABB);
feature_extractor.getOBB(inertiaMatrixEtc.min_point_OBB, inertiaMatrixEtc.max_point_OBB, inertiaMatrixEtc.position_OBB, inertiaMatrixEtc.rotational_matrix_OBB);

feature_extractor.getRegressionVectors(inertiaMatrixEtc.major_vector, inertiaMatrixEtc.middle_vector, inertiaMatrixEtc.minor_vector);
feature_extractor.getMassCenter(inertiaMatrixEtc.mass_center);

}

`

and

`

template void pcl::RegressionEstimation::computeByPCA() {

if (!initCompute()) { deinitCompute(); return; }

auto number_of_points = staticcast(indices->size());

meanvalue(0) = meanvalue(1) = meanvalue(2) = 0.0f;

if (!essential_) { aabb_minpoint.x = aabb_minpoint.y = aabb_minpoint.z = std::numeric_limits::max();

aabb_max_point_.x = aabb_max_point_.y = aabb_max_point_.z =
    -std::numeric_limits<float>::max();

}

double SX, SY, SZ; SX = SY = SZ= 0;

double size = (double)number_of_points;

for (unsigned int i_point = 0; i_point < number_of_points; ipoint++) { auto point = ((*input)[(*indices_)[i_point]]);

SX += point.x;
SY += point.y;
SZ += point.z;

if (!essential_) {
  if (point.x <= aabb_min_point_.x)
    aabb_min_point_.x = point.x;
  if (point.y <= aabb_min_point_.y)
    aabb_min_point_.y = point.y;
  if (point.z <= aabb_min_point_.z)
    aabb_min_point_.z = point.z;

  if (point.x >= aabb_max_point_.x)
    aabb_max_point_.x = point.x;
  if (point.y >= aabb_max_point_.y)
    aabb_max_point_.y = point.y;
  if (point.z >= aabb_max_point_.z)
    aabb_max_point_.z = point.z;
}

}

if (size <= 0.0) size = 1;

meanvalue(0) = SX / size; meanvalue(1) = SY / size; meanvalue(2) = SZ / size;

pcl::PointCloud demean; demean.width = number_of_points; demean.height = 1; demean.resize(number_of_points);

if (!essential_) { x_m.resize(number_of_points); y_m.resize(number_of_points); z_m.resize(number_of_points); }

for (unsigned int i = 0; i < number_ofpoints; ++i) { auto p = ((*input)[(*indices)[i]]); // vector applied on the mass center if (essential) { demean[i].x = p.x - meanvalue(0); demean[i].y = p.y - meanvalue(1); demean[i].z = p.z - meanvalue(2); } else { demean[i].x = x_m[i] = p.x - meanvalue(0); demean[i].y = y_m[i] = p.y - meanvalue(1); demean[i].z = z_m[i] = p.z - meanvalue(2); }

}

// PCA : // Compute the product cloud_demean cloud_demean^T Eigen::Matrix3f alpha = Eigen::Matrix3f::Zero(); for (unsigned int i = 0; i < number_of_points; ++i) { alpha(0, 0) += demean[i].x demean[i].x; alpha(1, 1) += demean[i].y demean[i].y; alpha(2, 2) += demean[i].z demean[i].z; alpha(1, 0) += demean[i].x demean[i].y; alpha(2, 0) += demean[i].x demean[i].z; alpha(2, 1) += demean[i].y * demean[i].z; }

if (static_cast(number_of_points)) { alpha(0, 0) /= (static_cast(number_of_points)); alpha(1, 1) /= (static_cast(number_of_points)); alpha(2, 2) /= (static_cast(number_of_points)); alpha(1, 0) /= (static_cast(number_of_points)); alpha(2, 0) /= (static_cast(number_of_points)); alpha(2, 1) /= (static_cast(number_of_points)); }

alpha(0, 1) = alpha(1, 0);
alpha(0, 2) = alpha(2, 0);
alpha(0, 3) = alpha(3, 0);

// Compute eigen vectors and values Eigen::SelfAdjointEigenSolver evd(alpha); // Eigen::Vector3f eigenvalues; Eigen::Matrix3f eigenvectors;

eigenvectors_ = evd.eigenvectors();

minoraxis = eigenvectors_.col(0); middleaxis = eigenvectors_.col(1);

// cross product of the other two: x = y X z major = middle X minor majoraxis(0) = middleaxis(1) minoraxis(2) - middleaxis(2) minoraxis(1); majoraxis(1) = middleaxis(2) minoraxis(0) - middleaxis(0) minoraxis(2); majoraxis(2) = middleaxis(0) minoraxis(1) - middleaxis(1) minoraxis(0);

// arbitrary // to be done: take an estimate from the two eigen values majorvalue = 2.0; middlevalue = 1.5; minorvalue = 1.0;

if (!essential_) computeOBB();

isvalid = true;

deinitCompute(); }

`

[mynickmynick]

instead using PCA class it should be going like this:

`template bool PCA::initCompute () { if(!Base::initCompute ()) { PCL_THROWEXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed"); } if(indices->size () < 3) { PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3"); }

// Compute mean mean = Eigen::Vector4f::Zero (); compute3DCentroid (*input, indices, mean); // Compute demeanished cloud Eigen::MatrixXf cloud_demean; demeanPointCloud (input, *indices, mean_, cloud_demean); assert (clouddemean.cols () == int (indices->size ())); // Compute the product cloud_demean * cloud_demean^T const Eigen::Matrix3f alpha = (1.f / (staticcast(indices->size ()) - 1.f))

• cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ();

  // Compute eigen vectors and values Eigen::SelfAdjointEigenSolver evd (alpha); // Organize eigenvectors and eigenvalues in ascendent order for (int i = 0; i < 3; ++i) { eigenvalues[i] = evd.eigenvalues () [2-i]; eigenvectors.col (i) = evd.eigenvectors ().col (2-i); } // Enforce right hand rule eigenvectors.col(2) = eigenvectors.col(0).cross(eigenvectors_.col(1)); // If not basis only then compute the coefficients if (!basisonly) coefficients = eigenvectors.transpose() * cloud_demean.topRows<3> (); computedone = true; return (true); }`

` void InertiaMatrixEtc_PCA( //INPUT pcl::PointCloud::Ptr cloud, //OUTPUT _InertiaMatrixEtc& inertiaMatrixEtc

) {

pcl::PCA<pcl::PointXYZRGB> feature_extractor(true);

feature_extractor.setInputCloud(cloud);

Eigen::Matrix3f&  matt=feature_extractor.getEigenVectors();
inertiaMatrixEtc.major_vector = matt.col(0);
inertiaMatrixEtc.middle_vector = matt.col(1);
inertiaMatrixEtc.minor_vector = matt.col(2);
Eigen::Vector4f& mm= feature_extractor.getMean();
inertiaMatrixEtc.mass_center(0) = mm(0);
inertiaMatrixEtc.mass_center(1) = mm(1);
inertiaMatrixEtc.mass_center(2) = mm(2);

} `

[mynickmynick]

it remins that AABB and OBB calculations are integrated in the RegressionEstimation class and come not for free but with little processing time added. Please tell me what you think about it. Shall I consider this just as an exercise and keep my own implementation for me or shall i push something to pcl?

[mynickmynick]

i think that after this prolonged study the conclusion are that (1) as correctly suggested by @mvieth my iinitial use of Moment Of Inertia was not a good choice. PCA would have been better from the very beginning as suggested by @larshg . (2) i would like to know why if I basically reimplement the PCA by using less eigen and more pcl i get nearly twice as fast on my PC? (3) i will further investigate but it seems that regression method is beated not in speed but in invariance robustness by PCA (4) it might be useful to implement (AABB and) OBB integrated on top of PCA (possibly the fastest) as in hypothesis by @mvieth . It is currently available in my fork repo with pcl::RegressionEstimation::setEssential(false) pcl::RegressionEstimation::computeByPCA() but it might be added to the PCA class

[mynickmynick]

Regarding (2): now unfortunately I cannot check it but may be class PCA processes all components including RGB? (I fed XYZRGB clouds). If so which is the most efficient way to make it process XYZ only? thanks

[mvieth]

Regarding (2): now unfortunately I cannot check it but may be class PCA processes all components including RGB? (I fed XYZRGB clouds)

No, I am pretty sure this is not the case. My best guess is that the way PCA computes the covariance matrix (by storing the demeaned cloud in a Eigen::MatrixXf and multiplying that with a transposed version of itself) is simply not the fastest way to do this. This also does not make use of the symmetry of the covariance matrix.

(3) i will further investigate but it seems that regression method is beated not in speed but in invariance robustness by PCA

I have to say I am not very convinced of the regression approach to find the OBB. It may work well in your case since you mostly detect only the top of your objects (so you have quite flat/planar point clouds), but in general it does not seem like a good idea to fit a plane to a point cloud if you can't make this assumption. If I understand your benchmarks correctly, your regression approach would be roughly 10 ms faster than PCA currently (where are apparently also opportunities to optimize). My guess would be that this is relatively small compared to other steps in the processing pipeline, like the euclidean clustering?

(4) it might be useful to implement (AABB and) OBB integrated on top of PCA

PCA also offers the functionality of an incremental computation via the update function. This might be difficult to do with the OBB? So I think a free function would be better (e.g. computeMeanAndCovarianceMatrix, then eigen33) By the way: PCL has a function to compute the AABB: getMinMax3D

(1) as correctly suggested by @mvieth my iinitial use of Moment Of Inertia was not a good choice. PCA would have been better from the very beginning as suggested by @larshg .

We should clarify the documentation of the MomentOfInertiaEstimation class and the tutorial that this is not what anyone should use who only needs the center of mass/eigen vectors/aabb/obb (and what to use instead)

[mynickmynick]

My best guess is that the way PCA computes the covariance matrix ...This also does not make use of the symmetry

You're right, probably not using the simmetry is the cause. You might inject in it code similar to the one i wrote inside my forked pcl::RegressionEstimation::computeByPCA() where instead I use Eigen libs less than in PCA class and I exploit the symmetry (I pasted an extract above)

I have to say I am not very convinced of the regression approach to find the OBB. It may work well in your case

Yes I agree with you, as a matter of facts I think I stated, somewhere in the long post, that it might have problems with very vertical z parallel flat surfaces and with volumes and very 3d curved surfaces different from the ones I am processing. Even if actually I rotated a lot in simulation along all axes and I didn't get it. Anyway, as I wrote, in the end I added a method computeByPCA() (alternative to compute() using Regression) to my class where I reimplement the PCA (exploiting the product simmetry so calculating only 6 values instead of 9), having the same processing time of compute() and with the more invariance guarantee of PCA. So I agree to use PCA instead of regression for these purpouses. But my implementation of PCA as I wrote is about 60% faster than the PCA class (same time of Regression)

My guess would be that this is relatively small compared to other steps in the processing pipeline, like the euclidean clustering?

Yeah well in industrial robotic applications any 10 msec is precious ;) and this was just one refernce example, of course I might apply it to a higher number of points reaching higher values. The relative difference is about 60%-70% which is a lot.

PCA also offers the functionality of an incremental computation via the update function. This might be difficult to do with the OBB? So I think a free function would be better (e.g. computeMeanAndCovarianceMatrix, then eigen33) By the way: PCL has a function to compute the AABB: getMinMax3D

I am sorry I didn't understand here what you mean.

[mynickmynick]

@mvieth : i will wait before archiving the planar regression because it is highly parallelizable, and because (as I mentioned before) I can add the calculation of all three best fit planes z=ax+by+c+error1 x=dy+ez+f+error2 y=gx+hz+i + error3 and then select the best of the three (now I am only calculating the first) (which would prevent the already rarely seen vertical singularity) this can be done with little overhead because in the O(n) cycle I already gather what I need to obtain the other two

How do you best parallelize on PCL? CUDA? openMP? Do you allow to init std C++ threads and parallelize on those?? I think I had compile problems when i compiled the PCL so I left CUDA and openMP out (I will check it tomorrow)

Of course PCA will remain more invariant because it basically finds the planar(middle,major eigenvectors) best fit in the explicit general form with no singularity ax+by+cz+d+error=0 It would be nice to have a regression direct formula for that but I couldn't find it so far.

[mynickmynick]

@mvieth i will look in my forked repo for porting from pcl::RegressionEstimation::computeByPCA() to the PCA class the 1.6x faster implementation which exploits the simmetry

[mvieth]

@mynickmynick Please first look in centroid.hpp, I think the code you wrote for computing the covariance matrix already exists there Regarding parallelization: for CPU we mostly use OpenMP, but there are also some algorithms in CUDA (see the cuda and gpu modules)

[mynickmynick]

@mvieth yes centroid does have several methods for calculating mean and covariance matrix exploting the simmetry. Some method that avoid the use of indexes should be faster but they do not support subindexing? So now what? I guess this is what you mean by

So I think a free function would be better (e.g. computeMeanAndCovarianceMatrix, then eigen33)

do you mean to implement a function that uses computeMeanAndCovarianceMatrix and then calls // Compute eigen vectors and values Eigen::SelfAdjointEigenSolver evd(alpha); // Eigen::Vector3f eigenvalues; Eigen::Matrix3f eigenvectors; eigenvectors_ = evd.eigenvectors();

and then calculates OBB ?

an independent function?

[mvieth]

Some method that avoid the use of indexes should be faster but they do not support subindexing?

Not sure what you mean? There are functions in centroid that operate on the whole cloud, and there are functions that operate only on some points, specified by pcl::Indices or pcl::PointIndices.

do you mean to implement a function that uses computeMeanAndCovarianceMatrix and then calls // Compute eigen vectors and values Eigen::SelfAdjointEigenSolverEigen::Matrix3f evd(alpha); // Eigen::Vector3f eigenvalues; Eigen::Matrix3f eigenvectors; eigenvectors_ = evd.eigenvectors();

and then calculates OBB ?

an independent function?

Yes, but my thought was to use eigen33 instead of SelfAdjointEigenSolver. Not sure which one is faster.

[mynickmynick]

Yes, but my thought was to use [eigen33]instead of SelfAdjointEigenSolver. Not sure which one is faster.

ok I will check that.

There are functions in centroid that operate on the whole cloud, and there are functions that operate only on some points, specified by pcl::Indices or pcl::PointIndices.

yes ok, i will have a better look at the different overloads resolution

[mynickmynick]

@mvieth I have implemented centroid+PCA+OBB in a function (with some template variants float, double and with or without pcl::Indices or pcl::PointIndices.) and benchmarked (the case without indexing, using the full cloud) and got to interesting faster but accurate results:

as before: class PCA(basis_only=true) providing only mean point and 3 eigen vectors (no OBB): 21 msec (as seen before, not exploiting simmetry) Regression (+2D PCA) providing only center of mass and 3 pseudoeigen vectors ( setEssential(true), no OBB): 13msec PCA reimplemented in pcl providing only mean point and 3 eigen vectors (no OBB) ; 12 msec new: centroid+PCA+OBB <float, SelfAdjointEigenSolver> : 7msec centroid+PCA+OBB <double, SelfAdjointEigenSolver> : 11msec centroid+PCA+OBB <float, eigen33> : 8msec centroid+PCA+OBB <double, eigen33> : 11msec

with eigen33 I am not so sure about the condition "positive semi definite" if it holds or not

by the way does PCL only support float xyz?? (with no double option?)

I have also manually compared the results of centroid+PCA+OBB by rototranslating XYZ and comparing with class PCA and moment of inertia and the results have good level of coincidence, look pretty good

[mynickmynick]

i have pushed it on my fork https://github.com/mynickmynick/pcl as a branch function_computePCAAndBB

[mvieth]

with eigen33 I am not so sure about the condition "positive semi definite" if it holds or not

Covariance matrices are positive semi-definite, see for example Wikipedia: https://en.wikipedia.org/wiki/Covariance_matrix

by the way does PCL only support float xyz?? (with no double option?)

PCL only offers point types with float xyz (because it is faster and usually precise enough), and using custom point types with double xyz is quite limited. Some time ago, I tested adding more support for double precision, but did not make much progress because of how many changes would be necessary.

Thanks for the benchmarks! I had a quick look at the code on your fork, these are my first thoughts:

• I would not add functions for PointIndices. The existing functions for PointIndices are mostly kept for legacy reasons, and typically, Indices are used now
• I don't think the template specializations for Scalar=float and Scalar=double are necessary. To be honest, I am not sure why some functions in PCL have them, but I would not add them for new functions
• I think it is not a good idea to make these functions compute and return everything (and have 14 output parameters). I would focus on one clear purpose (I believe we agreed that would be the OBB here), and maybe also return some by-products, if sensible. More concretely, the function should not compute the AABB (PCL has getMinMax3D for that purpose). major_axis, middle_axis , and minor_axis are already accessible from obb_rotational_matrix, so there is no need to have additional output parameters for them. Instead of obb_min_point and obb_max_point we could just return a Vector3f with the side lengths. major, middle, and minor value I would return as one Vector3f
• Please reuse the other functions from centroid.hpp, especially computeMeanAndCovarianceMatrix

[mynickmynick]

I would not add functions for PointIndices. The existing functions for PointIndices are mostly kept for legacy reasons, and typically, Indices are used now

OK no problem I will delete it.

I don't think the template specializations for Scalar=float and Scalar=double are necessary. To be honest, I am not sure why some functions in PCL have them, but I would not add them for new functions

Ok I will take them off. I had put them because I saw them available for other functions inside the centroid module

I think it is not a good idea to make these functions compute and return everything (and have 14 output parameters). I would focus on one clear purpose (I believe we agreed that would be the OBB here), and maybe also return some by-products, if sensible. More concretely, the function should not compute the AABB (PCL has getMinMax3D for that purpose). major_axis, middle_axis , and minor_axis are already accessible from obb_rotational_matrix, so there is no need to have additional output parameters for them.

Yes I imagined those outputs would be considered too many. Ok sure for the redundancy of the rotation matrix (may be I will put in the comments how to extract the 3 axes from the matrix because it would not be straightforward for the average or occasional user). Can I also take off the major middle min values and the covariance matrix from the outputs? For AABB ok no problem: the strength of this function is that it computes all this stuff very efficiently and fast with just one for() O(n) cycle for everything except OBB and a 2nd for() O(n) cycle just for OBB. If you do not make available AABB then another call to getMinMax3D() is needed which will increase the process time. Are we sure we wanna do that? The user that needs only AABB can always call getMinMax3D() alone

Instead of obb_min_point and obb_max_point we could just return a Vector3f with the side lengths. major, middle, and minor value I would return as one Vector3f

Ok sure. Why not keeping the Scalar interface as it is available inside the function (obposition = centroid+ obb_rotational_matrix * shift;) and more uniform with the other outputs?

In short the outputs could for instance be either:

                Eigen::Matrix<Scalar, 3, 1> &centroid,
                PointT &obb_position,
                Eigen::Vector<Scalar, 3> &obb_dimensions,
                Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix);

or

                Eigen::Matrix<Scalar, 3, 1> &centroid,
                PointT &aabb_min_point,
                PointT &aabb_max_point,
                PointT &obb_position,
                Eigen::Vector<Scalar, 3> &obb_dimensions,
                Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix);

as you prefer.

In the first case you might like Eigen::Vector<Scalar, 3> &obb_position, more than PointT &obb_position, (up to you)

Please reuse the other functions from centroid.hpp, especially computeMeanAndCovarianceMatrix

Ok but please note the small improvements I made like lines 718,750,963,996 if (point.x <= aabb_min_point.x) aabb_min_point.x = point.x; else if (point.x >= aabb_max_point.x) aabb_max_point.x = point.x;

and

lines 962,992,etc. for (const auto &index : indices) { auto point = cloud[index]; ...... (that's probably done by the compiler but... are we sure?)

and please note the comments which make those hard coded formulas more human readable like for instance:

covariance_matrix.coeffRef (0) = accu [0] - accu [6] accu [6];//(0,0)xx : E[(x-E[x])^2]=E[x^2]-E[x]^2=E[(x-Kx)^2]-E[x-Kx]^2 covariance_matrix.coeffRef (1) = accu [1] - accu [6] accu [7];//(0,1)xy : E[(x-E[x])(y-E[y])]=E[xy]-E[x]E[y]=E[(x-Kx)(y-Ky)]-E[x-Kx]E[y-Ky] covariance_matrix.coeffRef (2) = accu [2] - accu [6] accu [8];//(0,2)xz covariance_matrix.coeffRef (4) = accu [3] - accu [7] accu [7];//(1,1)yy covariance_matrix.coeffRef (5) = accu [4] - accu [7] accu [8];//(1,2)yz covariance_matrix.coeffRef (8) = accu [5] - accu [8] accu [8];//(2,2)zz covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); //(1,0)yx covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); //(2,0)zx covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); //(2,1)zy

(those comments might be ported inside computeMeanAndCovarianceMatrix if I substitute this code with it)

[mvieth]

Ok sure for the redundancy of the rotation matrix (may be I will put in the comments how to extract the 3 axes from the matrix because it would not be straightforward for the average or occasional user).

Sounds good.

Can I also take off the major middle min values and the covariance matrix from the outputs?

You mean remove them from the list of outputs? Sure, I am fine with that.

For AABB ok no problem: the strength of this function is that it computes all this stuff very efficiently and fast with just one for() O(n) cycle for everything except OBB and a 2nd for() O(n) cycle just for OBB. If you do not make available AABB then another call to getMinMax3D() is needed which will increase the process time. Are we sure we wanna do that? The user that needs only AABB can always call getMinMax3D() alone

What about a user who only wants the OBB, but not the AABB? If the function would compute both, this user would have to "pay" for the unneeded AABB (same problem you initially had with MomentOfInertiaEstimation where you did not need the global descriptors but still had to "pay" for them). Just to be clear: I am suggesting to not compute the AABB inside this new function at all. I do not mean to only remove the AABB from the output list but still compute it. It is not necessary to compute the AABB to get the OBB. If someone really needs both the OBB and the AABB, then additionally calling getMinMax3D would only negligibly increase the process time (considering the new compute-OBB function will be faster if it does not compute the AABB).

Ok sure. Why not keeping the Scalar interface as it is available inside the function (obposition = centroid+ obb_rotational_matrix * shift;) and more uniform with the other outputs?

Yes, sorry, I should have been clearer. Eigen::Matrix<Scalar, 3, 1> is preferable, not Eigen::Vector3f. And yes, I would prefer Eigen::Vector<Scalar, 3, 1> &obb_position

Ok but please note the small improvements I made like [...] (that's probably done by the compiler but... are we sure?)

I am not sure I see which improvements you mean. Ultimately, only a benchmark would prove if something is faster :smile: From my experience I can only say that the compiler is often really good at optimizing code.

(those comments might be ported inside computeMeanAndCovarianceMatrix if I substitute this code with it)

Sounds good to me.

[mynickmynick]

Ok cool, I have little time these days, I will try to prepare it, test it and push it probably by the end of the week

[mynickmynick]

i am not understanding how i am supposed to use [] or () or coeffRef() or coeff() to access in read and/or write the elements of a Matrix and which is most efficient. I don't find the definition of operator[] and why inside computeMeanAndCovarianceMatrix you use covariance_matrix.coeffRef (0) instead of covariance_matrix(0) ?? while centroid[0] is used instead of centroid.coeffRef(0)? how efficient and preferable is the use of one or the other?

also in eigen documentation is not clear: do they allow indexed read and write access by operator[] or not?? it seems they do but only for Vector or Matrix with one dimension==1

I don't find the definition of operator[] and operator() for Matrix (or from a base class). The code of eigen is so badly readable

from what I understand matrix[i] (only for vectors), matrix(i), matrix.coeffRef(i) all give write and read access to the element of index i, but I don't know how efficiently while instead coeff(i) is used only to get (read) the value

[mvieth]

i am not understanding how i am supposed to use [] or () or coeffRef() or coeff() to access in read and/or write the elements of a Matrix and which is most efficient. I don't find the definition of operator[] and why inside computeMeanAndCovarianceMatrix you use covariance_matrix.coeffRef (0) instead of covariance_matrix[0] ?? while centroid[0] is used instead of centroid.coeffRef(0)? how effcient and preferable is the use of one or the other?

I found the following documentation helpful: https://eigen.tuxfamily.org/dox/group__QuickRefPage.html#title2 https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#title4 https://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html (open all the "Public Member Functions inherited from ...") My impression is that in most cases it does not really matter which one is used, but in my opinion vector(i) or matrix(i, j) is the most readable (and I think also the most widely used in the Eigen documentation).

also in eigen documentation is not clear: do they allow indexed read and write access by operator[] or not?? "The operator[] is also overloaded for index-based access in vectors, but keep in mind that C++ doesn't allow operator[] to take more than one argument. We restrict operator[] to vectors, because an awkwardness in the C++ language would make matrix[i,j] compile to the same thing as matrix[j]!" What sort of sentence is this?! There is nothing awkard! There is no binary operator[,] in C or C++. The , is just an expression sequence separator and that's it

Whether "awkwardness" is the right word is surely debatable, but the hint that matrix[i, j] does not do what most people (e.g. coming from another language) would expect, is correct and important.

I don't find the definition of operator[] and operator() for Matrix (or from a base class). The code of eigen is so badly readable

Again: https://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html (open all the "Public Member Functions inherited from ...") There you should find what you are searching for. I feel like if you want to complain about Eigen, this is not the place for it since we are not responsible for Eigen :wink: In my opinion, that the code of Eigen can be badly readable may be a symptom of heavily prioritizing performance over readability. Not saying this is necessarily a bad thing, but there is surely a trade-off between performance and readability.

[mynickmynick]

thnaks I'll have a look

[mynickmynick]

this issue can be closed as it had a long path which eventually led to #5690