samples/android/camera-calibration/src/org/opencv/samples/cameracalibration/CameraCalibrator.java

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