功能:SE3中求点反对称矩阵

格式:SLAM_SE3_PAntiSym(p)

P:为1*4或者4*1的矩阵变量

说明:
1、程序执行成功返回4*6的矩阵变量

原理:
1、设p由一个3*1的向量a与标量b组成，即 p = [a;b]
2、设A = {SLAM_AntiSymMatrix<矩阵运算\SLAM_AntiSymMatrix>}(a)
3、设z = {Zeros<矩阵运算\Zeros>}(1,6)
4、设I为3*3的单位矩阵
5、则本函数返回这么一个矩阵 C = [b * I, -A; z]

例子:
p = [1,2,3,4];
s = SLAM_SE3_PAntiSym(p)//回车得到如下结果
s =
[ 4   0   0   0   3  -2
  0   4   0  -3   0   1
  0   0   4   2  -1   0
  0   0   0   0   0   0]