AMCL系列之pf_pdf.c

//创建高斯pdf：使用协方差分解，然后pdf->cd更新，得到协方差分解的对角矩阵;
//输入为mean和covariance
// Create a gaussian pdf 
pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
{
  pf_matrix_t cd;
  pf_pdf_gaussian_t *pdf;
  
  /*calloc函数，其原型void *calloc(size_t n, size_t size)；
其比malloc函数多一个参数，并不需要人为的计算空间的大小，比如如果他要申请20个int类型空间，会int *p = (int *)calloc(20, sizeof(int)）,这样就省去了人为空间计算的麻烦。但这并不是他们之间最重要的区别，malloc申请后空间的值是随机的，并没有进行初始化，而calloc却在申请后，对空间逐一进行初始化，并设置值为0;
*/
  pdf = calloc(1, sizeof(pf_pdf_gaussian_t));

  pdf->x = x;
  pdf->cx = cx;

  //将协方差矩阵分解成旋转矩阵和对角矩阵
  // Decompose the convariance matrix into a rotation
  // matrix and a diagonal matrix.
  // Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
  // matrix [d] such that a = r d r^T.

  //使用协方差分解，得到pdf->cd. -->对角矩阵；
  pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);

  pdf->cd.v[0] = sqrt(cd.m[0][0]);
  pdf->cd.v[1] = sqrt(cd.m[1][1]);
  pdf->cd.v[2] = sqrt(cd.m[2][2]);

  // Initialize the random number generator

  srand48(++pf_pdf_seed);

  return pdf;
}