车路云-设备数据坐标转换

一、摄像机目标像素坐标转地理位置坐标

1.1 相机标定&路口标定

• 使用棋盘格标定板采集多张图片，用于求解相机内参矩阵和畸变系数。
• 使用RTK设备打点路口不同位置的经纬度，用于求解坐标转换的单应性矩阵。

1.2 像素坐标转地理位置坐标

需要使用到的的配置信息：相机内参矩阵、相机畸变系数、路口单应性矩阵

• 1. 转换为齐次坐标：
  • 人为构造一个值恒为1的坐标纬度，方便后续参与单应性矩阵计算。
  • 将目标检测得出的检测框角点坐标进行齐次化，即将左上角、左下角、右上角、右下角等坐标值转换为xys形式构成一个3*N（N为框角点个数）的目标角点齐次坐标。

  detect_obj_box[0]=x1;// leftdetect_obj_box[1]=y1;// topdetect_obj_box[2]=x2;// rightdetect_obj_box[3]=y2;// bottom// 左上角的点 (取y坐标的20%位置，避免取到车顶)obj_pixel_xys_mat.at<double>(0,4*i+0)=detect_obj_box[0];// xobj_pixel_xys_mat.at<double>(1,4*i+0)=detect_obj_box[1]*0.2+detect_obj_box[3]*0.8;// yobj_pixel_xys_mat.at<double>(2,4*i+0)=1;// 齐次坐标: 人为构造一个值恒为1的坐标，方便后续参与单应性矩阵计算// 左下角的点obj_pixel_xys_mat.at<double>(0,4*i+1)=detect_obj_boxes[i][0];obj_pixel_xys_mat.at<double>(1,4*i+1)=detect_obj_boxes[i][3];obj_pixel_xys_mat.at<double>(2,4*i+1)=1;

• 2. 利用相机的内参进行畸变校正（可选）

  • 相机内参矩阵和畸变系数都是通过棋盘格标定板工具采集数据计算出来的。

  • 相机的内参通常是纬度为3*3的矩阵。

  • 如果不进行内参畸变矫正，则位于图片边缘位置的目标经纬度计算会有较大误差 。

  • internal_mtx_vector：相机内参矩阵（3*3）
    

  • internal_dist_vector：相机畸变系数

    • 大多数工业相机、普通镜头等畸变不太严重的情况，一般采取5个 (k1,k2,k3,p1,p2)畸变系数；使用更多系数可能导致过拟合。
    • 鱼眼镜头、广角镜头等畸变严重的情况，会采取8个 (k1-k6,p1,p2)畸变系数。
      
• 3. 执行单应性矩阵变换
  • 单应性矩阵是通过RTK设备采集真实参考点的物理经纬度计算出来的。
  • 单应性变换是透视变换，包含旋转、平移、缩放、透视畸变。
  • 单应性矩阵的纬度也是3*3；是通过相机标定计算得出来的矩阵。
  • 将经过内参矫正后的目标框角点齐次坐标矩阵（3N）与单应性矩阵(33)进行矩阵乘法计算，得到透视信息矩阵（3*N）。
  • 将带有透视信息的齐次坐标矩阵转换为笛卡尔坐标矩阵，即对齐次坐标（x,y,w）进行透视归一化。

  // 单应性矩阵乘以齐次坐标矩阵得到结果矩阵Mat rst_mat=my_homo_mat*obj_pixel_xys_mat;// 齐次坐标转为笛卡尔坐标：根据结果矩阵中透视信息进行归一化rst_mat.row(0)=rst_mat.row(0)/rst_mat.row(2);rst_mat.row(1)=rst_mat.row(1)/rst_mat.row(2);

• 4. 计算目标的经纬度
  • 将透视变换后的笛卡尔坐标转为世界坐标中的经纬度。
  • base_num： 标定时对经纬度坐标的放大倍数，通常为1000*1000。经纬度坐标通常是小数值（如 120.123456, 30.654321 ），直接用于单应性矩阵标定会导致：浮点数运算精度不够；矩阵条件数差，影响求解稳定性。因此在标定时通常会放大标定的经纬度后，再用于求解单应性矩阵。再计算实际经纬度时需要缩小对应倍数。
  • base_lng/base_lat：单应性矩阵标定时所选定的坐标系原点的经纬度，即标定坐标系的零点；
    • 标定点坐标 = (标定位置实际经度 - base_lng) × base_num
    • 如果标定时以相机位置为原点，则为设备安装位置
    • 选取路口中心、道路起点等区域的某个参考点作为base_lng或base_lat
    • 选取标定板的某个角点作为base_lng或base_lat
  • 实际经度 = base_lng + (变换结果 / base_num)

  // 获取变换后的目标4个角点经纬度，此时的经纬度是放大后的经纬度doublelng1_last=rst_mat.at<double>(0,4*i+0);doublelat1_last=rst_mat.at<double>(1,4*i+0);doublelng2_last=rst_mat.at<double>(0,4*i+1);doublelat2_last=rst_mat.at<double>(1,4*i+1);doublelng3_last=rst_mat.at<double>(0,4*i+2);doublelat3_last=rst_mat.at<double>(1,4*i+2);doublelng4_last=rst_mat.at<double>(0,4*i+3);doublelat4_last=rst_mat.at<double>(1,4*i+3);//计算目标中心点的实际经纬度：基准经纬度+还原标定放大倍数后的经纬度doublecenter_lng=base_lng+(lng1_last+lng2_last+lng3_last+lng4_last)/4/base_num;doublecenter_lat=base_lat+(lat1_last+lat2_last+lat3_last+lat4_last)/4/base_num;//计算目标框4个角点的实际经纬度doublelng1=base_lng+(lng1_last/base_num);doublelat1=base_lat+(lat1_last/base_num);

二、毫米波雷达目标位置转地理位置坐标

• device_north_deviation_angle：设备正北偏转角。
• device_lng / device_lat：设备经度、纬度。

三、WGS84&UTM&GCJ02坐标系