void applyRandomRotTrans(MatrixBase<Derived> & points) {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3, Eigen::Dynamic)
Quaternion q;
q.coeffs().setRandom();
q.normalize();
Matrix33 R = q.matrix();
Vector3 trans;
trans.setRandom();
points = R*points;
points.colwise() += trans;
}
void applyRandomRotTrans(MatrixBase<Derived> & points, Gen & g) {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3, Eigen::Dynamic)
Quaternion q;
q.coeffs() = q.coeffs().unaryExpr(g); // TODO: Check if q=[0,0,0,0] is correctly normalized !! otherwise crash! NaN
q.normalize();
Matrix33 R = q.matrix();
Vector3 trans;
trans = trans.unaryExpr(g);
trans.unaryExpr(g);
points = R*points;
points.colwise() += trans;
}
