1// CalibCamera calibrates the camera
 2func CalibCamera(imgDatas [][]byte, chessboardRowCornerCount int, chessboardColCornerCount int, curImageIndex int) error {
 3	// Save the 2D coordinates of the corner points in each image, i.e., pixel coordinates
 4	imgPoints := gocv.NewPoints2fVector()
 5	defer imgPoints.Close()
 6	// Save the 3D coordinates of the corner points in each image, i.e., world coordinates
 7	objPoints := gocv.NewPoints3fVector()
 8	defer objPoints.Close()
 9	// Save the image size
10	imgSize := image.Point{X: 0, Y: 0}
11
12	// Define the world coordinate system for the 3D points
13	objectPoints := make([]gocv.Point3f, 0)
14	for i := 0; i < chessboardColCornerCount; i++ {
15		for j := 0; j < chessboardRowCornerCount; j++ {
16			objectPoints = append(objectPoints, gocv.Point3f{X: float32(j), Y: float32(i), Z: 0})
17		}
18	}
19	objPointsVector := gocv.NewPoint3fVectorFromPoints(objectPoints)
20	defer objPointsVector.Close()
21
22	// 1. Detect corner points
23	for index, imgData := range imgDatas {
24		err := func() error {
25			// Read as a grayscale image
26			grayImg, err := gocv.IMDecode(imgData, gocv.IMReadGrayScale)
27			if err != nil {
28				return err
29			}
30			if grayImg.Empty() {
31				return nil
32			}
33			defer grayImg.Close()
34			if index == 0 {
35				imgSize = image.Point{X: grayImg.Cols(), Y: grayImg.Rows()}
36			}
37			// Save the 2D corner points of the image
38			corners := gocv.NewMat()
39			defer corners.Close()
40			// Return true if the required number of corner points are found in the image
41			if found := gocv.FindChessboardCorners(grayImg, image.Pt(chessboardRowCornerCount, chessboardColCornerCount), &corners, gocv.CalibCBAdaptiveThresh|gocv.CalibCBFastCheck|gocv.CalibCBNormalizeImage); !found {
42				return errors.New("Not Found Corner Points")
43			}
44			// Termination criteria for the iterative algorithm (type of termination criteria, maximum number of iterations, desired accuracy)
45			criteria := gocv.NewTermCriteria(gocv.EPS|gocv.MaxIter, 30, 0.01)
46			// Further extract sub-pixel corner points to improve accuracy
47			gocv.CornerSubPix(grayImg, &corners, image.Pt(11, 11), image.Pt(-1, -1), criteria)
48			if corners.Cols()*corners.Rows() != chessboardColCornerCount*chessboardRowCornerCount {
49				return errors.New("Not Matched Number Of Corner Points")
50			}
51			imagePoints := make([]gocv.Point2f, 0)
52			// Note that the corners matrix has only one column, for example, 9*6 corner points are actually a 54*1 matrix, so world coordinates cannot be added here together
53			for i := 0; i < corners.Rows(); i++ {
54				for j := 0; j < corners.Cols(); j++ {
55					pixelX, pixelY := corners.GetFloatAt(i, j*2), corners.GetFloatAt(i, j*2+1)
56					imagePoints = append(imagePoints, gocv.Point2f{X: pixelX, Y: pixelY})
57				}
58			}
59			// Add pixel coordinates
60			imgPointsVector := gocv.NewPoint2fVectorFromPoints(imagePoints)
61			imgPoints.Append(imgPointsVector)
62			imgPointsVector.Close()
63			// Add world coordinates
64			objPoints.Append(objPointsVector)
65			return nil
66		}()
67		if err != nil {
68			return err
69		}
70	}
71
72	// 2. Calibration
73	// Translation vector for each image
74	transMat := gocv.NewMat()
75	defer transMat.Close()
76	// Rotation vector for each image
77	rotMat := gocv.NewMat()
78	defer rotMat.Close()
79	// Camera intrinsic matrix 3*3
80	cameraMatrix := gocv.NewMatWithSize(3, 3, gocv.MatTypeCV32F)
81	defer cameraMatrix.Close()
82	// 5 distortion coefficients of the camera: k1,k2,p1,p2,k3 1*5
83	distCoeffs := gocv.NewMat()
84	defer distCoeffs.Close()
85	res := gocv.CalibrateCamera(objPoints, imgPoints, imgSize, &cameraMatrix, &distCoeffs, &rotMat, &transMat, gocv.CalibFlag(0))
86
87	// 3. Save the calibration results
88	// Get the corrected new camera matrix
89	newCameramtx, _ := gocv.GetOptimalNewCameraMatrixWithParams(cameraMatrix, distCoeffs, imgSize, 1, imgSize, false)
90	defer newCameramtx.Close()
91}