rmoss_contrib

rmoss_contrib copied to clipboard

## Feature request #### Feature description 在自瞄中，需要在相机与云台之间做坐标变换，采用tf2可以方便的管理维护坐标关系，其还提供了简洁的坐标变换接口，以及tf消息同步功能（将IMU转换为tf，间接的实现IMU-相机同步），可以极大的方便开发。 #### Implementation considerations （1）建立坐标系之间关系： * base_link->gimbal_yaw->gimbal_pitch->camera->camera_optical->target(识别到的目标装甲板)：其中gimbal_yaw和gimbal_pitch根据云台角度实时更新维护，camera_optical->target之间的变换由自瞄算法计算出来。 * 在使用枪管瞄准的时候，需要使用一个额外的参考坐标系gimbal_home，用于计算瞄准所需要的云台转动yaw,pitch角度，其为当云台角度为0时的gimbal_pitch参考系，可使用静态tf维护base_link->gimbal_home。 > Tip： > * gimbal_pitch->camera->camera_optical 可简化为gimbal_pitch->camera_optical > * gimbal_home参考系可以灵活选取，如果base_link->gimbal_home绑定静态变换，那计算出来的yaw为绝对角，如果gimbal_yaw->gimbal_home绑定静态变换，则计算出的yaw为相对角度。 （2）瞄准解算： * 使用tf2工具，将target转换到gimbal_home坐标系下，计算云台转到绝对角。

gezp

既然PnP求解姿态时有三个自由度，那么我们可以降低自由度来构造关于yaw的单峰函数。通过求极值点来求出正确的yaw。官方的机器人制造规范手册中说明了，装甲板安装时，roll应该为0°，pitch应该为15°。因此装甲板在陀螺仪坐标系中的姿态关于roll和pitch这两个量的大小就已经固定了，需要解算的就是yaw的值。 构造单峰函数的第一件事是确定函数的应变量和自变量。显然自变量是yaw，而应变量应该是反投影过后的装甲板图像点与识别到的装甲板图像点之间的差。由于装甲板识别出来在本质上是一个平行四边形，因此主要考虑平行四边形怎么作差。主要有以下三种思路： （1）对应的点的二维坐标直接使用欧氏距离作为平行四边形的差； （2）对应的两个平行四边形的面积作差； （3）对应的两个平行四边形的夹角加上法向量信息作差； 对于方法（1）（2），笔者都尝试过，结果就是在通常的情况下，构造出来的函数有多个极值点。对于方法三，采用的方法为计算平行四边形中一组夹角的差加上法向量的信息，即夹角加上正负号，即可构造出一个比较合理的单峰函数，但是比较遗憾的是笔者暂时未在matlab上进行验证，并且在实际情况中发现还是会有偶然的情况使得函数走样，不过在一般的情况下能够满足大部分的需求并且不会出错。  由图可知，反投影的装甲板的角点和识别到的装甲板的角点是保持了角点之间的顺序和相对位置关系的（图中平行四边形角点的编号0、1、2、3）。因此我们可以选择∠103进行作差，可知实际姿态和绕yaw轴相对0点对称位置的反投影可以被过滤掉，但是此时中心对称的反投影结果和实际姿态一样。这时候我们可以通过求法向量夹角的方式确定正负号，从而将中心对称的反投影过滤掉。 此方法在2024年的南部赛区区域赛Helios战队的自瞄算法中已经实践测试。

JellyfishKnight