Conocimientos básicos de detección de objetivos 3D.

Cómo describir un objeto en el espacio 3D

  • La posición
    es coordenadas xyz.

  • El tamaño
    lwh es el largo, ancho y alto, es decir, el tamaño del cuadro 3D (x_size, y_size, z_size). Se define convencionalmente como la longitud del cuadro 3D del objeto a lo largo de los ejes x, y, z cuando el ángulo de guiñada del ángulo de rumbo es 0.

  • Tres métodos de expresión para la actitud
    : ángulo de Euler, matriz de rotación, cuaternión

    • El ángulo de Euler
      se divide en ángulo de cabeceo, ángulo de guiñada (ángulo de guiñada) y ángulo de balanceo. Los objetos en el suelo, como los vehículos, generalmente no se voltean ni se inclinan,
      por lo que generalmente solo es necesario considerar el ángulo de rumbo.
      La definición de ángulo de rumbo: seleccione un eje como eje de gravedad, seleccione una dirección de referencia en el plano perpendicular al eje de gravedad, luego el ángulo de orientación de la dirección de referencia es 0 y el ángulo de rumbo aumenta en el sentido de las agujas del reloj. Como se muestra en la figura siguiente, el eje z es el eje de gravedad, la dirección de referencia es el eje y y el ángulo entre la dirección de avance del vehículo y la dirección de referencia del eje y es la guiñada. Cabe señalar que los ejes de coordenadas en la figura siguiente se basan en el sistema de coordenadas del vehículo, es decir, el eje x está hacia adelante, el eje y está hacia la izquierda y el eje z está hacia arriba.

    • Matriz de rotación
      La relación de transformación entre coordenadas en un sistema de coordenadas y representación en otro sistema de coordenadas.

    • Cuaternión
      (w,x,y,z), diferentes representaciones de poses de referencia


transformación de coordenadas espaciales

La relación de transformación de ángulo ( rotation) se puede utilizar para expresar la actitud de un objeto en el sistema de coordenadas de referencia. Además del cambio de ángulo, también existe una relación de posición relativa entre el origen del sistema de referencia. Generalmente, la matriz de traducción ( ) se utiliza para translationexpresar
el cuaternión y la matriz de rotación. , matriz de traducción y matriz de transformación compuesta. Las definiciones y relaciones del código del archivo convert_nuScenes.py 三、main 函数se presentan en detalle para facilitar una lectura coherente. El contenido anterior se cita directamente a continuación.

matriz de traducción

Un punto P 0 ( x 0 , y 0 , z 0 ) en el espacio conocido P_ {0} (x_ {0}, y_ {0}, z_ {0})PAG0( x0,y0,z0) , entoncesP 0 P_{0}PAG0Traducir tx, ty, tz a lo largo de los ejes X, Y y Z t_{x}, t_{y}, t_{z}txtytzdistancia, obtenga el punto P 1 ( x 1 , y 1 , z 1 ) P_{1}(x_{1}, y_{1}, z_{1})PAG1( x1,y1,z1) , entonces este proceso se puede expresar comoP 1 = Matriz de traducción M ⋅ P 0 P1 = Matriz de traducción \cdot P0P1_ _=T r an s l a c i o n M a t r i xP 0 , es decir,P 0 P_ {0}PAG0Multiplique una matriz de traducción hacia la izquierda para obtener P 1 P_{1}PAG1
[ x 1 y 1 z 1 1 ] = [ x 0 + txy 0 + tyz 0 + tz 1 ] = [ 1 0 0 tx 0 1 0 ty 0 0 1 tz 0 0 0 1 ] [ x 0 y 0 z ] \ izquierda[\begin{array}{c}\mathrm{x}_{1}\\\mathrm{y}_{1}\\\mathrm{z}_{1}\\1 \end{array }\ derecha]=\izquierda[\begin{array}{c}\mathrm{x}_{0}+\mathrm{t}_{\mathrm{x}}\\\mathrm{y}_{0} +\ mathrm{t}_{\mathrm{y}}\\\mathrm{z}_{0}+\mathrm{t}_{\mathrm{z}}\\1 \end{array}\right] =\ izquierda[\begin{array}{cccc}1&0&0&\mathrm{t}_{\mathrm{x}}\\0&1&0&\mathrm{t}_{\mathrm{y }} \\ 0 & 0 & 1 & \ mathrm {t}_{\mathrm {z}} \\ 0 & 0 & 0 & 1 \ end{array}\right]\left[\begin{array}{c } \mathrm{x}_{0}\\ \mathrm{y}_{0}\\\mathrm{z}_{0}\\1\end{array}\right] X1y1z11 = X0+txy0+tyz0+tz1 = 100001000010txtytz1 X0y0z01

