apps/interactive-calibration/calibController.cpp

   1 // This file is part of OpenCV project.
   2 // It is subject to the license terms in the LICENSE file found in the top-level directory
   3 // of this distribution and at http://opencv.org/license.html.
   4
   5 #include "calibController.hpp"
   6
   7 #include <algorithm>
   8 #include <cmath>
   9 #include <ctime>
  10
  11 #include <opencv2/calib3d.hpp>
  12 #include <opencv2/imgproc.hpp>
  13 #include <opencv2/imgcodecs.hpp>
  14
  15 double calib::calibController::estimateCoverageQuality()
  16 {
  17     int gridSize = 10;
  18     int xGridStep = mCalibData->imageSize.width / gridSize;
  19     int yGridStep = mCalibData->imageSize.height / gridSize;
  20     std::vector<int> pointsInCell(gridSize*gridSize);
  21
  22     std::fill(pointsInCell.begin(), pointsInCell.end(), 0);
  23
  24     for(std::vector<std::vector<cv::Point2f> >::iterator it = mCalibData->imagePoints.begin(); it != mCalibData->imagePoints.end(); ++it)
  25         for(std::vector<cv::Point2f>::iterator pointIt = (*it).begin(); pointIt != (*it).end(); ++pointIt) {
  26             int i = (int)((*pointIt).x / xGridStep);
  27             int j = (int)((*pointIt).y / yGridStep);
  28             pointsInCell[i*gridSize + j]++;
  29         }
  30
  31     for(std::vector<cv::Mat>::iterator it = mCalibData->allCharucoCorners.begin(); it != mCalibData->allCharucoCorners.end(); ++it)
  32         for(int l = 0; l < (*it).size[0]; l++) {
  33             int i = (int)((*it).at<float>(l, 0) / xGridStep);
  34             int j = (int)((*it).at<float>(l, 1) / yGridStep);
  35             pointsInCell[i*gridSize + j]++;
  36         }
  37
  38     cv::Mat mean, stdDev;
  39     cv::meanStdDev(pointsInCell, mean, stdDev);
  40
  41     return mean.at<double>(0) / (stdDev.at<double>(0) + 1e-7);
  42 }
  43
  44 calib::calibController::calibController()
  45 {
  46     mCalibFlags = 0;
  47 }
  48
  49 calib::calibController::calibController(cv::Ptr<calib::calibrationData> data, int initialFlags, bool autoTuning, int minFramesNum) :
  50     mCalibData(data)
  51 {
  52     mCalibFlags = initialFlags;
  53     mNeedTuning = autoTuning;
  54     mMinFramesNum = minFramesNum;
  55     mConfIntervalsState = false;
  56     mCoverageQualityState = false;
  57 }
  58
  59 void calib::calibController::updateState()
  60 {
  61     if(mCalibData->cameraMatrix.total()) {
  62         const double relErrEps = 0.05;
  63         bool fConfState = false, cConfState = false, dConfState = true;
  64         if(sigmaMult*mCalibData->stdDeviations.at<double>(0) / mCalibData->cameraMatrix.at<double>(0,0) < relErrEps &&
  65                 sigmaMult*mCalibData->stdDeviations.at<double>(1) / mCalibData->cameraMatrix.at<double>(1,1) < relErrEps)
  66             fConfState = true;
  67         if(sigmaMult*mCalibData->stdDeviations.at<double>(2) / mCalibData->cameraMatrix.at<double>(0,2) < relErrEps &&
  68                 sigmaMult*mCalibData->stdDeviations.at<double>(3) / mCalibData->cameraMatrix.at<double>(1,2) < relErrEps)
  69             cConfState = true;
  70
  71         for(int i = 0; i < 5; i++)
  72             if(mCalibData->stdDeviations.at<double>(4+i) / fabs(mCalibData->distCoeffs.at<double>(i)) > 1)
  73                 dConfState = false;
  74
  75         mConfIntervalsState = fConfState && cConfState && dConfState;
  76     }
  77
  78     if(getFramesNumberState())
  79         mCoverageQualityState = estimateCoverageQuality() > 1.8 ? true : false;
  80
  81     if (getFramesNumberState() && mNeedTuning) {
  82         if( !(mCalibFlags & cv::CALIB_FIX_ASPECT_RATIO) &&
  83             mCalibData->cameraMatrix.total()) {
  84             double fDiff = fabs(mCalibData->cameraMatrix.at<double>(0,0) -
  85                                 mCalibData->cameraMatrix.at<double>(1,1));
  86
  87             if (fDiff < 3*mCalibData->stdDeviations.at<double>(0) &&
  88                     fDiff < 3*mCalibData->stdDeviations.at<double>(1)) {
  89                 mCalibFlags |= cv::CALIB_FIX_ASPECT_RATIO;
  90                 mCalibData->cameraMatrix.at<double>(0,0) =
  91                         mCalibData->cameraMatrix.at<double>(1,1);
  92             }
  93         }
  94
  95         if(!(mCalibFlags & cv::CALIB_ZERO_TANGENT_DIST)) {
  96             const double eps = 0.005;
  97             if(fabs(mCalibData->distCoeffs.at<double>(2)) < eps &&
  98                     fabs(mCalibData->distCoeffs.at<double>(3)) < eps)
  99                 mCalibFlags |= cv::CALIB_ZERO_TANGENT_DIST;
 100         }
 101
 102         if(!(mCalibFlags & cv::CALIB_FIX_K1)) {
 103             const double eps = 0.005;
 104             if(fabs(mCalibData->distCoeffs.at<double>(0)) < eps)
 105                 mCalibFlags |= cv::CALIB_FIX_K1;
 106         }
 107
 108         if(!(mCalibFlags & cv::CALIB_FIX_K2)) {
 109             const double eps = 0.005;
 110             if(fabs(mCalibData->distCoeffs.at<double>(1)) < eps)
 111                 mCalibFlags |= cv::CALIB_FIX_K2;
 112         }
 113
 114         if(!(mCalibFlags & cv::CALIB_FIX_K3)) {
 115             const double eps = 0.005;
 116             if(fabs(mCalibData->distCoeffs.at<double>(4)) < eps)
 117                 mCalibFlags |= cv::CALIB_FIX_K3;
 118         }
 119
 120     }
 121 }
 122
 123 bool calib::calibController::getCommonCalibrationState() const
 124 {
 125     int rating = (int)getFramesNumberState() + (int)getConfidenceIntrervalsState() +
 126             (int)getRMSState() + (int)mCoverageQualityState;
 127     return rating == 4;
 128 }
 129
 130 bool calib::calibController::getFramesNumberState() const
 131 {
 132     return std::max(mCalibData->imagePoints.size(), mCalibData->allCharucoCorners.size()) > mMinFramesNum;
 133 }
 134
 135 bool calib::calibController::getConfidenceIntrervalsState() const
 136 {
 137     return mConfIntervalsState;
 138 }
 139
 140 bool calib::calibController::getRMSState() const
 141 {
 142     return mCalibData->totalAvgErr < 0.5;
 143 }
 144
 145 int calib::calibController::getNewFlags() const
 146 {
 147     return mCalibFlags;
 148 }
 149
 150
 151 //////////////////// calibDataController
 152
 153 double calib::calibDataController::estimateGridSubsetQuality(size_t excludedIndex)
 154 {
 155     {
 156         int gridSize = 10;
 157         int xGridStep = mCalibData->imageSize.width / gridSize;
 158         int yGridStep = mCalibData->imageSize.height / gridSize;
 159         std::vector<int> pointsInCell(gridSize*gridSize);
 160
 161         std::fill(pointsInCell.begin(), pointsInCell.end(), 0);
 162
 163         for(size_t k = 0; k < mCalibData->imagePoints.size(); k++)
 164             if(k != excludedIndex)
 165                 for(std::vector<cv::Point2f>::iterator pointIt = mCalibData->imagePoints[k].begin(); pointIt != mCalibData->imagePoints[k].end(); ++pointIt) {
 166                     int i = (int)((*pointIt).x / xGridStep);
 167                     int j = (int)((*pointIt).y / yGridStep);
 168                     pointsInCell[i*gridSize + j]++;
 169                 }
 170
 171         for(size_t k = 0; k < mCalibData->allCharucoCorners.size(); k++)
 172             if(k != excludedIndex)
 173                 for(int l = 0; l <  mCalibData->allCharucoCorners[k].size[0]; l++) {
 174                     int i = (int)(mCalibData->allCharucoCorners[k].at<float>(l, 0) / xGridStep);
 175                     int j = (int)(mCalibData->allCharucoCorners[k].at<float>(l, 1) / yGridStep);
 176                     pointsInCell[i*gridSize + j]++;
 177                 }
 178
 179         cv::Mat mean, stdDev;
 180         cv::meanStdDev(pointsInCell, mean, stdDev);
 181
 182         return mean.at<double>(0) / (stdDev.at<double>(0) + 1e-7);
 183     }
 184 }
 185
 186 calib::calibDataController::calibDataController(cv::Ptr<calib::calibrationData> data, int maxFrames, double convParameter) :
 187     mCalibData(data), mParamsFileName("CamParams.xml")
 188 {
 189     mMaxFramesNum = maxFrames;
 190     mAlpha = convParameter;
 191 }
 192
 193 calib::calibDataController::calibDataController()
 194 {
 195
 196 }
 197
 198 void calib::calibDataController::filterFrames()
 199 {
 200     size_t numberOfFrames = std::max(mCalibData->allCharucoIds.size(), mCalibData->imagePoints.size());
 201     CV_Assert(numberOfFrames == mCalibData->perViewErrors.total());
 202     if(numberOfFrames >= mMaxFramesNum) {
 203
 204         double worstValue = -HUGE_VAL, maxQuality = estimateGridSubsetQuality(numberOfFrames);
 205         size_t worstElemIndex = 0;
 206         for(size_t i = 0; i < numberOfFrames; i++) {
 207             double gridQDelta = estimateGridSubsetQuality(i) - maxQuality;
 208             double currentValue = mCalibData->perViewErrors.at<double>((int)i)*mAlpha + gridQDelta*(1. - mAlpha);
 209             if(currentValue > worstValue) {
 210                 worstValue = currentValue;
 211                 worstElemIndex = i;
 212             }
 213         }
 214         showOverlayMessage(cv::format("Frame %zu is worst", worstElemIndex + 1));
 215
 216         if(mCalibData->allFrames.size())
 217             mCalibData->allFrames.erase(mCalibData->allFrames.begin() + worstElemIndex);
 218
 219         if(mCalibData->imagePoints.size()) {
 220             mCalibData->imagePoints.erase(mCalibData->imagePoints.begin() + worstElemIndex);
 221             mCalibData->objectPoints.erase(mCalibData->objectPoints.begin() + worstElemIndex);
 222         }
 223         else {
 224             mCalibData->allCharucoCorners.erase(mCalibData->allCharucoCorners.begin() + worstElemIndex);
 225             mCalibData->allCharucoIds.erase(mCalibData->allCharucoIds.begin() + worstElemIndex);
 226         }
 227
 228         cv::Mat newErrorsVec = cv::Mat((int)numberOfFrames - 1, 1, CV_64F);
 229         std::copy(mCalibData->perViewErrors.ptr<double>(0),
 230                   mCalibData->perViewErrors.ptr<double>((int)worstElemIndex), newErrorsVec.ptr<double>(0));
 231         if((int)worstElemIndex < (int)numberOfFrames-1) {
 232             std::copy(mCalibData->perViewErrors.ptr<double>((int)worstElemIndex + 1), mCalibData->perViewErrors.ptr<double>((int)numberOfFrames),
 233                     newErrorsVec.ptr<double>((int)worstElemIndex));
 234         }
 235         mCalibData->perViewErrors = newErrorsVec;
 236     }
 237 }
 238
 239 void calib::calibDataController::setParametersFileName(const std::string &name)
 240 {
 241     mParamsFileName = name;
 242 }
 243
 244 void calib::calibDataController::deleteLastFrame()
 245 {
 246     if(!mCalibData->allFrames.empty())
 247     {
 248         mCalibData->allFrames.pop_back();
 249     }
 250
 251     if( !mCalibData->imagePoints.empty()) {
 252         mCalibData->imagePoints.pop_back();
 253         mCalibData->objectPoints.pop_back();
 254     }
 255
 256     if (!mCalibData->allCharucoCorners.empty()) {
 257         mCalibData->allCharucoCorners.pop_back();
 258         mCalibData->allCharucoIds.pop_back();
 259     }
 260
 261     if(!mParamsStack.empty()) {
 262         mCalibData->cameraMatrix = (mParamsStack.top()).cameraMatrix;
 263         mCalibData->distCoeffs = (mParamsStack.top()).distCoeffs;
 264         mCalibData->stdDeviations = (mParamsStack.top()).stdDeviations;
 265         mCalibData->totalAvgErr = (mParamsStack.top()).avgError;
 266         mParamsStack.pop();
 267     }
 268 }
 269
 270 void calib::calibDataController::rememberCurrentParameters()
 271 {
 272     cv::Mat oldCameraMat, oldDistcoeefs, oldStdDevs;
 273     mCalibData->cameraMatrix.copyTo(oldCameraMat);
 274     mCalibData->distCoeffs.copyTo(oldDistcoeefs);
 275     mCalibData->stdDeviations.copyTo(oldStdDevs);
 276     mParamsStack.push(cameraParameters(oldCameraMat, oldDistcoeefs, oldStdDevs, mCalibData->totalAvgErr));
 277 }
 278
 279 void calib::calibDataController::deleteAllData()
 280 {
 281     mCalibData->allFrames.clear();
 282     mCalibData->imagePoints.clear();
 283     mCalibData->objectPoints.clear();
 284     mCalibData->allCharucoCorners.clear();
 285     mCalibData->allCharucoIds.clear();
 286     mCalibData->cameraMatrix = mCalibData->distCoeffs = cv::Mat();
 287     mParamsStack = std::stack<cameraParameters>();
 288     rememberCurrentParameters();
 289 }
 290
 291 bool calib::calibDataController::saveCurrentCameraParameters() const
 292 {
 293
 294     for(size_t i = 0; i < mCalibData->allFrames.size(); i++)
 295         cv::imwrite(cv::format("calibration_%zu.png", i), mCalibData->allFrames[i]);
 296
 297     bool success = false;
 298     if(mCalibData->cameraMatrix.total()) {
 299             cv::FileStorage parametersWriter(mParamsFileName, cv::FileStorage::WRITE);
 300             if(parametersWriter.isOpened()) {
 301                 time_t rawtime;
 302                 time(&rawtime);
 303                 char buf[256];
 304                 strftime(buf, sizeof(buf)-1, "%c", localtime(&rawtime));
 305
 306                 parametersWriter << "calibrationDate" << buf;
 307                 parametersWriter << "framesCount" << std::max((int)mCalibData->objectPoints.size(), (int)mCalibData->allCharucoCorners.size());
 308                 parametersWriter << "cameraResolution" << mCalibData->imageSize;
 309                 parametersWriter << "cameraMatrix" << mCalibData->cameraMatrix;
 310                 parametersWriter << "cameraMatrix_std_dev" << mCalibData->stdDeviations.rowRange(cv::Range(0, 4));
 311                 parametersWriter << "dist_coeffs" << mCalibData->distCoeffs;
 312                 parametersWriter << "dist_coeffs_std_dev" << mCalibData->stdDeviations.rowRange(cv::Range(4, 9));
 313                 parametersWriter << "avg_reprojection_error" << mCalibData->totalAvgErr;
 314
 315                 parametersWriter.release();
 316                 success = true;
 317         }
 318     }
 319     return success;
 320 }
 321
 322 void calib::calibDataController::printParametersToConsole(std::ostream &output) const
 323 {
 324     const char* border = "---------------------------------------------------";
 325     output << border << std::endl;
 326     output << "Frames used for calibration: " << std::max(mCalibData->objectPoints.size(), mCalibData->allCharucoCorners.size())
 327            << " \t RMS = " << mCalibData->totalAvgErr << std::endl;
 328     if(mCalibData->cameraMatrix.at<double>(0,0) == mCalibData->cameraMatrix.at<double>(1,1))
 329         output << "F = " << mCalibData->cameraMatrix.at<double>(1,1) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(1) << std::endl;
 330     else
 331         output << "Fx = " << mCalibData->cameraMatrix.at<double>(0,0) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(0) << " \t "
 332                << "Fy = " << mCalibData->cameraMatrix.at<double>(1,1) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(1) << std::endl;
 333     output << "Cx = " << mCalibData->cameraMatrix.at<double>(0,2) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(2) << " \t"
 334            << "Cy = " << mCalibData->cameraMatrix.at<double>(1,2) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(3) << std::endl;
 335     output << "K1 = " << mCalibData->distCoeffs.at<double>(0) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(4) << std::endl;
 336     output << "K2 = " << mCalibData->distCoeffs.at<double>(1) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(5) << std::endl;
 337     output << "K3 = " << mCalibData->distCoeffs.at<double>(4) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(8) << std::endl;
 338     output << "TD1 = " << mCalibData->distCoeffs.at<double>(2) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(6) << std::endl;
 339     output << "TD2 = " << mCalibData->distCoeffs.at<double>(3) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(7) << std::endl;
 340 }
 341
 342 void calib::calibDataController::updateUndistortMap()
 343 {
 344     cv::initUndistortRectifyMap(mCalibData->cameraMatrix, mCalibData->distCoeffs, cv::noArray(),
 345                                 cv::getOptimalNewCameraMatrix(mCalibData->cameraMatrix, mCalibData->distCoeffs, mCalibData->imageSize, 0.0, mCalibData->imageSize),
 346                                 mCalibData->imageSize, CV_16SC2, mCalibData->undistMap1, mCalibData->undistMap2);
 347
 348 }