Silencejql
0%

HUD机器人标定及验证

发表于 更新于 Valine：
本文字数： 1.7k 阅读时长 ≈ 2 分钟

HUD相机标定完成后辅助调节机器人角度时角度的计算方法

坐标系说明

车辆坐标系

X-Z
X-Y
Y-Z

标定说明

  1. 根据标靶四孔的三坐标计算出标靶中心坐标OO
  2. 相机走到与OO点相同的YZYZ坐标处, 保证相机中心与OO点在同一轴线上
  3. 将标靶左上角圆孔中心坐标填入标定点.Point3DXYZ中
  4. 测量相机感光芯片到标靶的距离填入特征点.DotDistance中
  5. 调整相机角度使得图像中心十字位于标靶中心OO处, 且目测图像角度偏差不大
  6. 拍照识别四个圆孔
  7. 标定内外参
  8. 将ROI框住某个圆孔点击标定验证, 查看输出角度进行微调后重复567步, 直到输出满足要求
  9. 若角度偏差大但图像很正, 可能是坐标参数的输入有问题
  10. 若某个点的偏移量大, 可适当调整曝光使识别更准确后再验证

图像说明

标定结果图1
标定结果图2
标定验证
标定验证输出PCenter
PC控制运动到标定点验证

左上角为P1,右上角为P3
左下角为P7,右下角为P9

代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
PointF3D Point3d = CameraAgorithm.Pixel2World(p, Camera, Camera.Extrinsics_Tz_mm - Camera.Point3DZ);

PointF3D P1 = CameraAgorithm.Pixel2World(_calibrationAgorithm.DotLeftTop, Camera, Camera.Extrinsics_Tz_mm - Camera.Point3DZ);
PointF3D P3 = CameraAgorithm.Pixel2World(_calibrationAgorithm.DotRightTop, Camera, Camera.Extrinsics_Tz_mm - Camera.Point3DZ);
PointF3D P7 = CameraAgorithm.Pixel2World(_calibrationAgorithm.DotLeftBottom, Camera, Camera.Extrinsics_Tz_mm - Camera.Point3DZ);
PointF3D P9 = CameraAgorithm.Pixel2World(_calibrationAgorithm.DotRightBottom, Camera, Camera.Extrinsics_Tz_mm - Camera.Point3DZ);

//PCenter为图像中心点坐标在世界坐标系中的点
//PCenter.P_X=>车辆坐标系的Y
//PCenter.P_Y=>车辆坐标系的Z
//PCenter.P_Z=>车辆坐标系的X
//Camera.Point3DX=>车辆坐标系的Y
//Camera.Point3DY=>车辆坐标系的Z
//Camera.Point3DZ=>车辆坐标系的X
PointF3D PCenter = CameraAgorithm.Pixel2World(new PointF(Camera.PixelWidth/2,Camera.PixelHeight/2), Camera, Camera.Extrinsics_Tz_mm - Camera.Point3DZ);

//上面两点的倾斜度
double RX = Math.Atan((P3.P_Y - P1.P_Y) / (P3.P_X - P1.P_X)) * 180 / Math.PI;
//图像中心相对相机中心(机器人坐标)水平方向偏移
double RY = Math.Atan(((PCenter.P_X - m_Motion.Data_YPos.xValue.zFloat())) / (Camera.Extrinsics_Tz_mm - Camera.Point3DZ)) * 180 / Math.PI;
//图像中心相对相机中心(机器人坐标)竖直方向偏移
double RZ = Math.Atan(((PCenter.P_Y - m_Motion.Data_ZPos.xValue.zFloat())) / (Camera.Extrinsics_Tz_mm - Camera.Point3DZ)) * 180 / Math.PI;