En el caso de una variable específica
T ranslation M atrix = [ 1 0 0 tx 0 1 0 ty 0 0 1 tz 0 0 0 1 ] TranslationMatrix = \left[\begin{array}{cccc} 1 & 0 & 0 & \ mathrm{; t}_{\mathrm{x}}\\0&1&0&\mathrm{t}_{\mathrm{y}}\\0&0&1&\mathrm{t}_{\mathrm{z }} \\ 0&0&0&1\end {matriz}\derecha]T r an s l a c i o n M a t r i x= 100001000010txtytz1

matriz de rotación

Los cuatro elementos y la matriz de rotación se utilizan para describir la información de actitud del cuerpo rígido, pero las formas de expresión son diferentes. Se pueden convertir mediante fórmulas matemáticas. Aquí, los cuatro elementos deben convertirse en una matriz de rotación, porque la matriz de rotación debe usarse más adelante Cálculo correspondiente
: Cuaternión conocido
Cuaternión = ( w , x , y , z ) Cuaternión = (w, x, y, z)Q u a t r ni o n=( w ,x ,y ,z )
则可以转换为如下旋转矩阵
M atrix de rotación = [ w 2 + x 2 − y 2 − z 2 2 ( xy − wz ) 2 ( xz + wy ) 2 ( xy + wz ) w 2 − x 2 + y 2 − z 2 2 ( yz − wx ) 2 ( xz − wy ) 2 ( yz + wx ) w 2 − x 2 − y 2 + z 2 ] RotationMatrix = \left[\begin{array}{ccc} w^ {2}+x^{2}-y^{2}-z^{2} & 2(x yw z) & 2(x z+wy) \\ 2(x y+wz) & w^{2 }-x^{2}+y^{2}-z^{2} & 2(y zw x) \\ 2(x zw y) & 2(y z+wx) & w^{2}-x ^{2}-y^{2}+z^{2} \end{array}\right]R o t a c i ó n M a t r i x= w2+X2y2z22 ( x y+wz ) _2 ( x zw y )2 ( x ywz ) _w2X2+y2z22 ( yz+wx ) _2 ( x z+w y )2 ( yzwx ) _w2X2y2+z2

