Download apps/interactive-calibration/calibController.cpp from tgollogly/test: direct link, hf CLI and curl.
- Browser
- Download file 14.8 kB
-
https://huggingface.co/tgollogly/test/resolve/main/apps/interactive-calibration/calibController.cpp
- Command line
-
hf download hf://tgollogly/test/apps/interactive-calibration/calibController.cpp
-
curl -L -o calibController.cpp https://huggingface.co/tgollogly/test/resolve/main/apps/interactive-calibration/calibController.cpp
14.8 kB
| // This file is part of OpenCV project. | |
| // It is subject to the license terms in the LICENSE file found in the top-level directory | |
| // of this distribution and at http://opencv.org/license.html. | |
| double calib::calibController::estimateCoverageQuality() | |
| { | |
| int gridSize = 10; | |
| int xGridStep = mCalibData->imageSize.width / gridSize; | |
| int yGridStep = mCalibData->imageSize.height / gridSize; | |
| std::vector<int> pointsInCell(gridSize*gridSize); | |
| std::fill(pointsInCell.begin(), pointsInCell.end(), 0); | |
| for(std::vector<std::vector<cv::Point2f> >::iterator it = mCalibData->imagePoints.begin(); it != mCalibData->imagePoints.end(); ++it) | |
| for(std::vector<cv::Point2f>::iterator pointIt = (*it).begin(); pointIt != (*it).end(); ++pointIt) { | |
| int i = (int)((*pointIt).x / xGridStep); | |
| int j = (int)((*pointIt).y / yGridStep); | |
| pointsInCell[i*gridSize + j]++; | |
| } | |
| for(std::vector<cv::Mat>::iterator it = mCalibData->allCharucoCorners.begin(); it != mCalibData->allCharucoCorners.end(); ++it) | |
| for(int l = 0; l < (*it).size[0]; l++) { | |
| int i = (int)((*it).at<float>(l, 0) / xGridStep); | |
| int j = (int)((*it).at<float>(l, 1) / yGridStep); | |
| pointsInCell[i*gridSize + j]++; | |
| } | |
| cv::Mat mean, stdDev; | |
| cv::meanStdDev(pointsInCell, mean, stdDev); | |
| return mean.at<double>(0) / (stdDev.at<double>(0) + 1e-7); | |
| } | |
| calib::calibController::calibController() | |
| { | |
| mCalibFlags = 0; | |
| } | |
| calib::calibController::calibController(cv::Ptr<calib::calibrationData> data, int initialFlags, bool autoTuning, int minFramesNum) : | |
| mCalibData(data) | |
| { | |
| mCalibFlags = initialFlags; | |
| mNeedTuning = autoTuning; | |
| mMinFramesNum = minFramesNum; | |
| mConfIntervalsState = false; | |
| mCoverageQualityState = false; | |
| } | |
| void calib::calibController::updateState() | |
| { | |
| if(mCalibData->cameraMatrix.total()) { | |
| const double relErrEps = 0.05; | |
| bool fConfState = false, cConfState = false, dConfState = true; | |
| if(sigmaMult*mCalibData->stdDeviations.at<double>(0) / mCalibData->cameraMatrix.at<double>(0,0) < relErrEps && | |
| sigmaMult*mCalibData->stdDeviations.at<double>(1) / mCalibData->cameraMatrix.at<double>(1,1) < relErrEps) | |
| fConfState = true; | |
| if(sigmaMult*mCalibData->stdDeviations.at<double>(2) / mCalibData->cameraMatrix.at<double>(0,2) < relErrEps && | |
| sigmaMult*mCalibData->stdDeviations.at<double>(3) / mCalibData->cameraMatrix.at<double>(1,2) < relErrEps) | |
| cConfState = true; | |
| for(int i = 0; i < 5; i++) | |
| if(mCalibData->stdDeviations.at<double>(4+i) / fabs(mCalibData->distCoeffs.at<double>(i)) > 1) | |
| dConfState = false; | |
| mConfIntervalsState = fConfState && cConfState && dConfState; | |
| } | |
| if(getFramesNumberState()) | |
| mCoverageQualityState = estimateCoverageQuality() > 1.8 ? true : false; | |
| if (getFramesNumberState() && mNeedTuning) { | |
| if( !(mCalibFlags & cv::CALIB_FIX_ASPECT_RATIO) && | |
| mCalibData->cameraMatrix.total()) { | |
| double fDiff = fabs(mCalibData->cameraMatrix.at<double>(0,0) - | |
| mCalibData->cameraMatrix.at<double>(1,1)); | |
| if (fDiff < 3*mCalibData->stdDeviations.at<double>(0) && | |
| fDiff < 3*mCalibData->stdDeviations.at<double>(1)) { | |
| mCalibFlags |= cv::CALIB_FIX_ASPECT_RATIO; | |
| mCalibData->cameraMatrix.at<double>(0,0) = | |
| mCalibData->cameraMatrix.at<double>(1,1); | |
| } | |
| } | |
| if(!(mCalibFlags & cv::CALIB_ZERO_TANGENT_DIST)) { | |
| const double eps = 0.005; | |
| if(fabs(mCalibData->distCoeffs.at<double>(2)) < eps && | |
| fabs(mCalibData->distCoeffs.at<double>(3)) < eps) | |
| mCalibFlags |= cv::CALIB_ZERO_TANGENT_DIST; | |
| } | |
| if(!(mCalibFlags & cv::CALIB_FIX_K1)) { | |
| const double eps = 0.005; | |
| if(fabs(mCalibData->distCoeffs.at<double>(0)) < eps) | |
| mCalibFlags |= cv::CALIB_FIX_K1; | |
| } | |
| if(!(mCalibFlags & cv::CALIB_FIX_K2)) { | |
| const double eps = 0.005; | |
| if(fabs(mCalibData->distCoeffs.at<double>(1)) < eps) | |
| mCalibFlags |= cv::CALIB_FIX_K2; | |
| } | |
| if(!(mCalibFlags & cv::CALIB_FIX_K3)) { | |
| const double eps = 0.005; | |
| if(fabs(mCalibData->distCoeffs.at<double>(4)) < eps) | |
| mCalibFlags |= cv::CALIB_FIX_K3; | |
| } | |
| } | |
| } | |
| bool calib::calibController::getCommonCalibrationState() const | |
| { | |
| int rating = (int)getFramesNumberState() + (int)getConfidenceIntrervalsState() + | |
| (int)getRMSState() + (int)mCoverageQualityState; | |
| return rating == 4; | |
| } | |
| bool calib::calibController::getFramesNumberState() const | |
| { | |
| return std::max(mCalibData->imagePoints.size(), mCalibData->allCharucoCorners.size()) > mMinFramesNum; | |
| } | |
| bool calib::calibController::getConfidenceIntrervalsState() const | |
| { | |
| return mConfIntervalsState; | |
| } | |
| bool calib::calibController::getRMSState() const | |
| { | |
| return mCalibData->totalAvgErr < 0.5; | |
| } | |
| int calib::calibController::getNewFlags() const | |
| { | |
| return mCalibFlags; | |
| } | |
| //////////////////// calibDataController | |
| double calib::calibDataController::estimateGridSubsetQuality(size_t excludedIndex) | |
| { | |
| { | |
| int gridSize = 10; | |
| int xGridStep = mCalibData->imageSize.width / gridSize; | |
| int yGridStep = mCalibData->imageSize.height / gridSize; | |
| std::vector<int> pointsInCell(gridSize*gridSize); | |
| std::fill(pointsInCell.begin(), pointsInCell.end(), 0); | |
| for(size_t k = 0; k < mCalibData->imagePoints.size(); k++) | |
| if(k != excludedIndex) | |
| for(std::vector<cv::Point2f>::iterator pointIt = mCalibData->imagePoints[k].begin(); pointIt != mCalibData->imagePoints[k].end(); ++pointIt) { | |
| int i = (int)((*pointIt).x / xGridStep); | |
| int j = (int)((*pointIt).y / yGridStep); | |
| pointsInCell[i*gridSize + j]++; | |
| } | |
| for(size_t k = 0; k < mCalibData->allCharucoCorners.size(); k++) | |
| if(k != excludedIndex) | |
| for(int l = 0; l < mCalibData->allCharucoCorners[k].size[0]; l++) { | |
| int i = (int)(mCalibData->allCharucoCorners[k].at<float>(l, 0) / xGridStep); | |
| int j = (int)(mCalibData->allCharucoCorners[k].at<float>(l, 1) / yGridStep); | |
| pointsInCell[i*gridSize + j]++; | |
| } | |
| cv::Mat mean, stdDev; | |
| cv::meanStdDev(pointsInCell, mean, stdDev); | |
| return mean.at<double>(0) / (stdDev.at<double>(0) + 1e-7); | |
| } | |
| } | |
| calib::calibDataController::calibDataController(cv::Ptr<calib::calibrationData> data, int maxFrames, double convParameter) : | |
| mCalibData(data), mParamsFileName("CamParams.xml") | |
| { | |
| mMaxFramesNum = maxFrames; | |
| mAlpha = convParameter; | |
| } | |
| calib::calibDataController::calibDataController() | |
| { | |
| } | |
| void calib::calibDataController::filterFrames() | |
| { | |
| size_t numberOfFrames = std::max(mCalibData->allCharucoIds.size(), mCalibData->imagePoints.size()); | |
| CV_Assert(numberOfFrames == mCalibData->perViewErrors.total()); | |
| if(numberOfFrames >= mMaxFramesNum) { | |
| double worstValue = -HUGE_VAL, maxQuality = estimateGridSubsetQuality(numberOfFrames); | |
| size_t worstElemIndex = 0; | |
| for(size_t i = 0; i < numberOfFrames; i++) { | |
| double gridQDelta = estimateGridSubsetQuality(i) - maxQuality; | |
| double currentValue = mCalibData->perViewErrors.at<double>((int)i)*mAlpha + gridQDelta*(1. - mAlpha); | |
| if(currentValue > worstValue) { | |
| worstValue = currentValue; | |
| worstElemIndex = i; | |
| } | |
| } | |
| showOverlayMessage(cv::format("Frame %zu is worst", worstElemIndex + 1)); | |
| if(mCalibData->allFrames.size()) | |
| mCalibData->allFrames.erase(mCalibData->allFrames.begin() + worstElemIndex); | |
| if(mCalibData->imagePoints.size()) { | |
| mCalibData->imagePoints.erase(mCalibData->imagePoints.begin() + worstElemIndex); | |
| mCalibData->objectPoints.erase(mCalibData->objectPoints.begin() + worstElemIndex); | |
| if (mCalibData->allCharucoCorners.size()) { | |
| mCalibData->allCharucoCorners.erase(mCalibData->allCharucoCorners.begin() + worstElemIndex); | |
| mCalibData->allCharucoIds.erase(mCalibData->allCharucoIds.begin() + worstElemIndex); | |
| } | |
| } | |
| cv::Mat newErrorsVec = cv::Mat((int)numberOfFrames - 1, 1, CV_64F); | |
| std::copy(mCalibData->perViewErrors.ptr<double>(0), | |
| mCalibData->perViewErrors.ptr<double>((int)worstElemIndex), newErrorsVec.ptr<double>(0)); | |
| if((int)worstElemIndex < (int)numberOfFrames-1) { | |
| std::copy(mCalibData->perViewErrors.ptr<double>((int)worstElemIndex + 1), mCalibData->perViewErrors.ptr<double>((int)numberOfFrames), | |
| newErrorsVec.ptr<double>((int)worstElemIndex)); | |
| } | |
| mCalibData->perViewErrors = newErrorsVec; | |
| } | |
| } | |
| void calib::calibDataController::setParametersFileName(const std::string &name) | |
| { | |
| mParamsFileName = name; | |
| } | |
| void calib::calibDataController::deleteLastFrame() | |
| { | |
| if(!mCalibData->allFrames.empty()) | |
| { | |
| mCalibData->allFrames.pop_back(); | |
| } | |
| if( !mCalibData->imagePoints.empty()) { | |
| mCalibData->imagePoints.pop_back(); | |
| mCalibData->objectPoints.pop_back(); | |
| } | |
| if (!mCalibData->allCharucoCorners.empty()) { | |
| mCalibData->allCharucoCorners.pop_back(); | |
| mCalibData->allCharucoIds.pop_back(); | |
| } | |
| if(!mParamsStack.empty()) { | |
| mCalibData->cameraMatrix = (mParamsStack.top()).cameraMatrix; | |
| mCalibData->distCoeffs = (mParamsStack.top()).distCoeffs; | |
| mCalibData->stdDeviations = (mParamsStack.top()).stdDeviations; | |
| mCalibData->totalAvgErr = (mParamsStack.top()).avgError; | |
| mParamsStack.pop(); | |
| } | |
| } | |
| void calib::calibDataController::rememberCurrentParameters() | |
| { | |
| cv::Mat oldCameraMat, oldDistcoeefs, oldStdDevs; | |
| mCalibData->cameraMatrix.copyTo(oldCameraMat); | |
| mCalibData->distCoeffs.copyTo(oldDistcoeefs); | |
| mCalibData->stdDeviations.copyTo(oldStdDevs); | |
| mParamsStack.push(cameraParameters(oldCameraMat, oldDistcoeefs, oldStdDevs, mCalibData->totalAvgErr)); | |
| } | |
| void calib::calibDataController::deleteAllData() | |
| { | |
| mCalibData->allFrames.clear(); | |
| mCalibData->imagePoints.clear(); | |
| mCalibData->objectPoints.clear(); | |
| mCalibData->allCharucoCorners.clear(); | |
| mCalibData->allCharucoIds.clear(); | |
| mCalibData->cameraMatrix = mCalibData->distCoeffs = cv::Mat(); | |
| mParamsStack = std::stack<cameraParameters>(); | |
| rememberCurrentParameters(); | |
| } | |
| bool calib::calibDataController::saveCurrentCameraParameters() const | |
| { | |
| for(size_t i = 0; i < mCalibData->allFrames.size(); i++) | |
| cv::imwrite(cv::format("calibration_%zu.png", i), mCalibData->allFrames[i]); | |
| bool success = false; | |
| if(mCalibData->cameraMatrix.total()) { | |
| cv::FileStorage parametersWriter(mParamsFileName, cv::FileStorage::WRITE); | |
| if(parametersWriter.isOpened()) { | |
| time_t rawtime; | |
| time(&rawtime); | |
| char buf[256]; | |
| strftime(buf, sizeof(buf)-1, "%c", localtime(&rawtime)); | |
| parametersWriter << "calibrationDate" << buf; | |
| parametersWriter << "framesCount" << std::max((int)mCalibData->objectPoints.size(), (int)mCalibData->allCharucoCorners.size()); | |
| parametersWriter << "cameraResolution" << mCalibData->imageSize; | |
| parametersWriter << "cameraMatrix" << mCalibData->cameraMatrix; | |
| parametersWriter << "cameraMatrix_std_dev" << mCalibData->stdDeviations.rowRange(cv::Range(0, 4)); | |
| parametersWriter << "dist_coeffs" << mCalibData->distCoeffs; | |
| parametersWriter << "dist_coeffs_std_dev" << mCalibData->stdDeviations.rowRange(cv::Range(4, 9)); | |
| parametersWriter << "avg_reprojection_error" << mCalibData->totalAvgErr; | |
| parametersWriter.release(); | |
| success = true; | |
| } | |
| } | |
| return success; | |
| } | |
| void calib::calibDataController::printParametersToConsole(std::ostream &output) const | |
| { | |
| const char* border = "---------------------------------------------------"; | |
| output << border << std::endl; | |
| output << "Frames used for calibration: " << std::max(mCalibData->objectPoints.size(), mCalibData->allCharucoCorners.size()) | |
| << " \t RMS = " << mCalibData->totalAvgErr << std::endl; | |
| if(mCalibData->cameraMatrix.at<double>(0,0) == mCalibData->cameraMatrix.at<double>(1,1)) | |
| output << "F = " << mCalibData->cameraMatrix.at<double>(1,1) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(1) << std::endl; | |
| else | |
| output << "Fx = " << mCalibData->cameraMatrix.at<double>(0,0) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(0) << " \t " | |
| << "Fy = " << mCalibData->cameraMatrix.at<double>(1,1) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(1) << std::endl; | |
| output << "Cx = " << mCalibData->cameraMatrix.at<double>(0,2) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(2) << " \t" | |
| << "Cy = " << mCalibData->cameraMatrix.at<double>(1,2) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(3) << std::endl; | |
| output << "K1 = " << mCalibData->distCoeffs.at<double>(0) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(4) << std::endl; | |
| output << "K2 = " << mCalibData->distCoeffs.at<double>(1) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(5) << std::endl; | |
| output << "K3 = " << mCalibData->distCoeffs.at<double>(4) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(8) << std::endl; | |
| output << "TD1 = " << mCalibData->distCoeffs.at<double>(2) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(6) << std::endl; | |
| output << "TD2 = " << mCalibData->distCoeffs.at<double>(3) << " +- " << sigmaMult*mCalibData->stdDeviations.at<double>(7) << std::endl; | |
| } | |
| void calib::calibDataController::updateUndistortMap() | |
| { | |
| cv::initUndistortRectifyMap(mCalibData->cameraMatrix, mCalibData->distCoeffs, cv::noArray(), | |
| cv::getOptimalNewCameraMatrix(mCalibData->cameraMatrix, mCalibData->distCoeffs, mCalibData->imageSize, 0.0, mCalibData->imageSize), | |
| mCalibData->imageSize, CV_16SC2, mCalibData->undistMap1, mCalibData->undistMap2); | |
| } | |