Cambiando el marco de referencia de una distribución Gaussiana

El trabajo de funciones probabilísticas gausianas en el espacio es algo muy habitual en robótica. Este tipo de distribuciones permite expresar una posición o pose (por ejemplo de un objeto u objetivo) de la cual se tiene cierta incertidumbre. Desde mi punto de vista se trata de una temática bastante interesante y me está permitiendo profundizar en algunos conceptos matemáticos como autovalores, números complejos y quaterniones. Otros posts previos que realicé en el pasado con temáticas relacionadas son:

Una función gausiana está compuesta de su vector media \mu y su matriz de covarianza \Sigma y se denota como P(x)\sim\mathcal{N}(\mu,\Sigma). El problema consiste en modificar estos parámetros para pasar del marco de referencia F_{A} al marco de referencia F_{B}. Estos marcos de referencia están descritos por una transformación lineal que incluye rotación y translación respecto a un marco de referencia universal, de modo que:

F_{A}\equiv(R_{A},T_{A})

F_{B}\equiv(R_{B},T_{B})

Cambio del marco de referencia del vector media \mu

El cambio de referencia del vector media es trivial dada una transformación TF entre marcos de referencia básicamente consiste en aplicar la transformación lineal al vector \mu_{A}:

\mu_{B} = R \mu_{A} + T

Donde:

R=R_{B}R_{A}^{-1}
T=T_{B}-R_{B}T_{A}

De echo en ocasiones los valores universales de los marcos de referencia R_{A}, T_{A}, R_{B}  y T_{B} no son conocidos, sino que directamente la relación entre ambos marcos TF\equiv (R,T).

Cambio del marco de referencia de la matriz de Covarianza \Sigma

¿Pero que pasa con la matriz de covarianza? En el caso de un problema unidimensional, el cambio de referencia del origen de coordenadas no afectaría a la matriz de covarianza \Sigma=\begin{bmatrix} \sigma^{2} \end{bmatrix} (que es la varianza). Por ejemplo: P(x)\sim\mathcal{N}(\mu_{A}=3,\Sigma_{A}=5) se convertiría en P(x)\sim\mathcal{N}(\mu_{B}=-7,\Sigma_{B}=5) si cambiaramos el origen de coordenadas desde el O=0 a O=10.

Para una función gausiana 2D o 3D es mucho más complicado. La matriz de covarianza representa la orientación de la distribución además de la  escala de la variabilidad en dentro de los ejes principales de esta rotación. A la derecha se muestra una figura de una función gausiana 2D. En una función gaussiana (creo, a ver si un matemático me lo confirma) que la probabilidad se extiende en N vectores ortogonales (autovectores) con diferentes escalas (autovalores) verificando que:

\Sigma = E (l I) E^{t}

Siendo l el vector de autovalores, el punto curioso es que la matriz E es una matriz de rotación cuyas columnas son los autovectores de \Sigma.  Es intuitivo ver que si el marco de referencia cambia, las coordenadas de estos autovectores cambiará, sin embargo la escala (autovalores) se mantendrá constante. Parece entonces sencillo.

Si estamos trabajando en el marco de referencia F_{A}F_{B} es conocido, entonces basta con cambiar el marco de referencia de los autovectores:

E_{B}=RE_{A}

\Sigma_{B}=RE_{A}(lI)(RE_{A})^{t}

o lo que es lo mismo si tenemos la información completa de F_{A}F_{B}:

\Sigma_{B}=R_{B}R_{A}^{-1}E_{A}(lI)(R_{B}R_{A}^{-1}E_{A})^{t}

Casos particulares que aún no se resolver

Sin embargo existen casos particulares y estos son los que a mi, personalmente me inquietan y aún no tengo claro como resolver:

  1. ¿Qué ocurre si la matriz de covarianza no es diagonalizable? Por ejemplo, se tiene total certeza de su posición en X, su posición en Y y algo de incertidumbre en Z. Siendo \Sigma_{A}=\begin{bmatrix}  0 & 0 & 0\\  0 & 0 & 0\\  0 & 0 & 2.3  \end{bmatrix} ¿Podría obtenerse la matriz E_{A} si solo tenemos un autovector?
  2. ¿Qué ocurre cuando trabajamos (en lugar de con un vector “posición” en 3D) con una “pose en el espacio” xyz+rpy 6D con una matriz de covarianza de 36 valores, donde se mezclan distintas unidades? Este tipo de representación gausiana es también muy utilizada en robótica, sin embargo, el abordaje del problema de cambio de sistema referencia de este tipo de distribuciones es algo que no he logrado localizar en ningún sitio. Parece un caso muy extraño dado que además los valores de rpy dependen en sí mismos del los ejes x,y,z del marco de referencia actual.
