slam数学补充

协方差矩阵的计算

M=(p̃ − μ)(p̃ − μ) T / (n+1)
μ = Σ p̃ / (n+1)

首先需要了解协方差矩阵的特征值和特征向量有什么意义？

对于一个协方差矩阵，其特征值可以告诉我们数据在不同方向上的变化程度。特征值越大，表示数据在对应特征向量方向上的变化程度越大；特征值越小，则表示数据在对应特征向量方向上的变化程度越小。通过分析特征值的大小，我们可以确定数据的主要变化方向和次要变化方向。
若协方差矩阵M为3×3矩阵，
若a11的值大，则表明这些点在x轴上变化大
若a22的值大，则表明这些点在y轴上变化大
若a33的值大，则表明这些点在z轴上变化大
若a12的值大，则表明这些点在x轴和y轴上变化大
若a13的值大，则表明这些点在x轴和z轴上变化大
若a23的值大，则表明这些点在y轴和z轴上变化大
特征值λ1表示了数据在特征向量v1方向上的变化程度，如果λ1较大，说明数据在特征向量v1方向上的变化较大，特征向量v1表示了数据的主要变化方向。

如何根据特征值判断点集收敛为直线

为了判断点集是否为一条线，可通过第一个大的特征值是否大于第二大的特征值几倍，来判断是否为直线

协方差矩阵的意义 以及 特征向量和特征值的关系

从数据表面看：
当M(n,n)过大意味着在数据均值的n维上变化较大，反之M(n,n)过小意味着数据在均值 的n维上变化不大。
当M(n,m)过大意味着数据在均值的n和m维上变化较大，数据在m和n轴的相关性较强。
反之M(n,m)过小意味着数据在均值的n和m维上变化较小，数据在m和n轴的相关性较弱。
从特征向量和特征值看：
M * n = a * n (其中M为协方差矩阵,n为特征向量,a为特征值)
不知道如何表达，有点抽象
协方差矩阵的特征向量意义：样本在均值处拟合(投影？)的直线
协方差矩阵的特征值意义：表示样本拟合直线的程度，越大意味着样本越偏离于特征向量，特征值为1时拟合最好
从迹和行列式看：
迹：所有样本的每个维度和均值的差值平方和
行列式：越大误差越大

点乘

A·B = |A| |B| cosθ
A·B = x1 * x2 + y1 * y2 + z1 * z2

叉乘

A X B = (y1 z2 - y2 z1, x2 z1 - x1 z2, x1 y2 - x2 y1)叉乘的结果垂直于向量AB，且结果的模长大小等于向量A和B组成的平行四边形面积

RPY转换为旋转矩阵

绕目标姿态xyz方向 rpy角 外旋 左乘 不动坐标轴：
R1 = Rotz（y）× Roty（p） × Rotx（r）
外旋，根据目标姿态的xyz轴进行旋转rpy角度

绕自身zyx方向 ypr角 Euler角 内旋 右乘 动坐标轴：
R2 = Rotz（y）× Roty（p） × Rotx（r）
内旋，根据自身姿态的xyz轴进行旋转rpy角度

注意R1=R2不意味着内旋的旋转矩阵等于外旋的旋转矩阵，而是zyx顺序的内旋等于xyz的外旋
内旋和外旋等价性为：xyz顺序的外旋 RzRyRx 和 zyx顺序的内旋 RzRyRx 之间差一个负号

    Rotx << 1,   0, 0,
          0,  cx, -sx,
          0,  sx, cx;

    Roty << cy, 0, sy,
           0, 1, 0,
         -sy, 0, cy;

    Rotz << cz, -sz, 0,
          sz,  cz, 0,
          0 ,   0, 1;

pcl::getTransformation ()

template <typename Scalar> void 
pcl::getTransformation (Scalar x, Scalar y, Scalar z, 
                        Scalar roll, Scalar pitch, Scalar yaw, 
                        Eigen::Transform<Scalar, 3, Eigen::Affine> &t)
{
  Scalar A = cos (yaw),  B = sin (yaw),  C  = cos (pitch), D  = sin (pitch),
         E = cos (roll), F = sin (roll), DE = D*E,         DF = D*F;

  t (0, 0) = A*C;  t (0, 1) = A*DF - B*E;  t (0, 2) = B*F + A*DE;  t (0, 3) = x;
  t (1, 0) = B*C;  t (1, 1) = A*E + B*DF;  t (1, 2) = B*DE - A*F;  t (1, 3) = y;
  t (2, 0) = -D;   t (2, 1) = C*F;         t (2, 2) = C*E;         t (2, 3) = z;
  t (3, 0) = 0;    t (3, 1) = 0;           t (3, 2) = 0;           t (3, 3) = 1;
}