[common] multiStep (update/growing) extension of computeMeanAndCovarianceMatrix

[mynickmynick]

In my segmentation developments I needed and implemented an extension of computeMeanAndCovarianceMatrix to apply periodically to the growing segment without having every time to apply the for loops to all points

essentially as the segment/cluster grows, for segmentation conditions, I needed to recompute mean and covariance. to make each step faster i calculate mean and covariance as a variation of the previously old calculated values, with a for loop only on added points

the function looks like this

template <typename PointT, typename Scalar> inline unsigned int updateMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, Eigen::Matrix<Scalar, 3, 3> &covariance_matrix, Eigen::Matrix<Scalar, 4, 1> &centroid, unsigned int oldSize, size_t & point_count )

the first time it is called with oldSize=0 and it works exactly as computeMeanAndCovarianceMatrix new size and point_count are recorded after this first call

the second time it is called with oldSize= size of previous call inside the for loop will be shorter like for (size_t i=oldSize; i<cloud.points.size();++i) {

and then there are specifically developed formulas to update the global values of mean and covariance putting together the old and the new segment of point cloud

the algorithm is effective and fast

it can be reapplied at every added point or every n added points or also non periodically when you need to update mean or covariance as the set changes

at each step you have to pass mean and covariance of previous call, they will be updated with new values

I thought it might be useful for PCL common module? you can see it on my fork implemented and used with this chain of calls

segment_ByOBBMT (thread call)-> segment_ByOBBThread -> updateCentroidAndOBB ->updateMeanAndCovarianceMatrix

but it can be used general purpouse