Silencejql
0%

HUD机器人标定及验证

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

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

坐标系说明

车辆坐标系

X-Z
X-Y
Y-Z

标定说明

  1. 根据标靶四孔的三坐标计算出标靶中心坐标$O$
  2. 相机走到与$O$点相同的$YZ$坐标处, 保证相机中心与$O$点在同一轴线上
  3. 将标靶左上角圆孔中心坐标填入标定点.Point3DXYZ中
  4. 测量相机感光芯片到标靶的距离填入特征点.DotDistance中
  5. 调整相机角度使得图像中心十字位于标靶中心$O$处, 且目测图像角度偏差不大
  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;