void getTfDifference(
const AffineTransform& transform,
double& dist, double& angle)
{
Matrix3f R = transform.rotation();
Vector3f t = transform.translation();
dist = sqrt(t(0,0)*t(0,0) + t(1,0)*t(1,0) + t(2,0)*t(2,0));
double trace = R(0,0) + R(1,1) + R(2,2);
angle = acos(std::min(1.0, std::max(-1.0, (trace - 1.0)/2.0)));
}
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;
}
}
