void transformMeans(
Vector3fVector& means,
const AffineTransform& transform)
{
Matrix3f R = transform.rotation();
Vector3f t = transform.translation();
unsigned int size = means.size();
for(unsigned int i = 0; i < size; ++i)
{
Vector3f& m = means[i];
m = R * m + t;
}
}
void transformDistributions(
Vector3fVector& means,
Matrix3fVector& covariances,
const AffineTransform& transform)
{
Matrix3f R = transform.rotation();
Vector3f t = transform.translation();
Matrix3f R_T = R.transpose();
unsigned int size = means.size();
for(unsigned int i = 0; i < size; ++i)
{
Vector3f& m = means[i];
Matrix3f& c = covariances[i];
m = R * m + t;
c = R * c * R_T;
}
}
void pointCloudFromMeans(
const Vector3fVector& means,
PointCloudFeature& cloud)
{
unsigned int size = means.size();
cloud.points.resize(size);
for(unsigned int i = 0; i < size; ++i)
{
const Vector3f& m = means[i];
PointFeature& p = cloud.points[i];
p.x = m(0,0);
p.y = m(1,0);
p.z = m(2,0);
}
cloud.height = 1;
cloud.width = size;
cloud.is_dense = true;
}