网站推广服务合同判决书,电商店铺,室内设计师素材网,无锡建网站企业原始 Markdown文档、Visio流程图、XMind思维导图见#xff1a;https://github.com/LiZhengXiao99/Navigation-Learning 文章目录 一、Earth 类#xff1a;地球参数和坐标转换1、gravity()#xff1a;正常重力计算2、meridianPrimeVerticalRadius()#xff1a;计算子午圈半径… 原始 Markdown文档、Visio流程图、XMind思维导图见https://github.com/LiZhengXiao99/Navigation-Learning 文章目录 一、Earth 类地球参数和坐标转换1、gravity()正常重力计算2、meridianPrimeVerticalRadius()计算子午圈半径 RM、卯酉圈半径 RN3、RN()计算卯酉圈主半径 RN4、cne()n系(导航坐标系)到e系(地心地固坐标系)转换矩阵5、qne()计算n系(北东地)到e系(ECEF)转换四元数6、blh()从n系到e系转换四元数得到纬度和经度7、blh2ecef()大地坐标(经纬高)转地心地固坐标7、ecef2blh()地心地固坐标转大地坐标8、DRi() 计算 n 系相对位置转大地坐标相对位置的矩阵9、DR()计算大地坐标相对位置转 n 系相对位置的矩阵10、local2global()局部坐标(在origin处展开)转大地坐标11、global2local()大地坐标转局部坐标(在origin处展开)12、iewe()地球自转角速度投影到e系13、iewn()地球自转角速度投影到n系14、enwn()n系相对于e系转动角速度投影到n系 二、Rotation 类姿态转换1、matrix2quaternion()旋转矩阵转四元数2、quaternion2matrix()四元数转旋转矩阵3、matrix2euler()旋转矩阵转欧拉角4、quaternion2euler()四元数转欧拉角5、rotvec2quaternion()等效旋转矢量转四元数6、quaternion2vector()四元数转旋转矢量7、euler2matrix()欧拉角转旋转矩阵8、euler2quaternion()欧拉角转四元数9、skewSymmetric()计算三维向量反对称阵10、quaternionleft()、quaternionright()四元数矩阵 一、Earth 类地球参数和坐标转换 Earth 类里都是静态函数使用的时候直接类名::成员函数()文件的开头定义了一些椭球参数
/* WGS84椭球模型参数NOTE:如果使用其他椭球模型需要修改椭球参数 */
const double WGS84_WIE 7.2921151467E-5; /* 地球自转角速度*/
const double WGS84_F 0.0033528106647474805; /* 扁率 */
const double WGS84_RA 6378137.0000000000; /* 长半轴a */
const double WGS84_RB 6356752.3142451793; /* 短半轴b */
const double WGS84_GM0 398600441800000.00; /* 地球引力常数 */
const double WGS84_E1 0.0066943799901413156; /* 第一偏心率平方 */
const double WGS84_E2 0.0067394967422764341; /* 第二偏心率平方 */1、gravity()正常重力计算
重力是万有引力与离心力共同作用的结果随纬度升高离心力增大但引力减小、随高程升高引力减小共同作用下重力的计算公式如下 g L 9.7803267715 × ( 1 0.0052790414 × sin 2 L − 0.0000232718 × sin 2 2 L ) h × ( 0.0000000043977311 × sin 2 L − 0.0000030876910891 ) 0.0000000000007211 × sin 4 2 L g_{L}9.7803267715 \times\left(10.0052790414 \times \sin ^{2} L-0.0000232718 \times \sin ^{2} 2 L\right) \\ h\times(0.0000000043977311\times\sin ^{2} L-0.0000030876910891)0.0000000000007211\times\sin ^{4} 2 L gL9.7803267715×(10.0052790414×sin2L−0.0000232718×sin22L)h×(0.0000000043977311×sin2L−0.0000030876910891)0.0000000000007211×sin42L
static double gravity(const Vector3d blh) {double sin2 sin(blh[0]);sin2 * sin2;return 9.7803267715 * (1 0.0052790414 * sin2 0.0000232718 * sin2 * sin2) blh[2] * (0.0000000043977311 * sin2 - 0.0000030876910891) 0.0000000000007211 * blh[2] * blh[2];
}2、meridianPrimeVerticalRadius()计算子午圈半径 RM、卯酉圈半径 RN
返回值是 Vector2d第一个是子午圈主曲率半径 RM、第二个是卯酉圈主半径 RN R M R e ( 1 − e 2 ) ( 1 − e 2 sin 2 L ) 3 / 2 、 R N R e 1 − e 2 sin 2 L R_{M}\frac{R_{e}\left(1-e^{2}\right)}{\left(1-e^{2} \sin ^{2} L\right)^{3 / 2}}、R_{N}\frac{R_{e}}{\sqrt{1-e^{2} \sin ^{2} L}} RM(1−e2sin2L)3/2Re(1−e2)、RN1−e2sin2L Re
static Eigen::Vector2d meridianPrimeVerticalRadius(double lat) {double tmp, sqrttmp;tmp sin(lat); tmp * tmp;tmp 1 - WGS84_E1 * tmp;sqrttmp sqrt(tmp);return {WGS84_RA * (1 - WGS84_E1) / (sqrttmp * tmp), WGS84_RA / sqrttmp};
}3、RN()计算卯酉圈主半径 RN R N R e 1 − e 2 sin 2 L R_{N}\frac{R_{e}}{\sqrt{1-e^{2} \sin ^{2} L}} RN1−e2sin2L Re
static double RN(double lat) {double sinlat sin(lat);return WGS84_RA / sqrt(1.0 - WGS84_E1 * sinlat * sinlat);
}4、cne()n系(导航坐标系)到e系(地心地固坐标系)转换矩阵 C e n [ − sin φ 0 cos φ 0 1 0 − cos φ 0 − sin φ ] [ cos λ sin λ 0 − sin λ cos λ 0 0 0 1 ] [ − sin φ cos λ − sin φ sin λ cos φ − sin λ cos λ 0 − cos φ cos λ − cos φ sin λ − sin φ ] C_{e}^{n}\left[\begin{array}{ccc}-\sin \varphi 0 \cos \varphi \\ 0 1 0 \\ -\cos \varphi 0 -\sin \varphi\end{array}\right]\left[\begin{array}{ccc}\cos \lambda \sin \lambda 0 \\ -\sin \lambda \cos \lambda 0 \\ 0 0 1\end{array}\right]\\ \left[\begin{array}{ccc}-\sin \varphi \cos \lambda -\sin \varphi \sin \lambda \cos \varphi \\ -\sin \lambda \cos \lambda 0 \\ -\cos \varphi \cos \lambda -\cos \varphi \sin \lambda -\sin \varphi\end{array}\right] Cen −sinφ0−cosφ010cosφ0−sinφ cosλ−sinλ0sinλcosλ0001 −sinφcosλ−sinλ−cosφcosλ−sinφsinλcosλ−cosφsinλcosφ0−sinφ
static Matrix3d cne(const Vector3d blh) {double coslon, sinlon, coslat, sinlat;sinlat sin(blh[0]);sinlon sin(blh[1]);coslat cos(blh[0]);coslon cos(blh[1]);Matrix3d dcm;dcm(0, 0) -sinlat * coslon;dcm(0, 1) -sinlon;dcm(0, 2) -coslat * coslon;dcm(1, 0) -sinlat * sinlon;dcm(1, 1) coslon;dcm(1, 2) -coslat * sinlon;dcm(2, 0) coslat;dcm(2, 1) 0;dcm(2, 2) -sinlat;return dcm;
}5、qne()计算n系(北东地)到e系(ECEF)转换四元数
位置更新的时候调用此函数根据上一时刻经纬度得到上一时刻的 qne然后 qee * qne * qnn 得到当前时刻的 qne再调用下面的 blh() 得到经纬度。 q n e [ cos ( − π / 4 − φ / 2 ) cos ( λ / 2 ) − sin ( − π / 4 − φ / 2 ) sin ( λ / 2 ) sin ( − π / 4 − φ / 2 ) cos ( λ / 2 ) cos ( − π / 4 − sin / 2 ) sin ( λ / 2 ) ] ] \boldsymbol{q}_{n}^{e}\left[\begin{array}{c}\cos (-\pi / 4-\varphi / 2) \cos (\lambda / 2) \\ -\sin (-\pi / 4-\varphi / 2) \sin (\lambda / 2) \\ \sin (-\pi / 4-\varphi / 2) \cos (\lambda / 2) \\ \cos (-\pi / 4-\sin / 2) \sin (\lambda / 2)]\end{array}\right] qne cos(−π/4−φ/2)cos(λ/2)−sin(−π/4−φ/2)sin(λ/2)sin(−π/4−φ/2)cos(λ/2)cos(−π/4−sin/2)sin(λ/2)]
/* n系(导航坐标系)到e系(地心地固坐标系)转换四元数 */
static Quaterniond qne(const Vector3d blh) {Quaterniond quat;double coslon, sinlon, coslat, sinlat;coslon cos(blh[1] * 0.5);sinlon sin(blh[1] * 0.5);coslat cos(-M_PI * 0.25 - blh[0] * 0.5);sinlat sin(-M_PI * 0.25 - blh[0] * 0.5);quat.w() coslat * coslon;quat.x() -sinlat * sinlon;quat.y() sinlat * coslon;quat.z() coslat * sinlon;return quat;
}6、blh()从n系到e系转换四元数得到纬度和经度
位置更新的时候通过算当前时刻 n 系到 e 系转换四元数 qne然后调用此函数得到经纬度。
/* 从n系到e系转换四元数得到纬度和经度 */
static Vector3d blh(const Quaterniond qne, double height) {return {-2 * atan(qne.y() / qne.w()) - M_PI * 0.5, 2 * atan2(qne.z(), qne.w()), height};
}7、blh2ecef()大地坐标(经纬高)转地心地固坐标 x ( R N h ) cos L cos λ y ( R N h ) cos L sin λ z [ R N ( 1 − e 2 ) h ] sin L \begin{array}{l}x\left(R_{N}h\right) \cos L \cos \lambda \\ y\left(R_{N}h\right) \cos L \sin \lambda \\ z\left[R_{N}\left(1-e^{2}\right)h\right] \sin L\end{array} x(RNh)cosLcosλy(RNh)cosLsinλz[RN(1−e2)h]sinL
/* 大地坐标(纬度、经度和高程)转地心地固坐标 */
static Vector3d blh2ecef(const Vector3d blh) {double coslat, sinlat, coslon, sinlon;double rnh, rn;coslat cos(blh[0]);sinlat sin(blh[0]);coslon cos(blh[1]);sinlon sin(blh[1]);rn RN(blh[0]);rnh rn blh[2];return {rnh * coslat * coslon, rnh * coslat * sinlon, (rnh - rn * WGS84_E1) * sinlat};
}7、ecef2blh()地心地固坐标转大地坐标 B 0 arctan ( Z ( 1 − e 2 ) p ) N k a 1 − e 2 sin 2 B k − 1 H k p cos B k − 1 − N k B k arctan ( z ( 1 − e 2 N k N k ) p ) \begin{array}{c}B_{0}\arctan \left(\frac{Z}{\left(1-e^{2}\right) p}\right) \\ N_{k}\frac{a}{\sqrt{1-e^{2} \sin ^{2} B_{k-1}}} \\ H_{k}\frac{p}{\cos B_{k-1}}-N_{k} \\ B_{k}\arctan \left(\frac{z}{\left(1-\frac{e^{2} N_{k}}{N_{k}}\right)\ p }\right)\end{array} B0arctan((1−e2)pZ)Nk1−e2sin2Bk−1 aHkcosBk−1p−NkBkarctan (1−Nke2Nk) pz
static Vector3d ecef2blh(const Vector3d ecef) {double p sqrt(ecef[0] * ecef[0] ecef[1] * ecef[1]);double rn;double lat, lon;double h 0, h2;// 初始状态lat atan(ecef[2] / (p * (1.0 - WGS84_E1)));lon 2.0 * atan2(ecef[1], ecef[0] p);do {h2 h;rn RN(lat);h p / cos(lat) - rn;lat atan(ecef[2] / (p * (1.0 - WGS84_E1 * rn / (rn h))));} while (fabs(h - h2) 1.0e-4);return {lat, lon, h};
}8、DRi() 计算 n 系相对位置转大地坐标相对位置的矩阵
通过算出来的矩阵实现 ENU 和 LLH 之间的转换
捷联惯导求出的速度是 n 系的要转成经纬度增量就得用这个矩阵。杆臂误差补偿时也需要用这个函数因为杆臂是 n 系的算出的 IMU 坐标和给的 GNSS 解都是经纬高。存的位置是经纬高GNSS 量测更新时候计算的是 ENU 下位置的增量反馈的时候也需要此矩阵。 [ δ φ δ L δ H ] [ ( R M H ) − 1 0 0 0 ( R N H ) − 1 0 0 0 − 1 ] [ δ p N δ p E δ p B ] \left[\begin{array}{l}\delta \varphi \\ \delta L \\ \delta H\end{array}\right]\left[\begin{array}{ccc}\left(R_{M}H\right)^{-1} 0 0 \\ 0 \left(R_{N}H\right)^{-1} 0 \\ 0 0 -1 \end{array}\right]\left[\begin{array}{l}\delta \boldsymbol{p}_{N} \\ \delta \boldsymbol{p}_{E} \\ \delta \boldsymbol{p}_{B}\end{array}\right] δφδLδH (RMH)−1000(RNH)−1000−1 δpNδpEδpB
/* n系相对位置转大地坐标相对位置 */
static Matrix3d DRi(const Vector3d blh) {Matrix3d dri Matrix3d::Zero();Eigen::Vector2d rmn meridianPrimeVerticalRadius(blh[0]);dri(0, 0) 1.0 / (rmn[0] blh[2]);dri(1, 1) 1.0 / ((rmn[1] blh[2]) * cos(blh[0]));dri(2, 2) -1;return dri;
}9、DR()计算大地坐标相对位置转 n 系相对位置的矩阵
就是上面 DRI() 计算矩阵的倒数。 [ δ φ δ L δ H ] [ ( R M H ) 0 0 0 ( R N H ) 0 0 0 − 1 ] [ δ p N δ p E δ p B ] \left[\begin{array}{l}\delta \varphi \\ \delta L \\ \delta H\end{array}\right]\left[\begin{array}{ccc}\left(R_{M}H\right) 0 0 \\ 0 \left(R_{N}H\right) 0 \\ 0 0 -1 \end{array}\right]\left[\begin{array}{l}\delta \boldsymbol{p}_{N} \\ \delta \boldsymbol{p}_{E} \\ \delta \boldsymbol{p}_{B}\end{array}\right] δφδLδH (RMH)000(RNH)000−1 δpNδpEδpB
/* 大地坐标相对位置转n系相对位置 */
static Matrix3d DR(const Vector3d blh) {Matrix3d dr Matrix3d::Zero();Eigen::Vector2d rmn meridianPrimeVerticalRadius(blh[0]);dr(0, 0) rmn[0] blh[2];dr(1, 1) (rmn[1] blh[2]) * cos(blh[0]);dr(2, 2) -1;return dr;
}10、local2global()局部坐标(在origin处展开)转大地坐标
在 enwn() 中被调用为了方便能直接传入北东地n 系坐标计算 n 系相对于 e 系转动角速度在 n 系的投影。
static Vector3d local2global(const Vector3d origin, const Vector3d local) {Vector3d ecef0 blh2ecef(origin);Matrix3d cn0e cne(origin);Vector3d ecef1 ecef0 cn0e * local;Vector3d blh1 ecef2blh(ecef1);return blh1;
}11、global2local()大地坐标转局部坐标(在origin处展开)
好像整个程序中都没用到这个函数。
static Vector3d global2local(const Vector3d origin, const Vector3d global) {Vector3d ecef0 blh2ecef(origin);Matrix3d cn0e cne(origin);Vector3d ecef1 blh2ecef(global);return cn0e.transpose() * (ecef1 - ecef0);
}static Pose global2local(const Vector3d origin, const Pose global) {Pose local;Vector3d ecef0 blh2ecef(origin);Matrix3d cn0e cne(origin);Vector3d ecef1 blh2ecef(global.t);Matrix3d cn1e cne(global.t);local.t cn0e.transpose() * (ecef1 - ecef0);local.R cn0e.transpose() * cn1e * global.R;return local;
}12、iewe()地球自转角速度投影到e系 ω i e e [ 0 0 ω e ] T \boldsymbol{\omega}_{i e}^{e}\left[\begin{array}{lll}0 0 \omega_{e}\end{array}\right]^{T} ωiee[00ωe]T
static Vector3d iewe() {return {0, 0, WGS84_WIE};
}13、iewn()地球自转角速度投影到n系 ω i e n [ ω e cos φ 0 − ω e sin φ ] T \boldsymbol{\omega}_{i e}^{n}\left[\begin{array}{lll}\omega_{e} \cos \varphi 0 -\omega_{e} \sin \varphi\end{array}\right]^{T} ωien[ωecosφ0−ωesinφ]T
static Vector3d iewn(double lat) {return {WGS84_WIE * cos(lat), 0, -WGS84_WIE * sin(lat)};
}也可以直接传入北东地n 系坐标计算
static Vector3d iewn(const Vector3d origin, const Vector3d local) {Vector3d global local2global(origin, local);return iewn(global[0]);
}14、enwn()n系相对于e系转动角速度投影到n系
由载体运动线速度和地球曲率引起与东向、北向速度有关与天向速度无关 ω e n n [ v E R N h − v N R M h − v E tan φ R N h ] T \boldsymbol{\omega}_{e n}^{n}\left[\begin{array}{lll}\frac{v_{E}}{R_{N}h} \frac{-v_{N}}{R_{M}h} -\frac{v_{E} \tan \varphi}{R_{N}h}\end{array}\right]^{T} ωenn[RNhvERMh−vN−RNhvEtanφ]T
static Vector3d enwn(const Eigen::Vector2d rmn, const Vector3d blh, const Vector3d vel) {return {vel[1] / (rmn[1] blh[2]), -vel[0] / (rmn[0] blh[2]), -vel[1] * tan(blh[0]) / (rmn[1] blh[2])};
}同样也可以直接传入北东地n 系坐标计算
static Vector3d enwn(const Vector3d origin, const Vector3d local, const Vector3d vel) {Vector3d global local2global(origin, local);Eigen::Vector2d rmn meridianPrimeVerticalRadius(global[0]);return enwn(rmn, global, vel);
}二、Rotation 类姿态转换 1、matrix2quaternion()旋转矩阵转四元数
Eigen 中的四元数可以直接传入旋转矩阵三维矩阵构造
static Quaterniond matrix2quaternion(const Matrix3d matrix) {return Quaterniond(matrix);
}2、quaternion2matrix()四元数转旋转矩阵
四元数调用 toRotationMatrix() 函数转为旋转矩阵
static Matrix3d quaternion2matrix(const Quaterniond quaternion) {return quaternion.toRotationMatrix();
}3、matrix2euler()旋转矩阵转欧拉角
ZYX 旋转顺序前右下的 IMU输出 RPY
static Vector3d matrix2euler(const Eigen::Matrix3d dcm) {Vector3d euler;euler[1] atan(-dcm(2, 0) / sqrt(dcm(2, 1) * dcm(2, 1) dcm(2, 2) * dcm(2, 2)));if (dcm(2, 0) -0.999) {euler[0] atan2(dcm(2, 1), dcm(2, 2));euler[2] atan2((dcm(1, 2) - dcm(0, 1)), (dcm(0, 2) dcm(1, 1)));} else if (dcm(2, 0) 0.999) {euler[0] atan2(dcm(2, 1), dcm(2, 2));euler[2] M_PI atan2((dcm(1, 2) dcm(0, 1)), (dcm(0, 2) - dcm(1, 1)));} else {euler[0] atan2(dcm(2, 1), dcm(2, 2));euler[2] atan2(dcm(1, 0), dcm(0, 0));}// heading 0~2PIif (euler[2] 0) {euler[2] M_PI * 2 euler[2];}return euler;
}4、quaternion2euler()四元数转欧拉角
先调用 toRotationMatrix() 转为旋转矩阵再调用 matrix2euler() 转欧拉角
static Vector3d quaternion2euler(const Quaterniond quaternion) {return matrix2euler(quaternion.toRotationMatrix());
}5、rotvec2quaternion()等效旋转矢量转四元数
根据传入的旋转矢量计算向量的长度作为旋转的角度计算向量的归一化版本作为旋转的轴然后调用 AngleAxisd()将角度和轴转换为四元数。
static Quaterniond rotvec2quaternion(const Vector3d rotvec) {double angle rotvec.norm(); // 计算向量的长度作为旋转的角度Vector3d vec rotvec.normalized(); // 计算向量的归一化版本作为旋转的轴return Quaterniond(Eigen::AngleAxisd(angle, vec)); // 调用 AngleAxisd()将角度和轴转换为四元数
}6、quaternion2vector()四元数转旋转矢量
传入的四元数通过 Eigen::AngleAxisd 类的构造函数转换为角度轴angle-axis表示。角度轴是一个描述旋转的方法其中旋转角度和旋转轴是两个独立的部分。然后该函数返回这个角度轴表示的旋转的角度乘以旋转的轴得到一个三维向量。这个向量的 x、y 和 z 分量分别对应于旋转轴在x、y 和 z 轴上的分量而其长度或者说范数等于旋转角度。
static Vector3d quaternion2vector(const Quaterniond quaternion) {Eigen::AngleAxisd axisd(quaternion);return axisd.angle() * axisd.axis();
}7、euler2matrix()欧拉角转旋转矩阵
三个欧拉角分别转为 ZYX 角轴相乘之后构造旋转矩阵
static Matrix3d euler2matrix(const Vector3d euler) {return Matrix3d(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}8、euler2quaternion()欧拉角转四元数
三个欧拉角分别转为 ZYX 角轴相乘之后构造四元数
static Quaterniond euler2quaternion(const Vector3d euler) {return Quaterniond(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}9、skewSymmetric()计算三维向量反对称阵
static Matrix3d skewSymmetric(const Vector3d vector) {Matrix3d mat;mat 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1), vector(0), 0;return mat;
}10、quaternionleft()、quaternionright()四元数矩阵 P ∘ Q [ p 0 − p 1 − p 2 − p 3 p 1 p 0 − p 3 p 2 p 2 p 3 p 0 − p 1 p 3 − p 2 p 1 p 0 ] [ q 0 q 1 q 2 q 3 ] M P Q [ q 0 − q 1 − q 2 − q 3 q 1 q 0 q 3 − q 2 q 2 − q 3 q 0 q 1 q 3 q 2 − q 1 q 0 ] [ p 0 p 1 p 2 p 3 ] M Q ′ P \boldsymbol{P} \circ \boldsymbol{Q}\left[\begin{array}{cccc}p_{0} -p_{1} -p_{2} -p_{3} \\ p_{1} p_{0} -p_{3} p_{2} \\ p_{2} p_{3} p_{0} -p_{1} \\ p_{3} -p_{2} p_{1} p_{0}\end{array}\right]\left[\begin{array}{l}q_{0} \\ q_{1} \\ q_{2} \\ q_{3}\end{array}\right]\boldsymbol{M}_{P} \boldsymbol{Q}\left[\begin{array}{cccc}q_{0} -q_{1} -q_{2} -q_{3} \\ q_{1} q_{0} q_{3} -q_{2} \\ q_{2} -q_{3} q_{0} q_{1} \\ q_{3} q_{2} -q_{1} q_{0}\end{array}\right]\left[\begin{array}{l}p_{0} \\ p_{1} \\ p_{2} \\ p_{3}\end{array}\right]\boldsymbol{M}_{Q}^{\prime} \boldsymbol{P} P∘Q p0p1p2p3−p1p0p3−p2−p2−p3p0p1−p3p2−p1p0 q0q1q2q3 MPQ q0q1q2q3−q1q0−q3q2−q2q3q0−q1−q3−q2q1q0 p0p1p2p3 MQ′P M P [ p 0 − p 1 − p 2 − p 3 p 1 p 0 − p 3 p 2 p 2 p 3 p 0 − p 1 p 3 − p 2 p 1 p 0 ] [ p 0 − p v T p v p 0 I ( p v × ) ] \boldsymbol{M}_{P}\left[\begin{array}{cccc}p_{0} -p_{1} -p_{2} -p_{3} \\ p_{1} p_{0} -p_{3} p_{2} \\ p_{2} p_{3} p_{0} -p_{1} \\ p_{3} -p_{2} p_{1} p_{0}\end{array}\right]\left[\begin{array}{cc}p_{0} -\boldsymbol{p}_{v}^{\mathrm{T}} \\ \boldsymbol{p}_{v} p_{0} \boldsymbol{I}\left(\boldsymbol{p}_{v} \times\right)\end{array}\right] MP p0p1p2p3−p1p0p3−p2−p2−p3p0p1−p3p2−p1p0 [p0pv−pvTp0I(pv×)]
static Eigen::Matrix4d quaternionleft(const Quaterniond q) {Eigen::Matrix4d ans;ans(0, 0) q.w();ans.block1, 3(0, 1) -q.vec().transpose();ans.block3, 1(1, 0) q.vec();ans.block3, 3(1, 1) q.w() * Eigen::Matrix3d::Identity() skewSymmetric(q.vec());return ans;
}M Q ′ [ q 0 − q 1 − q 2 − q 3 q 1 q 0 q 3 − q 2 q 2 − q 3 q 0 q 1 q 3 q 2 − q 1 q 0 ] [ q 0 − q v T q v q 0 I − ( q v × ) ] \boldsymbol{M}_{Q}^{\prime}\left[\begin{array}{cccc}q_{0} -q_{1} -q_{2} -q_{3} \\ q_{1} q_{0} q_{3} -q_{2} \\ q_{2} -q_{3} q_{0} q_{1} \\ q_{3} q_{2} -q_{1} q_{0}\end{array}\right]\left[\begin{array}{cc}q_{0} -\boldsymbol{q}_{v}^{\mathrm{T}} \\ \boldsymbol{q}_{v} q_{0} \boldsymbol{I}-\left(\boldsymbol{q}_{v} \times\right)\end{array}\right] MQ′ q0q1q2q3−q1q0−q3q2−q2q3q0−q1−q3−q2q1q0 [q0qv−qvTq0I−(qv×)]
static Eigen::Matrix4d quaternionright(const Quaterniond p) {Eigen::Matrix4d ans;ans(0, 0) p.w();ans.block1, 3(0, 1) -p.vec().transpose();ans.block3, 1(1, 0) p.vec();ans.block3, 3(1, 1) p.w() * Eigen::Matrix3d::Identity() - skewSymmetric(p.vec());return ans;
}egin{array}{cc}q_{0} -\boldsymbol{q}{v}^{\mathrm{T}} \ \boldsymbol{q}{v} q_{0} \boldsymbol{I}-\left(\boldsymbol{q}_{v} \times\right)\end{array}\right] $$
static Eigen::Matrix4d quaternionright(const Quaterniond p) {Eigen::Matrix4d ans;ans(0, 0) p.w();ans.block1, 3(0, 1) -p.vec().transpose();ans.block3, 1(1, 0) p.vec();ans.block3, 3(1, 1) p.w() * Eigen::Matrix3d::Identity() - skewSymmetric(p.vec());return ans;
}
