AMCL系列之pf_vector.c

主要看下面这个函数，用到了eig.c。

// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
// matrix [d] such that a = r d r^T.
//将协方差矩阵分解成旋转矩阵和对角矩阵
void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a)
{
  int i, j;

  double aa[3][3];//矩阵A
  double eval[3];//特征向量V
  double evec[3][3];//特征值组成的对角矩阵
  for (i = 0; i < 3; i++)
  {
    for (j = 0; j < 3; j++)
    {
      aa[i][j] = a.m[i][j];
    }
  }
  //使用eigen特征值分解得到特征向量和特征值组成的对角矩阵
  // Compute eigenvectors/values
  eigen_decomposition(aa,evec,eval);

  *d = pf_matrix_zero();
  for (i = 0; i < 3; i++)
  {
    d->m[i][i] = eval[i];
    for (j = 0; j < 3; j++)
    {
      r->m[i][j] = evec[i][j];
    }
  }
  return;
}