1. Rotación de coordenadas
La rotación de coordenadas incluye la rotación alrededor de los ejes X, Y y Z. En el sistema de coordenadas diestro , x-roll (balanceo), y-pitch (cabeceo), z-heading (guiñada). Si quieres saber más, puedes buscar en Internet
En PCL, la relación de conversión de enlace base a mapa es: primero rotar alrededor del eje x, luego rotar alrededor del eje y y finalmente rotar alrededor del eje z, es decir, la multiplicación de la izquierda es Rzyx y luego traducir t La relación de conversión de map a baseLink es exactamente En cambio, primero traduzca -t, luego gire alrededor del eje Z, alrededor del eje Y y finalmente alrededor del eje X. Tenga en cuenta que el ángulo de rotación en este momento es exactamente lo contrario de eso de baseLink a mapa.
La posición actual de la carrocería del coche es la actitud x, y, z, roll, pitch, yaw. Obteniendo pcl::getTransformationla matriz de rotación del sistema de coordenadas de la carrocería del automóvil al sistema de coordenadas de la tierra.
Eigen::Matrix3f getRomateZYXMat(float yaw, float pitch, float roll){
float cz = cos(yaw);
float sz = sin(yaw);
float cx = cos(roll);
float sx = sin(roll);
float cy = cos(pitch);
float sy = sin(pitch);
Eigen::Matrix3f Rx, Ry, Rz, mat;
Rx << 1, 0, 0,
0, cx, -sx,
0, sx, cx;
Ry << cy, 0, sy,
0, 1, 0,
-sy, 0, cy;
Rz << cz, -sz, 0,
sz, cz, 0,
0 , 0, 1;
mat = Rx * Ry * Rz;
return mat;
}
En una nube de puntos 3D,
Esta fórmula de rotación puede interpretar efectivamente su código de fórmula de selección
como sigue
cx = cos(x(1));
cy = cos(x(2));
cz = cos(x(3));
sx = sin(x(1));
sy = sin(x(2));
sz = sin(x(3));
R1 = [cy*cz, sx*sy*cz-cx*sz, cx*sy*cz+sx*sz;
cy*sz, cx*cz+sx*sy*sz, cx*sy*sz-sx*cz;
-sy, sx*cy, cx*cy];El código se entiende