# pnp_target_node **Repository Path**: myboyhood/pnp_target_node ## Basic Information - **Project Name**: pnp_target_node - **Description**: No description available - **Primary Language**: Unknown - **License**: MIT - **Default Branch**: tensorrt - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 1 - **Forks**: 0 - **Created**: 2020-12-25 - **Last Updated**: 2025-04-28 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README News --- - **2024-02-16** 对于元知场地的2024.05时候双机circle飞行数据，用mocap得到了真值，但是需要在评估时使用`tools/evaluate_with_gt.cpp`将PnP（或dwe）估计的相对位姿转移对齐到Mocap坐标系下。 需要调用`rosrun pnp_target_node evaluate_with_gt`命令。 - **2024-05-05** kun0 观测 kun1, kun1 观测 kun0, PnP求解结果不相同，尤其在x方向，发现kun0侧面D435相机和imu之间yaw角不是-90度。 而可能是-91度，同样, kun1侧面D435相机和imu之间yaw角不是90度，而可能是91度。这个偏差导致了PnP求解结果不同。**但这个很难标定出来**。 - **2024-05-05** goodfeaturestotrack的角点提取后加入亚像素提取，得到更加精确的角点位置。估计的PnP结果也更准确，帧间也更平滑。见下面章节`亚像素提取` - **2024-05-04** 使用两个减震后的IMU互补滤波效果，非常好，目前兼顾延迟和平滑性，选择alpha=0.5. --- ## run ros nodelet version ```shell roslaunch pnp_target_node pnp_target_node_nodelet.launch ``` ## run ros node version ```shell roslaunch pnp_target_node pnp_target_node_trt_ros.launch ``` ## issue ### issue 1 nodelet version can not add other `class` or `add_library()`, such as `class FILTER` ### issue 2 if the 3D structure of the landmarks of drone is plane, the PnP result is good. if not in a plane, the result will jump between two values. ### issue 3 相机PnP结果与Vicon有延时，需要将估计出的PnP结果的时间戳减去一个固定时间差（200ms左右），得到真实时刻位姿。 ### issue 4 采集两架飞机共同飞行的数据，发现运动的观测相机，由于姿态补偿不准确，那么转换到世界坐标系会对PnP结果产生很大影响。 - 一个是时间延迟造成的，需要改进。 - 另外可以将飞机VIO作为预测，PnP作为观测，构建kalman或者滑窗优化问题，在飞机抖动时将PnP的方差变大。 - 或者加入A对B的观测，由于相机静止时，对目标的观测还是很准确的。这样A对B有观测，B对A也有观测。 ### issue 5 相机PnP解算的xyz结果与Vicon得到两架飞机之间相对的xyz有偏差，可能是相机内参不准确,发现USB相机的内参太不准确，PnP的结果和实际Vicon差异大，D455相机的内参比较靠谱。目前计划采用D435相机 ### issue 6 若红外相机曝光减小，整幅图像过于灰暗，光流追踪经常失败，所以需要增加曝光时间，提高光流追踪成功率。 ### issue 7 kun0观测kun1，kun1观测kun0，PnP求解结果不同，尤其在x方向，发现kun0侧面D435相机和imu之间yaw角不是-90度。而可能是-91度，同样, kun1侧面D435相机和imu之间yaw角不是90度，而可能是91度。这个偏差导致了PnP求解结果不同。**但这个很难标定出来**。 目前只能相信kun0观测kun1这一种结果. ### issue 8 [20250216 imu漂移问题] 一般需要将视觉观测pnp值转到kun0的ENU坐标系下,但是imu的yaw角度会漂移,导致kun1在kun0的ENU坐标系下会不准确,尤其是表现在x方向上会因为转了次imu导致不准确.下图展示在元知20240504转圈实验中mocap得到的yaw和 xsens-imu获得的yaw,发现imu的yaw会朝着负角度缓慢漂移.因为我发现T265的yaw在动态飞行中不是很稳定.静止状态下确实不漂移.但是mavros/imu/data的yaw数据非常好,mavros/local_position/pose的yaw也非常好. ![Figure1.xsens-imu-drifting](./README_File/xsen-imu-drifting-kun0andkun1-2025-02-16_16-36-25.png) ![Figure2.mavros-imu-no-drifting](./README_File/mavros-imu-stable-no-drifting-2025-02-16_17-14-13.png) ## Road map - **[20231015紧急]** 两家飞机都运动时，位姿估计不准确。采用3种实验范式，kun0作为观测相机，kun1作为被观测目标。1.kun0固定，kun1飞行。 2.kun0飞行，kun1固定。3.kun0和kun1都飞行。 - 学习使用nodelet写代码，但貌似只能跑单线程，nodelet中不允许`imshow`，可能`imshow`需要调用了其他线程。 - 需要标定landmark和D455相机,T265相机的姿态关系，以便于PnP结果与T265坐标系统一。 - 计算kun0的T265 VIO坐标系与kun1相机坐标系的旋转转换关系，若都统一到世界坐标系，由PnP结果计算kun1相对kun0的位置，需要左乘kun1的当前姿态。 ## note ### landmark标定 - 飞机上mark点的位置标定可以采用vicon系统，选择自己的`subject`，点击`subject capture`采集几秒钟。点击左上角蓝色标1的图标`reconstructe and label` 恢复出之前的记录运动过程，选择右上角小齿轮，选择`pipeline`为`Save`，下面会显示`Export ASCII`打钩，这样就可以输出每个`marker`3D位置到文件 比如`kun0 Cal 05.csv` - 在csv中选择mark和起始的mark点做差，就得到mark点之间世界坐标系下相对位置，当然如果在vicon场地中摆mark不正，则本来一个平面的mark点也会出现几毫米的位置差异。 ### PnP结果的坐标系 - 位置xyz是相机图像坐标系下的，x右，y下，z前。和markers的姿态无关。 - 姿态得到的是旋转向量，需要用Eigen转为旋转矩阵或者四元数。姿态和mark在3D坐标系有关，和相机的姿态也有关。因为输出结果就是在相机坐标系下的位姿。 如果mark建系和相机坐标系一致，则得到的姿态为0附近。 - 姿态在0附近有跳变，若在yaw方向稍微倾斜一定角度，则姿态求解结果比较准确。需要设计mark结构时考虑设计成斜的。 - PnP解算出目标点的原点是设定的图像中(0, 0)的那个landmark点。如果发现当该点位于图像中心，但PnP解算在xy平面上不为0,则说明相机内参fx，fy，cx，cy有问题。 ### 实验注意事项 - 多架飞机初始位置摆放，由于PnP结果的位置坐标系完全取决于拍摄的相机位姿，所以相机初始摆放的yaw角，也就是x轴方向，尽量和Vicon坐标系x轴重合。 不然T265得到xy和vicon的xy对不齐，导致结果评估误差大。 - 多架飞机上互观测的红外相机也需要和机体坐标系之间标定姿态。不然小角度的偏差，会导致观测和T265的融合坐标系不重合。 ### 修复NVIDIA Jeston步骤 - 旧的NX20.04 只安装了ros-noetic-desktop,删除了libopencv4.5.4的包，仅安装libopencv4.2，避免冲突 - 证明仅从ros源安装的opencv4.2是可以使用cv::fsettings 读取，也可以编译yolo-tensorrt，放在pnp_target_node中也可以推理yolov4-tiny，也可以推理superpoint，superglue - 结果安装apt-get install 无法解决依赖问题，安装了sudo apt-get install aptitude，然后使用sudo aptitude install - cv_bridge 可以通过`sudo apt-get install ros-noetic-vision-opencv`解决 - 在jetpack5.1的ubuntu20.04环境中，librelsense-dev不能通过`sudo apt-get install`安装。只能源码编译librealsense - 源码编译过程中，在cmake阶段，将-DFORCE_RSUSB_BACKEND=false，这是禁掉第二种USB传输方式，而是用linux自带的USB传输方式。不然会影响多相机使用。 - - 另一台新的NX20.04，只安装了ros-noetic-desktop，但还保留了opencv4.5.4，准备安装docker试试效果, - jetpack5.1自带了docker，不需要安装，但需要自己将用户加入其中 ### 修复zph-desktop - 为了避免opencv3.2继续影响代码执行，我卸载了`ros-melodic-desktop-full` 以及`libopencv3.2` - 重新安装`ros-melodic-ros-base`，随后cv_bridge,rqt,rviz再想办法安装，由于melodic的cv_bridge并依赖于opencv3，我也就改回了`opencv3.4.10`系列 - 在编译`relative_pose_backend`时候，额外安装一下`sudo apt-get install ros-melodic-gazebo-ros` - 在编译opencv3.4.10时，需要卸载高版本的 openBLAS，安装低版本的`v0.2.20`的openBLAS。默认安装在`/opt` ### 实验精度 - 对比PnP求解相对姿态，两家飞机imu的差，以及使用mocap真值。发现PnP的求解在小范围跳变，imu的差相对稳定，但在yaw角上与mocap上有较大差异,大约有11度。 ```shell [PnP] q_drone2_to_drone1(wxyz) = 0.996, 0.026, 0.015, -0.080 [IMU_DIFF] q_drone2_to_drone1_imu (wxyz) = 0.995, -0.002, 0.019, -0.097 [IMU_DIFF] euler_imu (xyz) = -0.008, 0.037, -0.194 [GT] q_drone2_to_drone1_gt (wxyz) = 0.999, -0.017, 0.030, -0.018 [GT] euler_gt (wxyz) = -0.034, 0.059, -0.036 ``` IMU的差在室内外差别较大，不是很可信。如果使用它内置的orientation，只有把四元数的z归0,然后再沿x轴和y轴转动才能对应的上标签所指示的ENU坐标系。但是yaw角一直在飘，是短时间内持续地飘。所以纯用IMU不可信 ![Figure3.xsens-imu](./README_File/xsens-imu-yaw-drifting-2023-10-22_21-59-52.png) 使用t265的`/kun0/kun0_camera/odom/sample`和`/kun0/mavros/local_position/pose`，以及`/kun0/mavros/imu/data`这三种数据是可以记录绝对姿态的。而且不会随着yaw磁偏角偏移而偏离。 所以可以使用这个的差作为两个飞机初始roll和pitch的相对角度。 尽管camera对target的yaw角的一次观测是跳变不准的，但只要获取target的VIO或者imu信息，将target在一段时间内旋转了多少角度作为预测，再将camera的观测作为更新。 通过构建最小二乘是可以，恢复出姿态起始点和后面的姿态的。或者构建成最简单的kalman方程也可以。 但是下图`Figure1.yaw-compare`反映的PnP的观测噪声在yaw=0附近已经和需要估计yaw量程差不多了，观测几乎用处不大,但是在角度偏离0时候噪声减小了。可以设计斜的landmark。 另外，用marker_pixel的方法获得了极好的yaw角估计性能。红色和绿色细实线是对方相机在自己相机中angle。两者做差即可得到 冰蓝色虚线，即为marker_pixel的估计结果，粉紫色虚线为真值。 蓝色细线为PnP估计的结果。 ![Figure4.yaw-compare](./README_File/yaw-compare-pixelerror-PnP-with-Mocap2023-10-22_17-51-43.png) - 对于marker_pixel有两种方法，第一种是全程使用pixel的视角差，第二种是先用pixel的视角差来做初始化相对yaw_init角，然后后面用两个IMU的值作为yaw的差。 实验结果来看后者更为稳定。另外，我们对比了master和slave的pixel_yaw角差，slave的邻居pixel时间戳和本身pixel时间戳对齐更差一些(20ms~90ms)，master时间戳更好，可以在1帧曝光以内（30ms）。 发现两者的估计的yaw角差是有差别的，master的(绿色)更贴近imu预测的(蓝色)yaw角差，slave的(橙色)更偏离。 ![Figure5.yaw-compare-master-slave-with-different-time-diff](./README_File/yaw_compare_with_master_slave_and_pixel_imu.png) --- ### 参考文献 - 在Apriltag2文章位于`./doc/`，其大篇幅在讲述如何提取标记，一幅640*480图像处理时间也过长，需要78ms。而对如何精确求解位姿并未提及 - 在《2021-ICRA-Tracking and Relative Localization of Drone Swarms With a Vision-Based Headset》文献中作者采用了《2017-ICRA-6-DoF Object Pose from Semantic Keypoints》 从关键semantic的点训练网络获取6Dof位姿(https://github.com/geopavlakos/object3d/)。但2017年这篇文章是matlab代码， 而且是从一副RGB图像，得到关键点，然后预测出目标姿态。需要参照港科大Omni-Swarm的C++代码（只有训练好的模型）。 - 对于目标追踪，Omni-Swarm采用了MOSSETracker(https://github.com/amoudgl/mosse-tracker)，使用滤波器对图像中图标追踪。速度很快，但也是matlab代码，需要参考Omni-Swarm的代码。 ## 代码文件说明 - 最新维护的代码 - `src/pnp_target_node_trt_ros.cpp`是ROS的node版本,只包含了`PnPTargetNodeRos`类的定义。后期维护的代码都在这里。 - `src/pnp_target_node_trt_ros_test.cpp`是`PnPTargetNodeRos`类的接口进入的函数。包含了`main()`函数，用于测试`PnPTargetNodeRos`类的功能 - 旧代码 - `src/pnp_target_node_ros.cpp`是之前靶标追踪初级程序。 - `src/pnp_target_node_time.cpp`是之前靶标追踪初级程序+用image消息去寻找对其的imu姿态消息。 - `src/pnp_target_node_time_loop.cpp`是之前靶标追踪初级程序+用image消息去寻找对其的imu姿态消息+循环尝试各种二值化阈值和膨胀腐蚀参数大小。 - `src/pnp_target_node_trt.cpp`是ROS的nodelet版本,但是只能跑单线程，后期没有维护。 - `src/single_test.cpp`是测试单个本地图像的程序，用于寻找轮廓并imwrite() - `src/test_nodelet.cpp`是测试nodelet的最简单程序。 - `src/test_rs.cpp`是测试利用realsense相机sdk读取图像的程序。 - `src/test_yolo_node.cpp`调用`PnPTargetNodeRos`类，但只进行yolo检测测试。 - `src/uvc_camera.cpp`尝试使用uvc的普通USB相机读取图像，并手动设定曝光度，当时想用这个相机作为侧面的相机。 ## 互补滤波 - 检测加速度的bias - 用飞机悬停在空中的一段bag数据，计算出飞机的bias，然后在飞行时候，将bias减去，得到的加速度就是真实的加速度。 - kun0 xsens_imu bias = [-0.124477, 0.059362, 9.790904]，是经过8s左右的悬停后计算出来的。 - kun1 xsens_imu bias = [-0.157809, 0.001318, 9.791052]，是经过2s左右的悬停后计算出来的。 - 互补滤波的参数 alpha = 0.1, poswin = 0，暂时不需要对位置和加速度进行均值滤波。 - ![Figure6.complementary_filter](./README_File/poswin=0_alpha=0.1.png) ## 亚像素提取 - 亚像素提取的方法是使用`cv::cornerSubPix()`函数，对角点进行亚像素提取，得到更加精确的角点位置。估计的PnP结果也更准确，帧间也更平滑,xyz总体标准差从0.015降为0.013 - kun0观测kun1的结果 ![Figure7.subpixel](./README_File/traj_Comp_X-kun0-ob-kun1-only-harris.png) ![Figure7.subpixel](./README_File/traj_Comp_X-kun0-ob-kun1-subcorner.png) - kun1观测kun0的结果 ![Figure7.subpixel](./README_File/traj_Comp_X-kun1-ob-kun0-only-harris.png) ![Figure7.subpixel](./README_File/traj_Comp_X-kun1-ob-kun0-subcorner.png) ## 对于bidirectional visual differential的计算，考虑roll,pitch矫正 模拟程序见`src/validate/bidirectional_visual_observation.cpp` 第一种方法：由于我们知道侧视相机的四元数，于是可以知道当前相机是如何从水平状态按照zyx轴旋转到当前姿态的。那么反过来，在当前姿态下相机的2D观测，就应该对2D像素通过依次反向旋转xyz，可以恢复得到水平姿态下的像素位置。（这个方法不可行，因为必须知道marker的3D位置才可以旋转，仅知道2D观测没法旋转） 第二种方法：我通过PnP已经求得对方相机IR5 marker的3D位置，然后我根据当前相机姿态计算IR5在相机水平姿态下的3D位置，以及其2D投影，然后就可以直接拿这个投影计算了。