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