Ideas problema 1:
En el ejemplo tratado parece claro que el único autovector existente es v_0=\begin{vmatrix}\\ 0\\ 0\\ 1 \end{vmatrix} y su autovector \lambda_0=2.3 ya que verifica que \Sigma v_0 = \lambda v_0 . Intuyo que dada la precondición de ortogonalidad de los autovectores de la matriz E(¿inventado?) parece razonable que solo necesitamos un autovector para calcular E. De hecho  cualquier E que verifique que l=\begin{vmatrix}\\ \lambda_0\\ \lambda_1=0\\\lambda_2=0 \end{vmatrix} y sea ortogonal parece suficiente ¿sera correcto este razonamiento?Duda: Pienso que una matriz de covarianza no nula debería tener al menos un autovector real, pero de esto, no estoy seguro. Si fuera así, el problema estaría resuelto.
Ideas problema 2:
Creo que dado que el cambio de coordenadas en robótica solo afectaría a los ejes x, y y z el problema se simplificaría. El vector \mu para x, y y z cambiaría de la misma forma. Por el contrario los valores rpy en lugar de ser rotados serían trasladados. \mu_{B}= R* \mu_{A} + T donde:
R=\begin{bmatrix}  R_{xyz} & \O \\  \O & I\\\end{bmatrix}
y
T=\begin{bmatrix} t_x \\ t_y \\ t_z \\ getRoll(R) \\ getPitch(R) \\ getYaw(R) \\\end{bmatrix}
De esta forma las componentes de \Sigma_A definidas por covarianzas angulares, como \sigma_{row,pitch}=cov(row,pitch) permanecerían constantes tras la transformación TF, mientras que las componentes de la matríz como \sigma_{x,yaw}=cov(x,yaw) cambiarían su valor. Sin embargo, todo esto son por ahora solo intuiciones. ¿es correcto este razonamiento?¿Alguna ayuda?

2 Responses to Cambiando el marco de referencia de una distribución Gaussiana

  1. Dejo por aquí un trozo de código c++ con la librería matemática Eigen incompleto que he hecho de prueba que puede servirme en un futuro🙂

    //5.1 copy the covariance matrix to an eigenMatrix
    //http://eigen.tuxfamily.org/dox/TutorialMatrixClass.html

    Eigen::Matrix covariance(&state_correction->twist.covariance[0]);

    //5.2 get the eigenvalues and vectors
    //http://eigen.tuxfamily.org/dox/classEigen_1_1EigenSolver.html

    Eigen::Matrix<std::complex, 3, 1> eigvalues = covariance.eigenvalues();
    Eigen::EigenSolver<Eigen::Matrix > es(covariance, true);

    Eigen::Matrix<std::complex, 3, 3> eigenvectors = es.eigenvectors();

    tf::Vector3 eigen0(eigenvectors(0, 0).real(), eigenvectors(0, 1).real(), eigenvectors(0, 2).real());
    tf::Vector3 eigen1(eigenvectors(1, 0).real(), eigenvectors(1, 1).real(), eigenvectors(1, 2).real());
    tf::Vector3 eigen2(eigenvectors(2, 0).real(), eigenvectors(2, 1).real(), eigenvectors(2, 2).real());

    //TODO: We are still tring to solve this problem: https://geus.wordpress.com/2012/02/17/cambiando-el-marco-de-referencia-de-una-distribucion-gaussiana/
    //Eigen::Vector3d eigen0=eigenvectors.col(0);
    //Eigen::Vector3d eigen1(eigenvectors.col(1));
    //Eigen::Vector3d eigen2(eigenvectors.col(2));

    //eig0 = tf::Vector3(eigvectors[0][0], eigvectors[0, 0])

  2. Javier dice:

    Con respecto a la pregunta qué pasa cuando tenemos incertidumbre en posición y rotación.
    Echa un ojo a este paper: A framework for uncertainty and validation of 3-D registration methods based on Points and frames, Pennec et al.

    Un saludo.

Responder

Introduce tus datos o haz clic en un icono para iniciar sesión:

Logo de WordPress.com

Estás comentando usando tu cuenta de WordPress.com. Cerrar sesión / Cambiar )

Imagen de Twitter

Estás comentando usando tu cuenta de Twitter. Cerrar sesión / Cambiar )

Foto de Facebook

Estás comentando usando tu cuenta de Facebook. Cerrar sesión / Cambiar )

Google+ photo

Estás comentando usando tu cuenta de Google+. Cerrar sesión / Cambiar )

Conectando a %s

A %d blogueros les gusta esto: