1package org.opencv.samples.cameracalibration;
2
3import java.util.ArrayList;
4import java.util.List;
5
6import org.opencv.calib3d.Calib3d;
7import org.opencv.core.Core;
8import org.opencv.core.CvType;
9import org.opencv.core.Mat;
10import org.opencv.core.MatOfDouble;
11import org.opencv.core.MatOfPoint2f;
12import org.opencv.core.MatOfPoint3f;
13import org.opencv.core.Point;
14import org.opencv.core.Scalar;
15import org.opencv.core.Size;
16import org.opencv.imgproc.Imgproc;
17
18import android.util.Log;
19
20public class CameraCalibrator {
21    private static final String TAG = "OCVSample::CameraCalibrator";
22
23    private final Size mPatternSize = new Size(4, 11);
24    private final int mCornersSize = (int)(mPatternSize.width * mPatternSize.height);
25    private boolean mPatternWasFound = false;
26    private MatOfPoint2f mCorners = new MatOfPoint2f();
27    private List<Mat> mCornersBuffer = new ArrayList<Mat>();
28    private boolean mIsCalibrated = false;
29
30    private Mat mCameraMatrix = new Mat();
31    private Mat mDistortionCoefficients = new Mat();
32    private int mFlags;
33    private double mRms;
34    private double mSquareSize = 0.0181;
35    private Size mImageSize;
36
37    public CameraCalibrator(int width, int height) {
38        mImageSize = new Size(width, height);
39        mFlags = Calib3d.CALIB_FIX_PRINCIPAL_POINT +
40                 Calib3d.CALIB_ZERO_TANGENT_DIST +
41                 Calib3d.CALIB_FIX_ASPECT_RATIO +
42                 Calib3d.CALIB_FIX_K4 +
43                 Calib3d.CALIB_FIX_K5;
44        Mat.eye(3, 3, CvType.CV_64FC1).copyTo(mCameraMatrix);
45        mCameraMatrix.put(0, 0, 1.0);
46        Mat.zeros(5, 1, CvType.CV_64FC1).copyTo(mDistortionCoefficients);
47        Log.i(TAG, "Instantiated new " + this.getClass());
48    }
49
50    public void processFrame(Mat grayFrame, Mat rgbaFrame) {
51        findPattern(grayFrame);
52        renderFrame(rgbaFrame);
53    }
54
55    public void calibrate() {
56        ArrayList<Mat> rvecs = new ArrayList<Mat>();
57        ArrayList<Mat> tvecs = new ArrayList<Mat>();
58        Mat reprojectionErrors = new Mat();
59        ArrayList<Mat> objectPoints = new ArrayList<Mat>();
60        objectPoints.add(Mat.zeros(mCornersSize, 1, CvType.CV_32FC3));
61        calcBoardCornerPositions(objectPoints.get(0));
62        for (int i = 1; i < mCornersBuffer.size(); i++) {
63            objectPoints.add(objectPoints.get(0));
64        }
65
66        Calib3d.calibrateCamera(objectPoints, mCornersBuffer, mImageSize,
67                mCameraMatrix, mDistortionCoefficients, rvecs, tvecs, mFlags);
68
69        mIsCalibrated = Core.checkRange(mCameraMatrix)
70                && Core.checkRange(mDistortionCoefficients);
71
72        mRms = computeReprojectionErrors(objectPoints, rvecs, tvecs, reprojectionErrors);
73        Log.i(TAG, String.format("Average re-projection error: %f", mRms));
74        Log.i(TAG, "Camera matrix: " + mCameraMatrix.dump());
75        Log.i(TAG, "Distortion coefficients: " + mDistortionCoefficients.dump());
76    }
77
78    public void clearCorners() {
79        mCornersBuffer.clear();
80    }
81
82    private void calcBoardCornerPositions(Mat corners) {
83        final int cn = 3;
84        float positions[] = new float[mCornersSize * cn];
85
86        for (int i = 0; i < mPatternSize.height; i++) {
87            for (int j = 0; j < mPatternSize.width * cn; j += cn) {
88                positions[(int) (i * mPatternSize.width * cn + j + 0)] =
89                        (2 * (j / cn) + i % 2) * (float) mSquareSize;
90                positions[(int) (i * mPatternSize.width * cn + j + 1)] =
91                        i * (float) mSquareSize;
92                positions[(int) (i * mPatternSize.width * cn + j + 2)] = 0;
93            }
94        }
95        corners.create(mCornersSize, 1, CvType.CV_32FC3);
96        corners.put(0, 0, positions);
97    }
98
99    private double computeReprojectionErrors(List<Mat> objectPoints,
100            List<Mat> rvecs, List<Mat> tvecs, Mat perViewErrors) {
101        MatOfPoint2f cornersProjected = new MatOfPoint2f();
102        double totalError = 0;
103        double error;
104        float viewErrors[] = new float[objectPoints.size()];
105
106        MatOfDouble distortionCoefficients = new MatOfDouble(mDistortionCoefficients);
107        int totalPoints = 0;
108        for (int i = 0; i < objectPoints.size(); i++) {
109            MatOfPoint3f points = new MatOfPoint3f(objectPoints.get(i));
110            Calib3d.projectPoints(points, rvecs.get(i), tvecs.get(i),
111                    mCameraMatrix, distortionCoefficients, cornersProjected);
112            error = Core.norm(mCornersBuffer.get(i), cornersProjected, Core.NORM_L2);
113
114            int n = objectPoints.get(i).rows();
115            viewErrors[i] = (float) Math.sqrt(error * error / n);
116            totalError  += error * error;
117            totalPoints += n;
118        }
119        perViewErrors.create(objectPoints.size(), 1, CvType.CV_32FC1);
120        perViewErrors.put(0, 0, viewErrors);
121
122        return Math.sqrt(totalError / totalPoints);
123    }
124
125    private void findPattern(Mat grayFrame) {
126        mPatternWasFound = Calib3d.findCirclesGrid(grayFrame, mPatternSize,
127                mCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
128    }
129
130    public void addCorners() {
131        if (mPatternWasFound) {
132            mCornersBuffer.add(mCorners.clone());
133        }
134    }
135
136    private void drawPoints(Mat rgbaFrame) {
137        Calib3d.drawChessboardCorners(rgbaFrame, mPatternSize, mCorners, mPatternWasFound);
138    }
139
140    private void renderFrame(Mat rgbaFrame) {
141        drawPoints(rgbaFrame);
142
143        Imgproc.putText(rgbaFrame, "Captured: " + mCornersBuffer.size(), new Point(rgbaFrame.cols() / 3 * 2, rgbaFrame.rows() * 0.1),
144                Core.FONT_HERSHEY_SIMPLEX, 1.0, new Scalar(255, 255, 0));
145    }
146
147    public Mat getCameraMatrix() {
148        return mCameraMatrix;
149    }
150
151    public Mat getDistortionCoefficients() {
152        return mDistortionCoefficients;
153    }
154
155    public int getCornersBufferSize() {
156        return mCornersBuffer.size();
157    }
158
159    public double getAvgReprojectionError() {
160        return mRms;
161    }
162
163    public boolean isCalibrated() {
164        return mIsCalibrated;
165    }
166
167    public void setCalibrated() {
168        mIsCalibrated = true;
169    }
170}
171