Silencejql
0%

BBK-相机外参标定

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

主要利用Opencv中的Cv2.SolvePnP计算旋转平移矩阵来计算轮眉高度

1

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
List<Point2f> ImagePointlist = new List<Point2f>();
ImagePointlist.Add(new Point2f(_calibrationAgorithm.DotLeftTop.X, _calibrationAgorithm.DotLeftTop.Y));
ImagePointlist.Add(new Point2f(_calibrationAgorithm.DotRightTop.X, _calibrationAgorithm.DotRightTop.Y));
ImagePointlist.Add(new Point2f(_calibrationAgorithm.DotLeftBottom.X, _calibrationAgorithm.DotLeftBottom.Y));
ImagePointlist.Add(new Point2f(_calibrationAgorithm.DotRightBottom.X, _calibrationAgorithm.DotRightBottom.Y));

float LengthHor = (float)(_calibrationAgorithm.DotHorLength);
float LengthVer = (float)(_calibrationAgorithm.DotVerLength);
List<Point3f> WorldPointlist = new List<Point3f>();

if (_calibrationAgorithm.CoordinateType == ImageAgorithmDot.xCoordinateType.Theory)
{
    WorldPointlist.Add(new Point3f(0, 0, 0));
    WorldPointlist.Add(new Point3f(LengthHor, 0, 0));
    WorldPointlist.Add(new Point3f(0, LengthVer * -1, 0));
    WorldPointlist.Add(new Point3f(LengthHor , LengthVer * -1 , 0));
    Camera.ExtrinsicsCalibriton(WorldPointlist, ImagePointlist);
}
if (_calibrationAgorithm.CoordinateType == ImageAgorithmDot.xCoordinateType.Actual)
{
    Camera.Point3DX = _calibrationAgorithm.W_DotLeftTop.X;
    Camera.Point3DY = _calibrationAgorithm.W_DotLeftTop.Y;
    Camera.Point3DZ = Math.Abs(_calibrationAgorithm.W_DotLeftTop.Z);
    WorldPointlist.Add(new Point3f(0, 0, 0));
    WorldPointlist.Add(new Point3f((float)(_calibrationAgorithm.W_DotRightTop.X + _calibrationAgorithm.W_DotLeftTop.X), (float)(_calibrationAgorithm.W_DotRightTop.Y + _calibrationAgorithm.W_DotLeftTop.Y), (float)(_calibrationAgorithm.W_DotRightTop.Z + Math.Abs(_calibrationAgorithm.W_DotLeftTop.Z))));
    WorldPointlist.Add(new Point3f((float)(_calibrationAgorithm.W_DotLeftBottom.X + _calibrationAgorithm.W_DotLeftTop.X), (float)(_calibrationAgorithm.W_DotLeftBottom.Y + _calibrationAgorithm.W_DotLeftTop.Y), (float)(_calibrationAgorithm.W_DotLeftBottom.Z + Math.Abs(_calibrationAgorithm.W_DotLeftTop.Z))));
    WorldPointlist.Add(new Point3f((float)(_calibrationAgorithm.W_DotRightBottom.X + _calibrationAgorithm.W_DotLeftTop.X), (float)(_calibrationAgorithm.W_DotRightBottom.Y + _calibrationAgorithm.W_DotLeftTop.Y), (float)(_calibrationAgorithm.W_DotRightBottom.Z + Math.Abs(_calibrationAgorithm.W_DotLeftTop.Z))));

    Camera.ExtrinsicsCalibriton(WorldPointlist, ImagePointlist,true);
}

public void ExtrinsicsCalibriton(List<Point3f> WorldPoint, List<Point2f> ImagePoint, bool bActual = false)
{
    double[] Tvecs = new double[3];
    double[] Rvecs = new double[3];
    double[] tvecs = new double[3];
    double[] rvecs = new double[3];
    double[,] camera_matrix = new double[3, 3];

    double[] dist_coeffs = new double[5];

    camera_matrix[0, 0] = Intrinsics_Fx;
    camera_matrix[0, 1] = 0;
    camera_matrix[0, 2] = Intrinsics_Cx;
    camera_matrix[1, 0] = 0;
    camera_matrix[1, 1] = Intrinsics_Fy;
    camera_matrix[1, 2] = Intrinsics_Cy;
    camera_matrix[2, 0] = 0;
    camera_matrix[2, 1] = 0;
    camera_matrix[2, 2] = 1;

    //dist_coeffs[0] = Distortion_K1;
    //dist_coeffs[1] = Distortion_K2;
    //dist_coeffs[2] = Distortion_P1;
    //dist_coeffs[3] = Distortion_P2;
    //dist_coeffs[4] = Distortion_K3;

    Cv2.SolvePnP(WorldPoint, ImagePoint, camera_matrix, dist_coeffs.ToList(), ref Rvecs, ref Tvecs);

    Extrinsics_Rx = Rvecs[0];
    Extrinsics_Ry = Rvecs[1];
    Extrinsics_Rz = Rvecs[2];
    Extrinsics_Tx = Tvecs[0];
    Extrinsics_Ty = Tvecs[1];
    Extrinsics_Tz = Tvecs[2];

    double[,] rotM = new double[3, 3];
    Cv2.Rodrigues(Rvecs, out rotM);  //将旋转向量变换成旋转矩阵

    //不参与计算
    double theta_x = Math.Atan2(rotM[2, 1], rotM[2, 2]);
    double theta_y = Math.Atan2(-rotM[2, 0], Math.Sqrt(rotM[0, 0] * rotM[0, 0] + rotM[1, 0] * rotM[1, 0]));
    double theta_z = Math.Atan2(rotM[1, 0], rotM[0, 0]);
    //和上面一样
    //theta_x = Math.Atan2(rotM[2, 1], rotM[2, 2]);
    //theta_y = Math.Atan2(-rotM[2, 0], Math.Sqrt(rotM[2, 1] * rotM[2, 1] + rotM[2, 2] * rotM[2, 2]));
    //theta_z = Math.Atan2(rotM[1, 0], rotM[0, 0]);

    //将弧度转化为角度
    Extrinsics_Rx_Angle = theta_x * (180 / Math.PI);
    if (Extrinsics_Rx_Angle > 90)
        Extrinsics_Rx_Angle = 180 - Extrinsics_Rx_Angle;
    if (Extrinsics_Rx_Angle < -90)
        Extrinsics_Rx_Angle = -180 - Extrinsics_Rx_Angle;
    Extrinsics_Ry_Angle = theta_y * (180 / Math.PI);
    Extrinsics_Rz_Angle = theta_z * (180 / Math.PI);

    Matrix matrix = (new Matrix(Tvecs) * Matrix.transpose(new Matrix(rotM)));
    matrix = new Matrix(new double[] { 0, 0, 0 }) - matrix;

    Extrinsics_Tx_mm = matrix[0, 0];
    Extrinsics_Ty_mm = matrix[0, 1];
    Extrinsics_Tz_mm = matrix[0, 2];
    if (bActual)
    {
        Extrinsics_Tx_mm = Extrinsics_Tx_mm + Point3DX;
        Extrinsics_Ty_mm = Extrinsics_Ty_mm + Point3DY;
        Extrinsics_Tz_mm = Extrinsics_Tz_mm + Point3DZ;
    }
}