Un punto P 0 ( x 0 , y 0 , z 0 ) en el espacio conocido P_ {0} (x_ {0}, y_ {0}, z_ {0})PAG0( x0,y0,z0) , entoncesP 0 P_{0}PAG0Al girar en diferentes ángulos alrededor del eje X, el eje Y y el eje Z (suponiendo que la relación de rotación está representada por la cuádruple anterior), el punto P 1 (x 1, y 1, z 1) P_ {1}(se obtiene x_{1}, y_{1}, z_{1})PAG1( x1,y1,z1) , entonces este proceso se puede transformar en una matriz de rotación expresada comoP 1 = [ Rotation Matrix 0 0 1 ] ⋅ P 0 P1 = \left[\begin{array}{cc} RotationMatrix & 0 \\ 0 & 1 \end {matriz}\right] \cdot P0P1_ _=[R o t a c i ó n M a t r i x001]P 0 , es decir,P 0 P_ {0}PAG0Multiplica una matriz de rotación a la izquierda para obtener P 1 P_{1}PAG1,Ahora mismo
P 1 = [ x 1 y 1 z 1 1 ] = [ w 2 + x 2 − y 2 − z 2 2 ( xy − wz ) 2 ( xz + wy ) 0 2 ( xy + wz ) w 2 − x 2 + y 2 − z 2 2 ( yz − wx ) 0 2 ( xz − wy ) 2 ( yz + wx ) w 2 − x 2 − y 2 + z 2 0 0 0 0 1 ] [ x 0 y 0 z P1 = \ izquierda[\begin{array}{c}\mathrm{x}_{1}\\\mathrm{y}_{1}\\\mathrm{z}_{1}\\1\end{ array}\ derecha]=\izquierda[\begin{array}{cccc}\mathrm{w}^{2}+\mathrm{x}^{2}-\mathrm{y}^{2}-\mathrm{ z}^ {2} & 2(\mathrm{xy}-\mathrm{wz}) & 2(\mathrm{xz}+\mathrm{wy}) & 0\\2(\mathrm{xy}+\mathrm {wz} ) & \mathrm{w}^{2}-\mathrm{x}^{2}+\mathrm{y}^{2}-\mathrm{z}^{2}&2(\mathrm{ yz}-\ mathrm{wx})&0\\2(\mathrm{xz}-\mathrm{wy})&2(\mathrm{yz}+\mathrm{wx})&\mathrm{w}^{ 2}-\mathrm{ x}^{2}-\mathrm{y}^{2}+\mathrm{z}^{2}&0\\0&0&0&1\end{array}\right ]\left[\begin{array}{c}\ matemáticas{x}_{0}\\\mathrm{y}_{0}\\\mathrm{z}_{0}\\1\end{array}\right]P1_ _= X1y1z11 = w2+X2y2z22 ( xy+wz )2 ( xzwy )02 ( xywz )w2X2+y2z22 ( yz+wx )02 ( xz+wy )2 ( yzwx )w2X2y2+z200001 X0y0z01

Matriz de transformación compuesta

Un punto P 0 ( x 0 , y 0 , z 0 ) en el espacio conocido P_ {0} (x_ {0}, y_ {0}, z_ {0})PAG0( x0,y0,z0) , entoncesP 0 P_{0}PAG0Al girar en diferentes ángulos alrededor del eje X, el eje Y y el eje Z (suponiendo que la relación de rotación está representada por la cuádruple anterior), el punto P 1 (x 1, y 1, z 1) P_ {1}(se obtiene x_{1}, y_{1}, z_{1})PAG1( x1,y1,z1) , y luego el puntoP 1 P_{1}PAG1Traducir tx, ty, tz t_{x}, t_{y}, t_{z} a lo largo del eje X, el eje Y y el eje Z respectivamentetxtytzdistancia, obtenga el punto P 2 ( x 2 , y 2 , z 2 ) P_{2}(x_{2}, y_{2}, z_{2})PAG2( x2,y2,z2) , entonces el proceso de transformación es
P 2 = [ x 2 y 2 z 2 1 ] = [ 1 0 0 tx 0 1 0 ty 0 0 1 tz 0 0 0 1 ] [ w 2 + x 2 − y 2 − z 2 2 ( xy − wz ) ( xz + wy ) 0 2 ( xy + wz ) w 2 − x 2 + y 2 − z 2 2 ( yz − wx ) 0 2 ( xz − wy ) 2 ( yz + wx ) w 2 − x 2 − y + z 2 0 0 0 0 1 ] [ x 0 y 0 z 0 1 ] P2 = \left[\begin{array}{c}\mathrm{x}_{2}\\\mathrm{y}_{2 } \ \\mathrm{z}_{2}\\1\end{array}\right]=\left[\begin{array}{cccc}1 & 0 & 0 & \mathrm{t}_{\mathrm {x }}\\0&1&0&\mathrm{t}_{\mathrm{y}}\\0&0&1&\mathrm{t}_{\mathrm{z}}\\0& 0 & 0 & 1 \end{array}\right] \left[\begin{array}{cccc}\mathrm{w}^{2}+\mathrm{x}^{2}-\mathrm{y}^ {2}-\mathrm{z}^{2} &2(\mathrm{xy}-\mathrm{wz})&2(\mathrm{xz}+\math{wy})&0\\2(\ mathrm{xy}+\mathrm{wz}) & \mathrm{w }^{2}-\mathrm{x}^{2}+\mathrm{y}^{2}-\mathrm{z}^{2} &2(\mathrm{yz}-\mathrm{wx}) & 0 \\ 2(\mathrm{xz}-\mathrm{wy}) & 2(\mathrm{yz}+\mathrm{wx}) & \mathrm {w}^{2}-\mathrm{x}^{2}-\mathrm{y}^{2}+\mathrm{z}^{2}&0\\0&0&0&1\end {array}\right]\ izquierda[\begin{array}{c}\mathrm{x}_{0}\\\mathrm{y}_{0}\\\mathrm{z}_{0}\\ 1 \end{array}\ bien]P2_ _= X2y2z21 = 100001000010txtytz1 w2+X2y2z22 ( xy+wz )2 ( xzwy )02 ( xywz )w2X2+y2z22 ( yz+wx )02 ( xz+wy )2 ( yzwx )w2X2y2+z200001 X0y0z01
Eso es P 0 P_ {0}PAG0Multiplique una matriz de rotación hacia la izquierda y luego una matriz de traslación hacia la izquierda para obtener P 2 P_{2}PAG2, este proceso se puede expresar como una matriz de transformación compuesta como

Matriz de transformación = Matriz de traducción ⋅ Matriz de rotación = [ w 2 + x 2 − y 2 − z 2 2 ( xy − wz ) 2 ( xz + wy ) tx 2 ( xy + wz ) w 2 − x 2 + y 2 − z 2 2 ( yz − wx ) ty 2 ( xz − wy ) 2 ( yz + wx ) w 2 − x 2 − y 2 + z 2 tz 0 0 0 1 ] TransformMatrix = TranslationMatrix \cdot RotationMatrix = \left[ \begin{array}{cccc}\mathrm{w}^{2}+\mathrm{x}^{2}-\mathrm{y}^{2}-\mathrm{z}^{2} & 2( \mathrm{xy}-\mathrm{wz}) & 2(\mathrm{xz}+\mathrm{wy}) &\mathrm{t}_{\mathrm{x}} \\ 2(\mathrm {xy} +\mathrm{wz}) & \mathrm{w}^{2}-\mathrm{x}^{2}+\mathrm{y}^{2}-\mathrm{z}^{2} & 2( \mathrm{yz}-\mathrm{wx}) &\mathrm{t}_{\mathrm{y}}\2(\mathrm{xz}-\mathrm{wy}) & 2(\mathrm {yz}+ \mathrm{wx}) & \mathrm{w}^{2}-\mathrm{x}^{2}-\mathrm{y}^{2}+\mathrm{z}^{2} &\mathrm{ t}_{\mathrm{z}}\\0&0& 0 y 1\end{array}\right]T r an s f o m M a t r i x=T r an s l a c i o n M a t r i xR o t a c i ó n M a t r i x= w2+X2y2z22 ( xy+wz )2 ( xzwy )02 ( xywz )w2X2+y2z22 ( yz+wx )02 ( xz+wy )2 ( yzwx )w2X2y2+z20txtytz1
Es decir, primero rotar y luego trasladar.


Sistema coordinado

En el sistema de percepción de la conducción autónoma, además del sistema de coordenadas del vehículo autónomo, la detección de objetivos 3D también involucra sensores como cámaras y lidar, y cada sensor tiene su propio sistema de coordenadas. Como se muestra abajo

  • El eje x del sistema de coordenadas lidar
    está hacia adelante, el eje y está hacia la izquierda y el eje z está hacia arriba. El eje de gravedad es el eje z, la dirección de referencia es la dirección positiva del eje x y el ángulo de rumbo rz es el ángulo entre la dirección de avance del objetivo y el eje x en el sistema de coordenadas lidar.

  • El eje z del sistema de coordenadas de la cámara
    está hacia adelante, el eje y está hacia abajo y el eje x está hacia la derecha. El eje de gravedad es el eje y, la dirección de referencia es la dirección positiva del eje x y el ángulo de rumbo ry es el ángulo entre la dirección de avance del objetivo y el eje x en las coordenadas de la cámara.


La relación entre el ángulo de guiñada, el ángulo de observación y el ángulo de azimut objetivo.

Como se muestra en la figura siguiente, el azul es el vehículo del ego, el verde es el objeto objetivo y el eje z del sistema de coordenadas de la cámara está hacia adelante.

  • El ángulo de azimut theta se define como el ángulo entre la línea que conecta el vehículo y el objeto objetivo que se desvía de la dirección de avance del vehículo.

  • El ángulo de rumbo rotación_y es el ángulo entre la dirección del objetivo y la dirección positiva del eje X de la cámara (la dirección en el sentido de las agujas del reloj es positiva), que describe la orientación del objetivo en el mundo real. Como se muestra en la figura ∠ BOC \angle BOC∠Como se muestra en BOC . El rango de valores de rotaiton_y es[ − π , π ] [-\pi, \pi][ π ,π ] , no cambia con el cambio de la posición objetivo,

  • El ángulo de observación alfa describe la orientación del objetivo en relación con la perspectiva de la cámara. Se define como el centro del origen de la cámara y la línea que conecta el origen de la cámara con el centro del objeto como el radio. El objetivo gira alrededor de la gravedad. eje hasta que la dirección de avance del objetivo sea la misma que la del vehículo del ego. El ángulo requerido, como se muestra en la figura ∠ BOD \angle BOD Se muestra BO D. El rango de valores del ángulo de observación alfa es[ − π , π ] [-\pi, \pi][ π ,π ] , cambia con el cambio de la posición objetivo

Rotation_y y Alpha se pueden convertir entre sí. Porque ∠ AOC = 9 0 ∘ − theta \angle \mathrm{AOC}=90^{\circ}-\text { theta }AOC=9 0 theta  ,
ángulo∠ AOB = ∠ AOC − ∠ BOC = 9 0 ∘ − theta − rotaion_y \angle \mathrm{AOB}=\angle \mathrm{AOC}-\angle \mathrm{BOC}=90^{\circ} - \text {theta}-\text{rotación\_y}AOB=AOCBOC=9 0 theta  rotaion_y 
y porque∠ AOB + ∠ DBO = 9 0 ∘ \angle \mathrm{AOB} + \angle \mathrm{BOD} = 90^{\circ}AOB+DBO=9 0 , Alfa interno
 = ∠ DBO = 9 0 ∘ − ∠ AOB = theta + rotación_y \text { alfa }=\angle \mathrm{BOD}=90^{\circ}-\angle \mathrm{AOB}=\text { theta }+\text { rotación\_y } alfa =DBO=9 0AOB= theta + rotación_y 
considera que tanto rotación_y como alfa son negativos en el sentido contrario a las agujas del reloj, por lo que
− alfa = theta − rotación_y -\text { alfa }=\text { theta }-\text { rotación\_y } alfa = theta  rotación_y 

 alfa = rotación_y − theta \text { alfa }=\text { rotación\_y } - \text { theta } alfa = rotación_y  theta 


conjunto de datos

NuEscenas

Formato original de datos de nube de puntos

Hay nubes de puntos lidar y nubes de puntos de radar en los datos de NuScenes. Lidar es una nube de puntos densa con coordenadas tridimensionales, el radar es una nube de puntos escasa. El radar general de ondas milimétricas FMCW no tiene información de altura (el radar 4D puede detectar la altura). Hay relativamente muchos estudios sobre datos de nubes de puntos LIDAR, por lo que no entraré en demasiadas introducciones aquí.

  • Formato de nube de puntos de radar
    El formato de datos de nube de puntos del radar es como se muestra en la siguiente figura: cada línea es un punto, incluidas las coordenadas, velocidad, intensidad y otra información del punto. Los datos de la nube de puntos generalmente se guardan a través de archivos .pcd, el encabezado y otra información están en texto sin formato y los datos de la nube de puntos específicos son datos binarios.

  • Visualización

    Al observar el mismo archivo pcd desde diferentes ángulos, el radar de ondas milimétricas ordinario no puede medir información de altura, por lo que el valor del eje Z en la imagen de la derecha es el mismo.

  • Verificar datos de altitud

    Los valores son todos 0.

Transformación de coordenadas

El conjunto de datos de Nuscenes se divide en sistema de coordenadas global, sistema de coordenadas del cuerpo, sistema de coordenadas de cámara, sistema de coordenadas de píxeles, sistema de coordenadas de radar y sistema de coordenadas lidar. Excepto el sistema de coordenadas global, todos son sistemas de coordenadas relativas y el valor real de la anotación se basa en el sistema de coordenadas global.

El principio básico

La mayoría de los datos ajustados por el algoritmo se basan en el sistema de coordenadas del sensor. Los valores verdaderos anotados deben convertirse del sistema de coordenadas global al sistema de coordenadas del sensor respectivo. Todas las conversiones primero deben convertirse al sistema de coordenadas del cuerpo a través de los parámetros externos del sensor y luego se convierten al sistema de coordenadas del sensor.

De la siguiente manera, convierta el cuadro de etiqueta en el sistema de coordenadas global al sistema de coordenadas del sensor

 # Move box to ego vehicle coord system.
 box.translate(-np.array(pose_record['translation']))
 box.rotate(Quaternion(pose_record['rotation']).inverse)

 #  Move box to sensor coord system.
 box.translate(-np.array(cs_record['translation']))
 box.rotate(Quaternion(cs_record['rotation']).inverse)

Cómo convertir entre dos sistemas de coordenadas

La conversión se realiza a través de las matrices de transformación de coordenadas (matriz de traducción + matriz de rotación) en los dos sistemas de coordenadas. La cámara debe convertirse al sistema de coordenadas de píxeles mediante parámetros internos adicionales.

def transform_matrix(translation: np.ndarray = np.array([0, 0, 0]),
                    rotation: Quaternion = Quaternion([1, 0, 0, 0]),
                    inverse: bool = False) -> np.ndarray:
    """
    Convert pose to transformation matrix.
    :param translation: <np.float32: 3>. Translation in x, y, z.
    :param rotation: Rotation in quaternions (w ri rj rk).
    :param inverse: Whether to compute inverse transform matrix.
    :return: <np.float32: 4, 4>. Transformation matrix.
    """
    tm = np.eye(4)

    if inverse:

        rot_inv = rotation.rotation_matrix.T
        trans = np.transpose(-np.array(translation))
        tm[:3, :3] = rot_inv
        tm[:3, 3] = rot_inv.dot(trans)
    else:
        # rotation.rotation_matrix 表示将四元素 rotation 转换为旋转矩阵
        # 并将旋转矩阵赋值给对角矩阵 tm 的前三行前三列
        tm[:3, :3] = rotation.rotation_matrix
        # 将矩阵 tm 的前三行第四列赋值为 translation
        tm[:3, 3] = np.transpose(np.array(translation))
    # 最后返回一个 4×4 的复合变换矩阵
    return tm

La matriz de transformación devuelta arriba es una matriz de transformación compuestatranslation , que incluye información de posición ( ) e información de rotación rotation(). Con respecto a la definición y relación de cuaterniones, matrices de rotación, matrices de traducción y matrices de transformación compuestas, consulte la explicación detallada del código de convert_nuScenes . Archivo三、main 函数 .py

Transformación de coordenadas durante la fusión multisensor

Diferentes sensores tienen diferentes frecuencias de recolección y no se activan sincrónicamente. Los datos de un sensor deben proyectarse al sistema de coordenadas global y luego proyectarse a otro sensor a través del sistema de coordenadas global para lograr la alineación del tiempo. La compensación del movimiento se realiza con la ayuda de un sistema de coordenadas global (sistema de coordenadas absoluto), completando así la alineación temporal entre diferentes sensores.

  • Parámetro externo del radar: matriz de transformación compuesta del sistema de coordenadas del radar al sistema de coordenadas del ego (cuerpo).
  • Parámetro externo de la cámara: matriz de transformación compuesta del sistema de coordenadas de la cámara al sistema de coordenadas del ego
  • ego_pose: matriz de transformación compuesta del sistema de coordenadas del ego al sistema de coordenadas global

Finalmente, la nube de puntos se proyecta en el sistema de coordenadas del cuadro actual mediante la multiplicación de matrices para completar la alineación temporal.

# Fuse four transformation matrices into one and perform transform.
trans_matrix = reduce(np.dot, [ref_from_car, car_from_global, global_from_car, car_from_current])
velocity_trans_matrix = reduce(np.dot, [ref_from_car_rot, car_from_global_rot, global_from_car_rot, car_from_current_rot])
current_pc.transform(trans_matrix)

Preprocesamiento de datos

gatito


REFERENCIAS

Supongo que te gusta

Origin blog.csdn.net/weixin_41817841/article/details/128370378
Recomendado
Clasificación