DR转自车

/*Eigen::Vector3d 是 Eigen 库C 线性代数库中定义的3 维双精度浮点型向量你可以把它理解成 一个 “打包好的容器”专门装 3 个 double 类型的数值x/y/z 这个容器自带 “矩阵 / 向量运算功能”比如和旋转矩阵相乘、求逆变换*/voidFusionObjectMonitorWidget::drToCar(constDrInfodrInfo,ObstacleInfo_3Dobs_3d){// 3D坐标从Dr 转到自车坐标系下// drInfo.quat存储自车在 DR 全局坐标系下的3D 旋转姿态四元数包括偏航角 yaw、俯仰角 pitch、横滚角 roll// 简单说就是 “自车朝哪个方向、是否抬头 / 低头、是否侧翻”/*drInfo.position自车在 DR 全局的位置是 (1000, 2000, 0),用于给一个点加偏移量 drInfo.quat自车朝向正北偏航角 90° pose 就代表把自车坐标系下的点比如自车前方 5 米的点 (5,0,0) 先旋转 90°再平移到 (1000,2000,0)最终得到该点在 DR 全局的坐标 (1000, 2005, 0)。*/// * 运算符Eigen 中表示 “变换的组合”顺序是「先旋转后平移」右乘优先Eigen::Affine3d poseEigen::Translation3d(drInfo.position)*drInfo.quat;//求 pose 这个仿射变换的 “逆变换”把「自车坐标系→DR 全局坐标系」的变换反转成「DR 全局坐标系→自车坐标系」的变换最终赋值给 invPoseEigen::Affine3d invPosepose.inverse();Eigen::Vector3dpoint_global(obs_3d.x,obs_3d.y,obs_3d.z);// 用「全局→自车」的逆仿射变换 invPose作用于全局坐标向量 point_global最终得到该点在自车坐标系下的坐标向量 point_vehicleEigen::Vector3d point_vehicleinvPose*point_global;// 将转换后自车坐标系下的坐标值重新赋值给 obs_3d 的 x/y/z 字段完成最终的坐标更新obs_3d.xpoint_vehicle.x();obs_3d.ypoint_vehicle.y();obs_3d.zpoint_vehicle.z();// 把描述自车 3D 姿态的四元数drInfo.quat转换成等价的 3×3 正交旋转矩阵Eigen::Matrix3d rotationdrInfo.quat.toRotationMatrix();Eigen::Matrix3d invRotationrotation.transpose();// 旋转矩阵的逆等于其转置// 速度从全局坐标系转换到自车坐标系下Eigen::Vector3dvel_global(obs_3d.speedOrigin.x,obs_3d.speedOrigin.y,obs_3d.speedOrigin.z);// invRotation 是自车旋转矩阵的转置正交矩阵逆 转置作用是 “把全局坐标系的速度方向旋转为自车坐标系的方向”// 速度是矢量只有方向 大小无 “位置” 属性因此只需旋转方向无需平移这是和坐标转换的核心区别Eigen::Vector3d vel_vehicleinvRotation*vel_global;obs_3d.speedOrigin.xvel_vehicle.x();obs_3d.speedOrigin.yvel_vehicle.y();obs_3d.speedOrigin.zvel_vehicle.z();//加速度从Dr 转到自车坐标系下Eigen::Vector3dacc_global(obs_3d.accelerat.x,obs_3d.accelerat.y,obs_3d.accelerat.z);Eigen::Vector3d acc_vehicleinvRotation*acc_global;obs_3d.accelerat.xacc_vehicle.x();obs_3d.accelerat.yacc_vehicle.y();obs_3d.accelerat.zacc_vehicle.z();// 障碍物全局朝向 - 自车全局朝向」得到障碍物相对于自车的朝向//本质是用障碍物的全局绝对朝向减去自车的全局绝对朝向得到 “障碍物相对于自车的朝向”。obs_3d.yaw-drInfo.h;}