Vicon动作捕捉系统使用心得

坐标系建立

vicon坐标系建立规则：

• 第一个作为原点，第二个点连线指向第一个点的的方向是z轴正方向。第一个点指向第三个点的方向为y轴正方向，最后垂直于y-z平面，根据右手定则确定x方向。其余点都是防止捕获丢失或者多个segment混叠而加的，可以随意顺序选取。

选取经验

• 一般我们第一个点选择在飞机尾部，由飞机头部正对的点作为第二个点，所以z轴由前向后指向飞机正后方。然后第三个选择在原点的的正左侧，那么y轴就指向飞机正左，最后x轴也被确定，即指向正上方。
• 这时候vicon给出坐标是q(x,y,z,w)=(0, -0.707, 0, 0.707)也就是绕着y轴旋转了-90度。为了得到在vicon下姿态，这时只需要每次vicon给出的姿态q后，先乘q(0, 0.707, 0, 0.707)就可以置为当前姿态，即ENU坐标系状态。比如当前原始状态下肯定希望是(0, 0, 0, 1)，当然如果因为球没贴正，结果没有正好是(0, -0.707, 0, 0.707)，比如是(0, -0.700, 0, 0.714)，那么就乘(0, 0.700, 0, 0.714)就可以了。

landmark标定

• 飞机上mark点的位置标定可以采用vicon系统，选择自己的subject，点击subject capture采集几秒钟。点击左上角蓝色标1的图标reconstructe and label
  恢复出之前的记录运动过程，选择右上角小齿轮，选择pipeline为Save，下面会显示Export ASCII打钩，这样就可以输出每个marker3D位置到文件
  比如kun0 Cal 05.csv
• 在csv中选择mark和起始的mark点做差，就得到mark点之间世界坐标系下相对位置，当然如果在vicon场地中摆mark不正，则本来一个平面的mark点也会出现几毫米的位置差异。

vicon坐标系转换心得

/**
 * 这里有三个坐标系，Coordinate_drone 飞机坐标系，Coordinate_markers 反光球坐标系， Coordinate_enu 世界坐标系
 * drone 相对于 marker 有一个坐标关系，marker 相对于 enu 有个转换关系。总的来说有： T_enu_drone = T_enu_markers * T_markers_drone
 * T_enu_markers是vicon系统读出来的，T_markers_drone 是贴标记点时候和飞机坐标系确定的。想求飞机坐标系在enu坐标系下的表示，就假设飞机坐标系下有一个P_drone.
 * 于是有 P_enu = T_enu_markers * T_markers_drone * P_drone
 * 由于maker是瞎贴的， T_markers_drone 很难确定，于是将 Coordinate_drone 与 Coordinate_enu 对齐. 那么T_enu_drone是单位阵。T_enu_markers 和 T_markers_drone 就是互为逆。
 * 那么把vicon读出来的初始摆放位姿 T_enu_markers 的四元数求逆 就得到 T_markers_drone，也就是 T_markers_drone = T_enu_markers_init.inverse()， 此处 T_enu_markers_init 是飞机对齐后，读到的marker初始位姿。
 * 那么 假设 T_enu_markers_real_time 是后来vicon读取到marker实时位姿。 T_enu_drone = T_enu_markers_real_time * T_enu_markers_init.inverse() 就可以了
 */