diff --git a/modules/photometric_calib/CMakeLists.txt b/modules/photometric_calib/CMakeLists.txt new file mode 100644 index 00000000000..696dfc0659f --- /dev/null +++ b/modules/photometric_calib/CMakeLists.txt @@ -0,0 +1,2 @@ +set(the_description "Photometric Calibration") +ocv_define_module(photometric_calib opencv_core opencv_aruco opencv_calib3d opencv_highgui WRAP python) \ No newline at end of file diff --git a/modules/photometric_calib/README.md b/modules/photometric_calib/README.md new file mode 100644 index 00000000000..ba725c2b6c7 --- /dev/null +++ b/modules/photometric_calib/README.md @@ -0,0 +1,8 @@ +Photometric Calibration +================================================ + +Implementation of non-parametric photometric calibration algorithm proposed by J. Engel et al.: + +1. Camera Response Function Calibration +2. Vignette Calibration +3. Photometric Distortion Remover \ No newline at end of file diff --git a/modules/photometric_calib/doc/photometric_calib.bib b/modules/photometric_calib/doc/photometric_calib.bib new file mode 100644 index 00000000000..044d3ddca35 --- /dev/null +++ b/modules/photometric_calib/doc/photometric_calib.bib @@ -0,0 +1,9 @@ +@InProceedings{engel2016monodataset, + author = "J. Engel and V. Usenko and D. Cremers", + title = "A Photometrically Calibrated Benchmark For Monocular Visual Odometry", + booktitle = "arXiv:1607.02555", + arXiv = " arXiv:1607.02555", + year = "2016", + month = "July", + keywords={mono-ds,dso} +} \ No newline at end of file diff --git a/modules/photometric_calib/include/opencv2/photometric_calib.hpp b/modules/photometric_calib/include/opencv2/photometric_calib.hpp new file mode 100644 index 00000000000..2b5bfb6246a --- /dev/null +++ b/modules/photometric_calib/include/opencv2/photometric_calib.hpp @@ -0,0 +1,28 @@ +// 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. + +#ifndef __OPENCV_PHOTOMETRIC_CALIB_HPP__ +#define __OPENCV_PHOTOMETRIC_CALIB_HPP__ + +#include "opencv2/photometric_calib/Reader.hpp" +#include "opencv2/photometric_calib/GammaRemover.hpp" +#include "opencv2/photometric_calib/VignetteRemover.hpp" +#include "opencv2/photometric_calib/ResponseCalib.hpp" +#include "opencv2/photometric_calib/VignetteCalib.hpp" + +/** + * @defgroup photometric_calib Photometric Calibration + * The photometric_calib contains photomeric calibration algorithm proposed by Jakob Engel. \n + * The implementation is totally based on the paper \cite engel2016monodataset. \n + * Photometric calibration aimed at removing the camera response function and vitnetting artefact, + * by which the tracking and image alignment algorithms based on direct methods can be improved significantly. \n + * For details please refer to \cite engel2016monodataset. + */ + +namespace cv { +namespace photometric_calib { +} // namespace photometric_calib +} // namespace cv + +#endif \ No newline at end of file diff --git a/modules/photometric_calib/include/opencv2/photometric_calib/GammaRemover.hpp b/modules/photometric_calib/include/opencv2/photometric_calib/GammaRemover.hpp new file mode 100644 index 00000000000..37b8a8b332f --- /dev/null +++ b/modules/photometric_calib/include/opencv2/photometric_calib/GammaRemover.hpp @@ -0,0 +1,80 @@ +// 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. + +#ifndef _OPENCV_GAMMAREMOVER_HPP +#define _OPENCV_GAMMAREMOVER_HPP + +#include "opencv2/core.hpp" + +namespace cv { +namespace photometric_calib { + +//! @addtogroup photometric_calib +//! @{ + +/*! + * @brief Class for removing the camera response function (mostly gamma function) when provided with pcalib file. + * + */ + +class CV_EXPORTS GammaRemover +{ +public: + /*! + * @brief Constructor + * @param gammaPath the path of pcalib file of which the format should be .yaml or .yml + * @param w_ the width of input image + * @param h_ the height of input image + */ + GammaRemover(const std::string &gammaPath, int w_, int h_); + + /*! + * @brief get irradiance image in the form of cv::Mat. Convenient for display, etc. + * @param inputIm + * @return + */ + Mat getUnGammaImageMat(Mat inputIm); + + /*! + * @brief get irradiance image in the form of std::vector. Convenient for optimization or SLAM. + * @param inputIm + * @param outImVec + */ + void getUnGammaImageVec(Mat inputIm, std::vector &outImVec); + + /*! + * @brief get gamma function. + * @return + */ + inline float *getG() + { + if (!validGamma) + { return 0; } + else + { return G; } + }; + + /*! + * @brief get inverse gamma function + * @return + */ + inline float *getGInv() + { + if (!validGamma) + { return 0; } + else + { return GInv; } + }; + +private: + float G[256]; + float GInv[256]; + int w, h; + bool validGamma; +}; + +} // namespace photometric_calib +} // namespace cv + +#endif //_OPENCV__GAMMAREMOVER_HPP diff --git a/modules/photometric_calib/include/opencv2/photometric_calib/Reader.hpp b/modules/photometric_calib/include/opencv2/photometric_calib/Reader.hpp new file mode 100644 index 00000000000..b36ad9ccc58 --- /dev/null +++ b/modules/photometric_calib/include/opencv2/photometric_calib/Reader.hpp @@ -0,0 +1,99 @@ +// 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. + +#ifndef _OPENCV_READER_HPP +#define _OPENCV_READER_HPP + +#include "opencv2/core.hpp" + +#include +#include + +namespace cv { +namespace photometric_calib { + +//! @addtogroup photometric_calib +//! @{ + +/*! + * @brief Class for reading the sequence used for photometric calibration. Both the folder path of the sequence + * and the path of time file should be provided. The images of the sequence should be of format CV_8U. The time + * file should be .yaml or .yml. In the time file, the timestamps and exposure duration of the corresponding images + * of the sequence should be provided. + * + * The image paths are stored in std::vector images, timestamps are stored in std::vector timeStamps, + * exposure duration is stored in std::vector exposureTimes + */ + +class CV_EXPORTS Reader +{ +public: + /*! + * @brief Constructor + * @param folderPath the path of folder which contains the images + * @param imageExt the format of the input images, e.g., jpg or png. + * @param timesPath the path of time file + */ + Reader(const std::string &folderPath, const std::string &imageExt, const std::string ×Path); + + /*! + * @return the amount of images loaded + */ + unsigned long getNumImages() const; + + + /*! + * @brief Given the id of the image and return the image. id is in fact just the index of the image in the + * vector contains all the images. + * @param id + * @return Mat of the id^th image. + */ + Mat getImage(unsigned long id) const; + + /*! + * @brief Given the id of the image and return its timestamp value. id is in fact just the index of the image in the + * vector contains all the images. + * @param id + * @return timestamp of the id^th image. + */ + double getTimestamp(unsigned long id) const; + + /*! + * @brief Given the id of the image and return its exposure duration when is was taken. + * @param id + * @return exposure duration of the image. + */ + float getExposureDuration(unsigned long id) const; + + int getWidth() const; + + int getHeight() const; + + const std::string &getFolderPath() const; + + const std::string &getTimeFilePath() const; + +private: + /*! + * @brief Load timestamps and exposure duration. + * @param timesFile + */ + inline void loadTimestamps(const std::string ×File); + + std::vector images; //All the names/paths of images + std::vector timeStamps; //All the Unix Time Stamps of images + std::vector exposureDurations;//All the exposure duration for images + + int _width, _height;//The image width and height. All the images should be of the same size. + + std::string _folderPath; + std::string _timeFilePath; +}; + +//! @} + +} // namespace photometric_calib +} // namespace cv + +#endif //_OPENCV_READER_HPP \ No newline at end of file diff --git a/modules/photometric_calib/include/opencv2/photometric_calib/ResponseCalib.hpp b/modules/photometric_calib/include/opencv2/photometric_calib/ResponseCalib.hpp new file mode 100644 index 00000000000..e3657966b4e --- /dev/null +++ b/modules/photometric_calib/include/opencv2/photometric_calib/ResponseCalib.hpp @@ -0,0 +1,52 @@ +// 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. + +#ifndef _OPENCV_RESPONSECALIB_HPP +#define _OPENCV_RESPONSECALIB_HPP + +#include "opencv2/photometric_calib/Reader.hpp" + +namespace cv { +namespace photometric_calib { + +class CV_EXPORTS ResponseCalib +{ +public: + ResponseCalib(std::string folderPath, std::string timePath, std::string imageFormat); + + ResponseCalib(std::string folderPath, std::string timePath, int leakPadding, int nIts, int skipFrames, + std::string imageFormat); + + void plotE(const double *E, int w, int h, const std::string &saveTo); + + Vec2d rmse(const double *G, const double *E, const std::vector &exposureVec, + const std::vector &dataVec, int wh); + + void plotG(const double *G, const std::string &saveTo); + + void calib(bool debug); + + inline const std::string &getImageFolderPath() const + { + CV_Assert(imageReader); + return imageReader->getFolderPath(); + } + + inline const std::string &getTimeFilePath() const + { + CV_Assert(imageReader); + return imageReader->getTimeFilePath(); + } + +private: + int _leakPadding; + int _nIts; + int _skipFrames; + Reader *imageReader; +}; + +} // namespace photometric_calib +} // namespace cv + +#endif //_OPENCV_RESPONSECALIB_HPP diff --git a/modules/photometric_calib/include/opencv2/photometric_calib/VignetteCalib.hpp b/modules/photometric_calib/include/opencv2/photometric_calib/VignetteCalib.hpp new file mode 100644 index 00000000000..4308d7074e2 --- /dev/null +++ b/modules/photometric_calib/include/opencv2/photometric_calib/VignetteCalib.hpp @@ -0,0 +1,73 @@ +// 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. + +#ifndef _OPENCV_VIGNETTECALIB_HPP +#define _OPENCV_VIGNETTECALIB_HPP + +#include "opencv2/core.hpp" +#include "opencv2/photometric_calib/Reader.hpp" +#include "opencv2/photometric_calib/GammaRemover.hpp" + +namespace cv { +namespace photometric_calib { + + +class CV_EXPORTS VignetteCalib +{ +public: + VignetteCalib(std::string folderPath, std::string timePath, std::string cameraFile, std::string gammaFile, + std::string imageFormat); + + VignetteCalib(std::string folderPath, std::string timePath, std::string cameraFile, std::string gammaFile, + std::string imageFormat, int imageSkip, int maxIterations, int outlierTh, + int gridWidth, int gridHeight, float facW, float facH, int maxAbsGrad); + + virtual ~VignetteCalib(); + + //EIGEN_ALWAYS_INLINE float getInterpolatedElement(const float* const mat, const float x, const float y, const int width) + float getInterpolatedElement(const float *const mat, const float x, const float y, const int width); + + float calMeanExposureTime(); + + void displayImage(float *I, int w, int h, std::string name); + + void displayImageV(float *I, int w, int h, std::string name); + + bool preCalib(unsigned long id, float *&image, float *&plane2imgX, float *&plane2imgY, bool debug); + + void calib(bool debug); + + void calibFast(bool debug); + +private: + int _imageSkip; + int _maxIterations; + int _outlierTh; + + // grid width for template image. + int _gridWidth; + int _gridHeight; + + // width of grid relative to marker (fac times marker size) + float _facW; + float _facH; + + // remove pixel with absolute gradient larger than this from the optimization. + int _maxAbsGrad; + + Mat _cameraMatrix; + Mat _distCoeffs; + Matx33f _K_p2idx; + Matx33f _K_p2idx_inverse; + + Reader *imageReader; + GammaRemover *gammaRemover; + + float _meanExposure; +}; + +} // namespace photometric_calib +} // namespace cv + +#endif //_OPENCV_VIGNETTECALIB_HPP diff --git a/modules/photometric_calib/include/opencv2/photometric_calib/VignetteRemover.hpp b/modules/photometric_calib/include/opencv2/photometric_calib/VignetteRemover.hpp new file mode 100644 index 00000000000..c03ab275432 --- /dev/null +++ b/modules/photometric_calib/include/opencv2/photometric_calib/VignetteRemover.hpp @@ -0,0 +1,59 @@ +// 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. + +#ifndef _OPENCV_VIGNETTEREMOVER_HPP +#define _OPENCV_VIGNETTEREMOVER_HPP + +#include "opencv2/core.hpp" + +namespace cv { +namespace photometric_calib { + +//! @addtogroup photometric_calib +//! @{ + +/*! + * @brief Class for removing the vignetting artifact when provided with vignetting file. + * + */ + +class CV_EXPORTS VignetteRemover +{ +public: + /*! + * @brief Constructor + * @param vignettePath the path of vignetting file + * @param pcalibPath the path of pcalib file + * @param w_ the width of input image + * @param h_ the height of input image + */ + VignetteRemover(const std::string &vignettePath, const std::string &pcalibPath, int w_, int h_); + + ~VignetteRemover(); + + /*! + * @brief get vignetting-removed image in form of cv::Mat. + * @param oriImMat the image to be calibrated. + */ + Mat getUnVignetteImageMat(Mat oriImMat); + + /*! + * @brief get vignetting-removed image in form of std::vector. + * @param oriImMat the image to be calibrated. + * @param outImVec the vignetting-removed image vector. + */ + void getUnVignetteImageVec(Mat oriImMat, std::vector &outImVec); + +private: + float *vignetteMap; + float *vignetteMapInv; + std::string _pcalibPath; + int w, h; + bool validVignette; +}; + +} // namespace photometric_calib +} // namespace cv + +#endif //_OPENCV_VIGNETTEREMOVER_HPP diff --git a/modules/photometric_calib/samples/response_calibration.cpp b/modules/photometric_calib/samples/response_calibration.cpp new file mode 100644 index 00000000000..aec40fbda39 --- /dev/null +++ b/modules/photometric_calib/samples/response_calibration.cpp @@ -0,0 +1,52 @@ +#include "opencv2/core.hpp" +#include "opencv2/highgui.hpp" +#include "opencv2/photometric_calib.hpp" + +using namespace std; +using namespace cv; + +int main() +{ + // Please down load the sample dataset from: + // https://www.dropbox.com/s/5x48uhc7k2bgjcj/GSoC2017_PhotometricCalib_Sample_Data.zip?dl=0 + // By unzipping the file, you would get a folder named /GSoC2017_PhotometricCalib_Sample_Data which contains 4 subfolders: + // response_calib, response remover, vignette_calib, vignette_remover + // in this sample, we will use the data in the folder response_calib and response_remover + + // Prefix for the data, e.g. /Users/Yelen/GSoC2017_PhotometricCalib_Sample + string userPrefix = "/Users/Yelen/GSoC2017_PhotometricCalib_Sample_Data/"; + // The path for the images used for response calibration + string imageFolderPath = userPrefix + "response_calib/images"; + // The yaml file which contains the timestamps and exposure times for each image used for camera response calibration + string timePath = userPrefix + "response_calib/times.yaml"; + + // Construct a photometric_calib::ResponseCalib object by giving path of image, path of time file and specify the format of images + photometric_calib::ResponseCalib resCal(imageFolderPath, timePath, "jpg"); + + // Debug mode will generate some temporary data + bool debug = true; + // Calibration of camera response function begins + resCal.calib(debug); + + // The result and some intermediate data are stored in the folder ./photoCalibResult in which + // pcalib.yaml is the camera response function file + // Since we are using debug mode, we can visualize the response function: + Mat invRes = imread("./photoCalibResult/G-10.png", IMREAD_UNCHANGED); + // As shown as Fig.3 in the paper from J.Engel, et al. in the paper A Photometrically Calibrated Benchmark For Monocular Visual Odometry + namedWindow( "Inverse Response Function", WINDOW_AUTOSIZE ); + imshow("Inverse Response Function", invRes); + + // To see the response-calibrated image, we can use GammaRemover + Mat oriImg = imread(imageFolderPath + "/00480.jpg", IMREAD_UNCHANGED); + photometric_calib::GammaRemover gammaRemover("./photoCalibResult/pcalib.yaml", oriImg.cols, oriImg.rows); + Mat caliImg = gammaRemover.getUnGammaImageMat(oriImg); + + // Visualization + namedWindow( "Original Image", WINDOW_AUTOSIZE ); + imshow("Original Image", oriImg); + namedWindow( "Gamma Removed Image", WINDOW_AUTOSIZE ); + imshow("Gamma Removed Image", caliImg); + waitKey(0); + + return 0; +} \ No newline at end of file diff --git a/modules/photometric_calib/samples/vignette_calibration.cpp b/modules/photometric_calib/samples/vignette_calibration.cpp new file mode 100644 index 00000000000..b47b109beec --- /dev/null +++ b/modules/photometric_calib/samples/vignette_calibration.cpp @@ -0,0 +1,67 @@ +#include "opencv2/core.hpp" +#include "opencv2/highgui.hpp" +#include "opencv2/photometric_calib.hpp" + +using namespace std; +using namespace cv; + +int main() +{ + // Please down load the sample dataset from: + // https://www.dropbox.com/s/5x48uhc7k2bgjcj/GSoC2017_PhotometricCalib_Sample_Data.zip?dl=0 + // By unzipping the file, you would get a folder named /GSoC2017_PhotometricCalib_Sample_Data which contains 2 subfolders: + // response_calib, vignette_calib + // in this sample, we will use the data in the folder vignette_calib + + // Prefix for the data, e.g. /Users/Yelen/GSoC2017_PhotometricCalib_Sample + string userPrefix = "/Users/Yelen/GSoC2017_PhotometricCalib_Sample_Data/"; + // The path for the images used for response calibration + string imageFolderPath = userPrefix + "vignette_calib/images"; + // The yaml file which contains the timestamps and exposure times for each image used for vignette calibration + string timePath = userPrefix + "vignette_calib/times.yaml"; + // The yaml file which contains the camera intrinsics and extrinsics. + // Note that the images are already rectified, so the distortion parameters are 0s + string cameraPath = userPrefix + "vignette_calib/camera.yaml"; + // The pcalib file. Vignette calibration can be performed only when provided with pcalib file. + // We use the identical pcalib.yaml file generated by response_calibration.cpp + // You can refer to the code in response_calibration.cpp for details + string gammaPath = userPrefix + "vignette_calib/pcalib.yaml"; + + // Construct a photometric_calib::VignetteCalib object by giving path of image, path of time file, camera parameter file, pcalib file and specify the format of images + photometric_calib::VignetteCalib vigCal(imageFolderPath, timePath, cameraPath, gammaPath, "jpg"); + + // Debug mode will visualize the optimization process and generate some temporary data + bool debug = true; + // Calibration of camera response function begins + vigCal.calib(debug); + + // You can also use fast mode, but with much memory (potentially with 10GB+) + // vigCal.calibFast(debug); + + // The result and some intermediate data are stored in the folder ./vignetteCalibResult in which + // vignette.png and vignetteSmoothed.png are the vignette images. + // In practice, vignetteSomoothed.png is used, since it doesn't have the black boarders. + Mat vigSmoothed = imread("./vignetteCalibResult/vignetteSmoothed.png", IMREAD_UNCHANGED); + // As shown as Fig.4 in the paper from J.Engel, et al. in the paper A Photometrically Calibrated Benchmark For Monocular Visual Odometry + namedWindow( "Vignette Smoothed", WINDOW_AUTOSIZE ); + imshow("Vignette Smoothed", vigSmoothed); + + // To see the vignette-calibrated image, we can use VignetteRemover + Mat oriImg = imread(imageFolderPath + "/00480.jpg", IMREAD_UNCHANGED); + photometric_calib::GammaRemover gammaRemover(gammaPath, oriImg.cols, oriImg.rows); + photometric_calib::VignetteRemover vignetteRemover("./vignetteCalibResult/vignetteSmoothed.png", gammaPath, oriImg.cols, oriImg.rows); + Mat resCaliImg = gammaRemover.getUnGammaImageMat(oriImg); + Mat vigCaliImg = vignetteRemover.getUnVignetteImageMat(oriImg); + + // Visualization + namedWindow( "Original Image", WINDOW_AUTOSIZE ); + imshow("Original Image", oriImg); + namedWindow( "Gamma Removed Image", WINDOW_AUTOSIZE ); + imshow("Gamma Removed Image", resCaliImg); + namedWindow( "Vignette Removed Image", WINDOW_AUTOSIZE ); + imshow("Vignette Removed Image", vigCaliImg); + + waitKey(0); + + return 0; +} \ No newline at end of file diff --git a/modules/photometric_calib/src/GammaRemover.cpp b/modules/photometric_calib/src/GammaRemover.cpp new file mode 100644 index 00000000000..cd8daad6f62 --- /dev/null +++ b/modules/photometric_calib/src/GammaRemover.cpp @@ -0,0 +1,84 @@ +// 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. + +#include "precomp.hpp" +#include "opencv2/photometric_calib/GammaRemover.hpp" + +namespace cv { +namespace photometric_calib { + +GammaRemover::GammaRemover(const std::string &gammaPath, int w_, int h_) +{ + validGamma = false; + w = w_; + h = h_; + + // check the extension of the time file. + CV_Assert(gammaPath.substr(gammaPath.find_last_of(".") + 1) == "yaml" || + gammaPath.substr(gammaPath.find_last_of(".") + 1) == "yml"); + + FileStorage gammaFile; + gammaFile.open(gammaPath, FileStorage::READ); + CV_Assert(gammaFile.isOpened()); + + FileNode gammaNode = gammaFile["gamma"]; + CV_Assert(gammaNode.type() == FileNode::SEQ); + FileNodeIterator itS = gammaNode.begin(), itE = gammaNode.end(); + std::vector GInvVec; + for (; itS != itE; ++itS) + { + GInvVec.push_back((float) *itS); + } + CV_Assert(GInvVec.size() == 256); + + for (int i = 0; i < 256; i++) GInv[i] = GInvVec[i]; + for (int i = 0; i < 255; i++) + { + CV_Assert(GInv[i + 1] > GInv[i]); + } + float min = GInv[0]; + float max = GInv[255]; + for (int i = 0; i < 256; i++) GInv[i] = (float) (255.0 * (GInv[i] - min) / (max - min)); + for (int i = 1; i < 255; i++) + { + for (int s = 1; s < 255; s++) + { + if (GInv[s] <= i && GInv[s + 1] >= i) + { + G[i] = s + (i - GInv[s]) / (GInv[s + 1] - GInv[s]); + break; + } + } + } + G[0] = 0; + G[255] = 255; + gammaFile.release(); + validGamma = true; +} + +Mat GammaRemover::getUnGammaImageMat(Mat inputIm) +{ + CV_Assert(validGamma); + uchar *inputImArr = inputIm.data; + float *outImArr = new float[w * h]; + for (int i = 0; i < w * h; ++i) + { + outImArr[i] = GInv[inputImArr[i]]; + } + Mat _outIm(h, w, CV_32F, outImArr); + Mat outIm = _outIm * (1 / 255.0f); + delete[] outImArr; + return outIm; +} + +void GammaRemover::getUnGammaImageVec(Mat inputIm, std::vector &outImVec) +{ + CV_Assert(validGamma); + uchar *inputImArr = inputIm.data; + CV_Assert(outImVec.size() == (unsigned long) w * h); + for (int i = 0; i < w * h; i++) outImVec[i] = GInv[inputImArr[i]]; +} + +} // namespace photometric_calib +} // namespace cv diff --git a/modules/photometric_calib/src/Reader.cpp b/modules/photometric_calib/src/Reader.cpp new file mode 100644 index 00000000000..02e88524a98 --- /dev/null +++ b/modules/photometric_calib/src/Reader.cpp @@ -0,0 +1,129 @@ +// 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. + +#include "precomp.hpp" +#include "opencv2/photometric_calib/Reader.hpp" + +namespace cv { +namespace photometric_calib { + +unsigned long Reader::getNumImages() const +{ + return (unsigned long) images.size(); +} + +void Reader::loadTimestamps(const std::string ×File) +{ + // check the extension of the time file. + CV_Assert(timesFile.substr(timesFile.find_last_of(".") + 1) == "yaml" || + timesFile.substr(timesFile.find_last_of(".") + 1) == "yml"); + + FileStorage timeFile; + timeFile.open(timesFile, FileStorage::READ); + timeStamps.clear(); + exposureDurations.clear(); + + CV_Assert(timeFile.isOpened()); + + FileNode timeStampNode = timeFile["times"]; + FileNode exposureTimeNode = timeFile["exposures"]; + + CV_Assert(timeStampNode.type() == FileNode::SEQ && exposureTimeNode.type() == FileNode::SEQ); + + FileNodeIterator itTs = timeStampNode.begin(), itTsEnd = timeStampNode.end(); + FileNodeIterator itEt = exposureTimeNode.begin(), itEtEnd = exposureTimeNode.end(); + + for (; itTs != itTsEnd; ++itTs) + { + timeStamps.push_back((double) *itTs); + } + for (; itEt != itEtEnd; ++itEt) + { + exposureDurations.push_back((float) *itEt); + } + + timeFile.release(); + + CV_Assert(timeStamps.size() == getNumImages() && exposureDurations.size() == getNumImages()); + _timeFilePath = timesFile; +} + +Reader::Reader(const std::string &folderPath, const std::string &imageExt, const std::string ×Path) +{ + String cvFolderPath(folderPath); + +#if defined WIN32 || defined _WIN32 || defined WINCE + *cvFolderPath.end() == '\\' ? cvFolderPath = cvFolderPath : cvFolderPath += '\\'; +#else + *cvFolderPath.end() == '/' ? cvFolderPath = cvFolderPath : cvFolderPath += '/'; +#endif + + cvFolderPath += ("*." + imageExt); + glob(cvFolderPath, images); + CV_Assert(images.size() > 0); + std::sort(images.begin(), images.end()); + loadTimestamps(timesPath); + + _width = 0; + _height = 0; + + // images should be of CV_8U and same size + for (size_t i = 0; i < images.size(); ++i) + { + Mat img = imread(images[i], IMREAD_GRAYSCALE); + CV_Assert(img.type() == CV_8U); + if (i == 0) + { + _width = img.cols; + _height = img.rows; + } + else + { + CV_Assert(_width == img.cols && _height == img.rows); + } + } + + _folderPath = folderPath; +} + +Mat Reader::getImage(unsigned long id) const +{ + CV_Assert(id < images.size()); + return imread(images[id], IMREAD_GRAYSCALE); +} + +double Reader::getTimestamp(unsigned long id) const +{ + CV_Assert(id < timeStamps.size()); + return timeStamps[id]; +} + +float Reader::getExposureDuration(unsigned long id) const +{ + CV_Assert(id < exposureDurations.size()); + return exposureDurations[id]; +} + +int Reader::getWidth() const +{ + return _width; +} + +int Reader::getHeight() const +{ + return _height; +} + +const std::string &Reader::getFolderPath() const +{ + return _folderPath; +} + +const std::string &Reader::getTimeFilePath() const +{ + return _timeFilePath; +} + +} // namespace photometric_calib +} // namespace cv \ No newline at end of file diff --git a/modules/photometric_calib/src/ResponseCalib.cpp b/modules/photometric_calib/src/ResponseCalib.cpp new file mode 100644 index 00000000000..6b16bfca360 --- /dev/null +++ b/modules/photometric_calib/src/ResponseCalib.cpp @@ -0,0 +1,367 @@ +// 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. + +#include "precomp.hpp" +#include "opencv2/photometric_calib/ResponseCalib.hpp" + +#include +#include +#include + +namespace cv { +namespace photometric_calib { + +ResponseCalib::ResponseCalib(std::string folderPath, std::string timePath, std::string imageFormat) : _leakPadding(2), + _nIts(10), + _skipFrames(1) +{ + imageReader = new Reader(folderPath, imageFormat, timePath); +} + +ResponseCalib::ResponseCalib(std::string folderPath, std::string timePath, int leakPadding, int nIts, int skipFrames, + std::string imageFormat) : + _leakPadding(leakPadding), _nIts(nIts), _skipFrames(skipFrames) +{ + imageReader = new Reader(folderPath, imageFormat, timePath); +} + +Vec2d ResponseCalib::rmse(const double *G, const double *E, const std::vector &exposureVec, + const std::vector &dataVec, + int wh) +{ + long double e = 0; // yeah - these will be sums of a LOT of values, so we need super high precision. + long double num = 0; + + size_t n = dataVec.size(); + for (size_t i = 0; i < n; i++) + { + for (int k = 0; k < wh; k++) + { + if (dataVec[i][k] == 255) continue; + double r = G[dataVec[i][k]] - exposureVec[i] * E[k]; + if (!std::isfinite(r)) continue; + e += r * r * 1e-10; + num++; + } + } + + //return Eigen::Vector2d(1e5*sqrtl((e/num)), (double)num); + return Vec2d((double) (1e5 * sqrt((e / num))), (double) num); +} + +void ResponseCalib::plotE(const double *E, int w, int h, const std::string &saveTo) +{ + + // try to find some good color scaling for plotting. + double offset = 20; + double min = 1e10, max = -1e10; + + double Emin = 1e10, Emax = -1e10; + + for (int i = 0; i < w * h; i++) + { + double le = log(E[i] + offset); + if (le < min) min = le; + if (le > max) max = le; + + if (E[i] < Emin) Emin = E[i]; + if (E[i] > Emax) Emax = E[i]; + } + + cv::Mat EImg = cv::Mat(h, w, CV_8UC3); + cv::Mat EImg16 = cv::Mat(h, w, CV_16U); + + for (int i = 0; i < w * h; i++) + { + float val = (float) (3 * (exp((log(E[i] + offset) - min) / (max - min)) - 1) / 1.7183); + + int icP = (int) val; + float ifP = val - icP; + icP = icP % 3; + + Vec3b color; + if (icP == 0) color = cv::Vec3b(0, 0, (uchar) (255 * ifP)); + if (icP == 1) color = cv::Vec3b(0, (uchar) (255 * ifP), 255); + if (icP == 2) color = cv::Vec3b((uchar) (255 * ifP), 255, 255); + + EImg.at(i) = color; + EImg16.at(i) = (ushort) (255 * 255 * (E[i] - Emin) / (Emax - Emin)); + } + + std::cout << "Irradiance " << Emin << " - " << Emax << std::endl; + cv::imshow("lnE", EImg); + + if (!saveTo.empty()) + { + imwrite(saveTo + ".png", EImg); + std::cout << "Saved: " << saveTo + ".png" << std::endl; + imwrite(saveTo + "-16.png", EImg16); + std::cout << "Saved: " << saveTo + "-16.png" << std::endl; + } +} + +void ResponseCalib::plotG(const double *G, const std::string &saveTo) +{ + cv::Mat GImg = cv::Mat(256, 256, CV_32FC1); + GImg.setTo(0); + + double min = 1e10, max = -1e10; + + for (int i = 0; i < 256; i++) + { + if (G[i] < min) min = G[i]; + if (G[i] > max) max = G[i]; + } + + for (int i = 0; i < 256; i++) + { + double val = 256 * (G[i] - min) / (max - min); + for (int k = 0; k < 256; k++) + { + if (val < k) + { + GImg.at(255 - k, i) = (float) (k - val); + } + } + } + + std::cout << "Inv. Response " << min << " - " << max << std::endl; + cv::imshow("G", GImg); + if (!saveTo.empty()) cv::imwrite(saveTo, GImg * 255); + std::cout << "Saved: " << saveTo << std::endl; +} + +void ResponseCalib::calib(bool debug) +{ + int w = 0, h = 0; + size_t n = 0; + + std::vector exposureDurationVec; + std::vector dataVec; + + std::cout << "Preprocessing for response calibration... " << std::endl; + for (unsigned long i = 0; i < imageReader->getNumImages(); i += _skipFrames) + { + cv::Mat img = imageReader->getImage(i); + if (img.rows == 0 || img.cols == 0) continue; + CV_Assert(img.type() == CV_8U); + + if ((w != 0 && w != img.cols) || img.cols == 0) + { + std::cout << "Width mismatch!" << std::endl; + exit(1); + } + if ((h != 0 && h != img.rows) || img.rows == 0) + { + std::cout << "Height mismatch!" << std::endl; + exit(1); + } + w = img.cols; + h = img.rows; + + uchar *data = new uchar[w * h]; + memcpy(data, img.data, w * h); + dataVec.push_back(data); + exposureDurationVec.push_back((double) (imageReader->getExposureDuration(i))); + + unsigned char *data2 = new unsigned char[w * h]; + for (int j = 0; j < _leakPadding; ++j) + { + memcpy(data2, data, w * h); + for (int y = 1; y < h - 1; ++y) + { + for (int x = 1; x < w - 1; ++x) + { + if (data[x + y * w] == 255) + { + data2[x + 1 + w * (y + 1)] = 255; + data2[x + 1 + w * (y)] = 255; + data2[x + 1 + w * (y - 1)] = 255; + + data2[x + w * (y + 1)] = 255; + data2[x + w * (y)] = 255; + data2[x + w * (y - 1)] = 255; + + data2[x - 1 + w * (y + 1)] = 255; + data2[x - 1 + w * (y)] = 255; + data2[x - 1 + w * (y - 1)] = 255; + } + } + } + memcpy(data, data2, w * h); + } + delete[] data2; + } + n = dataVec.size(); + std::cout << "Loaded " << n << " images!" << std::endl; + std::cout << "Response calibration begin!" << std::endl; + + double *E = new double[w * h]; // scene irradiance + double *En = new double[w * h]; // scene irradiance + double *G = new double[256]; // inverse response function + + // set starting scene irradiance to mean of all images. + memset(E, 0, sizeof(double) * w * h); + memset(En, 0, sizeof(double) * w * h); + memset(G, 0, sizeof(double) * 256); + + for (size_t i = 0; i < n; i++) + { + for (int k = 0; k < w * h; k++) + { + //if(dataVec[i][k]==255) continue; + E[k] += dataVec[i][k]; + En[k]++; + } + } + for (int k = 0; k < w * h; k++) + { + E[k] = E[k] / En[k]; + } + + // TODO: System independent folder creating + // Only on Linux for now. + if (-1 == system("rm -rf photoCalibResult")) + { + std::cout << "could not delete old photoCalibResult folder!" << std::endl; + } + if (-1 == system("mkdir photoCalibResult")) + { + std::cout << "could not create photoCalibResult folder!" << std::endl; + } + + std::ofstream logFile; + logFile.open("photoCalibResult/log.txt", std::ios::trunc | std::ios::out); + logFile.precision(15); + + std::cout << "Initial RMSE = " << rmse(G, E, exposureDurationVec, dataVec, w * h)[0] << "!" << std::endl; + if (debug) + { + plotE(E, w, h, "photoCalibResult/E-0"); + cv::waitKey(100); + } + + bool optE = true; + bool optG = true; + + for (int it = 0; it < _nIts; it++) + { + std::cout << "Iteration " << it + 1 << "..." << std::endl; + if (optG) + { + // optimize log inverse response function. + double *GSum = new double[256]; + double *GNum = new double[256]; + memset(GSum, 0, 256 * sizeof(double)); + memset(GNum, 0, 256 * sizeof(double)); + for (size_t i = 0; i < n; i++) + { + for (int k = 0; k < w * h; k++) + { + int b = dataVec[i][k]; + if (b == 255) continue; + GNum[b]++; + GSum[b] += E[k] * exposureDurationVec[i]; + } + } + for (int i = 0; i < 256; i++) + { + G[i] = GSum[i] / GNum[i]; + if (!std::isfinite(G[i]) && i > 1) G[i] = G[i - 1] + (G[i - 1] - G[i - 2]); + } + delete[] GSum; + delete[] GNum; + printf("optG RMSE = %f! \t", rmse(G, E, exposureDurationVec, dataVec, w * h)[0]); + + if (debug) + { + char buf[1000]; + snprintf(buf, 1000, "photoCalibResult/G-%02d.png", it + 1); + plotG(G, buf); + } + } + + if (optE) + { + // optimize scene irradiance function. + double *ESum = new double[w * h]; + double *ENum = new double[w * h]; + memset(ESum, 0, w * h * sizeof(double)); + memset(ENum, 0, w * h * sizeof(double)); + for (size_t i = 0; i < n; i++) + { + for (int k = 0; k < w * h; k++) + { + int b = dataVec[i][k]; + if (b == 255) continue; + ENum[k] += exposureDurationVec[i] * exposureDurationVec[i]; + ESum[k] += (G[b]) * exposureDurationVec[i]; + } + } + for (int i = 0; i < w * h; i++) + { + E[i] = ESum[i] / ENum[i]; + if (E[i] < 0) E[i] = 0; + } + + delete[] ENum; + delete[] ESum; + printf("OptE RMSE = %f! \t", rmse(G, E, exposureDurationVec, dataVec, w * h)[0]); + + if (debug) + { + char buf[1000]; + snprintf(buf, 1000, "photoCalibResult/E-%02d", it + 1); + plotE(E, w, h, buf); + } + } + + // rescale such that maximum response is 255 (fairly arbitrary choice). + double rescaleFactor = 255.0 / G[255]; + for (int i = 0; i < w * h; i++) + { + E[i] *= rescaleFactor; + if (i < 256) G[i] *= rescaleFactor; + } + + Vec2d err = rmse(G, E, exposureDurationVec, dataVec, w * h); + printf("Rescaled RMSE = %f! \trescale with %f!\n\n", err[0], rescaleFactor); + + logFile << it << " " << n << " " << err[1] << " " << err[0] << "\n"; + + cv::waitKey(100); + } + + logFile.flush(); + logFile.close(); + + std::ofstream lg; + lg.open("photoCalibResult/pcalib.yaml", std::ios::trunc | std::ios::out); + lg << "%YAML:1.0\ngamma: ["; + lg.precision(15); + for (int i = 0; i < 255; i++) + { + lg << G[i] << ", "; + } + lg << G[255] << ']'; + lg << "\n"; + + lg.flush(); + lg.close(); + + std::cout << "pcalib file has been saved to: photoCalibResult/pcalib.yaml" << std::endl; + + delete[] E; + delete[] En; + delete[] G; + for (size_t i = 0; i < n; i++) + { + delete[] dataVec[i]; + } + + std::cout << "Camera response function calibration finished!" << std::endl; +} + +} // namespace photometric_calib +} // namespace cv \ No newline at end of file diff --git a/modules/photometric_calib/src/VignetteCalib.cpp b/modules/photometric_calib/src/VignetteCalib.cpp new file mode 100644 index 00000000000..75cd1f96d7f --- /dev/null +++ b/modules/photometric_calib/src/VignetteCalib.cpp @@ -0,0 +1,1097 @@ +// 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. + +#include "precomp.hpp" +#include "opencv2/photometric_calib/VignetteCalib.hpp" + +#include +#include + +namespace cv { +namespace photometric_calib { + + +VignetteCalib::VignetteCalib(std::string folderPath, std::string timePath, std::string cameraFile, + std::string gammaFile, std::string imageFormat) : + _imageSkip(1), _maxIterations(20), _outlierTh(15), _gridWidth(1000), _gridHeight(1000), _facW(5), _facH(5), + _maxAbsGrad(255) +{ + imageReader = new Reader(folderPath, imageFormat, timePath); + // check the extension of the camera file. + CV_Assert(cameraFile.substr(cameraFile.find_last_of(".") + 1) == "yaml" || + cameraFile.substr(cameraFile.find_last_of(".") + 1) == "yml"); + FileStorage cameraStorage(cameraFile, FileStorage::READ); + cameraStorage["cameraMatrix"] >> _cameraMatrix; + cameraStorage["distCoeffs"] >> _distCoeffs; + gammaRemover = new GammaRemover(gammaFile, imageReader->getWidth(), imageReader->getHeight()); + + // affine map from plane cordinates to grid coordinates. + _K_p2idx = Matx33f::eye(); + _K_p2idx(0, 0) = _gridWidth / _facW; + _K_p2idx(1, 1) = _gridHeight / _facH; + _K_p2idx(0, 2) = _gridWidth / 2.f; + _K_p2idx(1, 2) = _gridHeight / 2.f; + _K_p2idx_inverse = _K_p2idx.inv(); + + _meanExposure = calMeanExposureTime(); +} + +VignetteCalib::VignetteCalib(std::string folderPath, std::string timePath, std::string cameraFile, + std::string gammaFile, std::string imageFormat, int imageSkip, + int maxIterations, + int outlierTh, int gridWidth, int gridHeight, float facW, float facH, int maxAbsGrad) : + _imageSkip(imageSkip), _maxIterations(maxIterations), _outlierTh(outlierTh), _gridWidth(gridWidth), + _gridHeight(gridHeight), + _facW(facW), _facH(facH), _maxAbsGrad(maxAbsGrad) +{ + imageReader = new Reader(folderPath, imageFormat, timePath); + // check the extension of the camera file. + CV_Assert(cameraFile.substr(cameraFile.find_last_of(".") + 1) == "yaml" || + cameraFile.substr(cameraFile.find_last_of(".") + 1) == "yml"); + FileStorage cameraStorage(cameraFile, FileStorage::READ); + cameraStorage["cameraMatrix"] >> _cameraMatrix; + cameraStorage["distCoeffs"] >> _distCoeffs; + gammaRemover = new GammaRemover(gammaFile, imageReader->getWidth(), imageReader->getHeight()); + + // affine map from plane cordinates to grid coordinates. + _K_p2idx = Matx33f::eye(); + _K_p2idx(0, 0) = _gridWidth / _facW; + _K_p2idx(1, 1) = _gridHeight / _facH; + _K_p2idx(0, 2) = _gridWidth / 2.f; + _K_p2idx(1, 2) = _gridHeight / 2.f; + _K_p2idx_inverse = _K_p2idx.inv(); + + _meanExposure = calMeanExposureTime(); +} + +float VignetteCalib::getInterpolatedElement(const float *const mat, const float x, const float y, const int width) +{ + int ix = (int) x; + int iy = (int) y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx * dy; + const float *bp = mat + ix + iy * width; + + float res = dxdy * bp[1 + width] + + (dy - dxdy) * bp[width] + + (dx - dxdy) * bp[1] + + (1 - dx - dy + dxdy) * bp[0]; + + return res; +} + +void VignetteCalib::displayImage(float *I, int w, int h, std::string name) +{ + float vmin = 1e10; + float vmax = -1e10; + + for (int i = 0; i < w * h; i++) + { + if (vmin > I[i]) vmin = I[i]; + if (vmax < I[i]) vmax = I[i]; + } + + Mat img = Mat(h, w, CV_8UC3); + + for (int i = 0; i < w * h; i++) + { + //isnanf? isnan? + if (cvIsNaN(I[i]) == 1) + { img.at(i) = Vec3b(0, 0, 255); } + else + { + img.at(i) = Vec3b((uchar) (255 * (I[i] - vmin) / (vmax - vmin)), + (uchar) (255 * (I[i] - vmin) / (vmax - vmin)), + (uchar) (255 * (I[i] - vmin) / (vmax - vmin))); + } + } + + std::cout << "plane image values " << vmin << " - " << vmax << "!" << std::endl; + + imshow(name, img); + imwrite("vignetteCalibResult/plane.png", img); +} + +void VignetteCalib::displayImageV(float *I, int w, int h, std::string name) +{ + Mat img = Mat(h, w, CV_8UC3); + for (int i = 0; i < w * h; i++) + { + if (cvIsNaN(I[i]) == 1) + { + img.at(i) = Vec3b(0, 0, 255); + } + else + { + float c = 254 * I[i]; + img.at(i) = Vec3b((uchar) c, (uchar) c, (uchar) c); + } + + } + imshow(name, img); +} + +float VignetteCalib::calMeanExposureTime() +{ + float meanExposure = 0.f; + for (unsigned long i = 0; i < imageReader->getNumImages(); i += _imageSkip) + { + meanExposure += imageReader->getExposureDuration(i); + } + meanExposure = meanExposure / imageReader->getNumImages(); + if (meanExposure == 0) meanExposure = 1; + return meanExposure; +} + +bool VignetteCalib::preCalib(unsigned long id, float *&image, float *&plane2imgX, float *&plane2imgY, bool debug) +{ + int wI = imageReader->getWidth(); + int hI = imageReader->getHeight(); + + std::vector markerIds; + std::vector > markerCorners, rejectedCandidates; + + Mat oriImg = imageReader->getImage(id); + Mat undisImg; + undistort(oriImg, undisImg, _cameraMatrix, _distCoeffs); + + Ptr parameters = aruco::DetectorParameters::create(); + Ptr dictionary = aruco::getPredefinedDictionary(aruco::DICT_ARUCO_ORIGINAL); + + aruco::detectMarkers(undisImg, dictionary, markerCorners, markerIds, parameters, rejectedCandidates); + if (markerCorners.size() != 1) + { + return false; + } + + std::vector ptsP; + std::vector ptsI; + ptsI.push_back(Point2f(markerCorners[0][0].x, markerCorners[0][0].y)); + ptsI.push_back(Point2f(markerCorners[0][1].x, markerCorners[0][1].y)); + ptsI.push_back(Point2f(markerCorners[0][2].x, markerCorners[0][2].y)); + ptsI.push_back(Point2f(markerCorners[0][3].x, markerCorners[0][3].y)); + ptsP.push_back(Point2f(-0.5, 0.5)); + ptsP.push_back(Point2f(0.5, 0.5)); + ptsP.push_back(Point2f(0.5, -0.5)); + ptsP.push_back(Point2f(-0.5, -0.5)); + + Matx33f H(findHomography(ptsP, ptsI)); + + std::vector imgRaw(wI * hI); + gammaRemover->getUnGammaImageVec(imageReader->getImage(id), imgRaw); + + plane2imgX = new float[_gridWidth * _gridHeight]; + plane2imgY = new float[_gridWidth * _gridHeight]; + + Matx33f HK = H * _K_p2idx_inverse; + + int idx = 0; + for (int y = 0; y < _gridHeight; y++) + { + for (int x = 0; x < _gridWidth; x++) + { + Vec3f pp = HK * Vec3f((float) x, (float) y, 1); + plane2imgX[idx] = pp[0] / pp[2]; + plane2imgY[idx] = pp[1] / pp[2]; + idx++; + } + } + + float expDuration; + expDuration = (imageReader->getExposureDuration(id) == 0 ? 1 : imageReader->getExposureDuration(id)); + image = new float[wI * hI]; + for (int y = 0; y < hI; y++) + { + for (int x = 0; x < wI; x++) + { + image[x + y * wI] = _meanExposure * imgRaw[x + y * wI] / expDuration; + } + } + + for (int y = 2; y < hI - 2; y++) + { + for (int x = 2; x < wI - 2; x++) + { + for (int deltax = -2; deltax < 3; deltax++) + { + for (int deltay = -2; deltay < 3; deltay++) + { + if (fabsf(image[x + y * wI] - image[x + deltax + (y + deltay) * wI]) > _maxAbsGrad) + { + image[x + y * wI] = NAN; + image[x + deltax + (y + deltay) * wI] = NAN; + } + } + } + } + } + + if (debug) + { + // debug-plot. + Mat dbgImg(hI, wI, CV_8UC3); + for (int j = 0; j < wI * hI; j++) + { + dbgImg.at(j) = Vec3b((uchar) imgRaw[j], (uchar) imgRaw[j], (uchar) imgRaw[j]); + } + + for (int x = 0; x <= _gridWidth; x += 200) + { + for (int y = 0; y <= _gridHeight; y += 10) + { + int idxS = (x < _gridWidth ? x : _gridWidth - 1) + (y < _gridHeight ? y : _gridHeight - 1) * _gridWidth; + int idxT = (x < _gridWidth ? x : _gridWidth - 1) + + ((y + 10) < _gridHeight ? (y + 10) : _gridHeight - 1) * _gridWidth; + + int u_dS = (int) lround((plane2imgX[idxS] + 0.5)); + int v_dS = (int) lround((plane2imgY[idxS] + 0.5)); + + int u_dT = (int) lround((plane2imgX[idxT] + 0.5)); + int v_dT = (int) lround((plane2imgY[idxT] + 0.5)); + + if (u_dS >= 0 && v_dS >= 0 && u_dS < wI && v_dS < hI && u_dT >= 0 && v_dT >= 0 && u_dT < wI && + v_dT < hI) + { + line(dbgImg, Point(u_dS, v_dS), Point(u_dT, v_dT), Scalar(0, 0, 255), 10, LINE_AA); + } + } + } + + for (int x = 0; x <= _gridWidth; x += 10) + { + for (int y = 0; y <= _gridHeight; y += 200) + { + int idxS = (x < _gridWidth ? x : _gridWidth - 1) + (y < _gridHeight ? y : _gridHeight - 1) * _gridWidth; + int idxT = ((x + 10) < _gridWidth ? (x + 10) : _gridWidth - 1) + + (y < _gridHeight ? y : _gridHeight - 1) * _gridWidth; + + int u_dS = (int) lround(plane2imgX[idxS] + 0.5); + int v_dS = (int) lround(plane2imgY[idxS] + 0.5); + + int u_dT = (int) lround(plane2imgX[idxT] + 0.5); + int v_dT = (int) lround(plane2imgY[idxT] + 0.5); + + if (u_dS >= 0 && v_dS >= 0 && u_dS < wI && v_dS < hI && u_dT >= 0 && v_dT >= 0 && u_dT < wI && + v_dT < hI) + { + line(dbgImg, Point(u_dS, v_dS), Point(u_dT, v_dT), Scalar(0, 0, 255), 10, LINE_AA); + } + } + } + + for (int x = 0; x < _gridWidth; x++) + { + for (int y = 0; y < _gridHeight; y++) + { + int u_d = (int) lround(plane2imgX[x + y * _gridWidth] + 0.5); + int v_d = (int) lround(plane2imgY[x + y * _gridWidth] + 0.5); + if (!(u_d > 1 && v_d > 1 && u_d < wI - 2 && v_d < hI - 2)) + { + plane2imgX[x + y * _gridWidth] = NAN; + plane2imgY[x + y * _gridWidth] = NAN; + } + } + } + + imshow("inRaw", dbgImg); + waitKey(1); + + if (rand() % 40 == 0) + { + char buf[1000]; + snprintf(buf, 1000, "vignetteCalibResult/img%u.png", (unsigned) id); + imwrite(buf, dbgImg); + } + } + + else + { + for (int x = 0; x < _gridWidth; x++) + { + for (int y = 0; y < _gridHeight; y++) + { + int u_d = (int) lround(plane2imgX[x + y * _gridWidth] + 0.5); + int v_d = (int) lround(plane2imgY[x + y * _gridWidth] + 0.5); + if (!(u_d > 1 && v_d > 1 && u_d < wI - 2 && v_d < hI - 2)) + { + plane2imgX[x + y * _gridWidth] = NAN; + plane2imgY[x + y * _gridWidth] = NAN; + } + } + } + } + + return true; +} + +void VignetteCalib::calib(bool debug) +{ + // Create folder for vignette calibration + if (-1 == system("rm -rf vignetteCalibResult")) + { + std::cout << "could not delete old vignetteCalibResult folder!" << std::endl; + } + if (-1 == system("mkdir vignetteCalibResult")) + { + std::cout << "could not delete old vignetteCalibResult folder" << std::endl; + } + + int wO, hO; + wO = imageReader->getWidth(); + hO = imageReader->getHeight(); + int wI = wO, hI = hO; + + // log file + std::ofstream logFile; + logFile.open("vignetteCalibResult/log.txt", std::ios::trunc | std::ios::out); + logFile.precision(15); + + // number of images in total + unsigned long n = imageReader->getNumImages(); + + float *planeColor = new float[_gridWidth * _gridHeight]; + float *planeColorFF = new float[_gridWidth * _gridHeight]; + float *planeColorFC = new float[_gridWidth * _gridHeight]; + float *vignetteFactor = new float[hI * wI]; + float *vignetteFactorTT = new float[hI * wI]; + float *vignetteFactorCT = new float[hI * wI]; + + // initialize vignette factors to 1. + for (int i = 0; i < hI * wI; i++) vignetteFactor[i] = 1; + + double E = 0; + double R = 0; + + // optimization begins + for (int it = 0; it < _maxIterations; it++) + { + std::cout << "Iteration " << it << "..." << std::endl; + + int oth2 = _outlierTh * _outlierTh; + if (it < _maxIterations / 2) oth2 = 10000 * 10000; + + // ============================ optimize planeColor ================================ + std::cout << "Optimize planeColor..." << std::endl; + memset(planeColorFF, 0, _gridWidth * _gridHeight * sizeof(float)); + memset(planeColorFC, 0, _gridWidth * _gridHeight * sizeof(float)); + E = 0; + R = 0; + + // for each plane pixel, it's optimum is at sum(CF)/sum(FF) + for (unsigned long img = 0; img < n; img++) // for all images + { + float *plane2imgX = NULL; + float *plane2imgY = NULL; + float *image = NULL; + if (!preCalib(img, image, plane2imgX, plane2imgY, debug)) + { + continue; + } + for (int pi = 0; pi < _gridWidth * _gridHeight; pi++) // for all plane points + { + if (cvIsNaN(plane2imgX[pi]) == 1) continue; + // get vignetted color at that point, and add to build average. + float color = getInterpolatedElement(image, plane2imgX[pi], plane2imgY[pi], wI); + float fac = getInterpolatedElement(vignetteFactor, plane2imgX[pi], plane2imgY[pi], wI); + if (cvIsNaN(fac) == 1) continue; + if (cvIsNaN(color) == 1) continue; + double residual = (double) ((color - planeColor[pi] * fac) * (color - planeColor[pi] * fac)); + if (abs(residual) > oth2) + { + E += oth2; + R++; + continue; + } + planeColorFF[pi] += fac * fac; + planeColorFC[pi] += color * fac; + if (cvIsNaN(planeColor[pi]) == 1) continue; + E += residual; + R++; + } + delete[] plane2imgX; + delete[] plane2imgY; + delete[] image; + } + + for (int pi = 0; pi < _gridWidth * _gridWidth; pi++) // for all plane points + { + if (planeColorFF[pi] < 1) + { + planeColor[pi] = NAN; + } + else + { + planeColor[pi] = planeColorFC[pi] / planeColorFF[pi]; + } + } + if (debug) + { + displayImage(planeColor, _gridWidth, _gridWidth, "Plane"); + } + std::cout << R << " residual terms => " << sqrt(E / R) << std::endl; + + // ================================ optimize vignette ======================================= + std::cout << "Optimize Vignette..." << std::endl; + memset(vignetteFactorTT, 0, hI * wI * sizeof(float)); + memset(vignetteFactorCT, 0, hI * wI * sizeof(float)); + E = 0; + R = 0; + for (unsigned long img = 0; img < n; img++) // for all images + { + float *plane2imgX = NULL; + float *plane2imgY = NULL; + float *image = NULL; + if (!preCalib(img, image, plane2imgX, plane2imgY, debug)) + { + continue; + } + for (int pi = 0; pi < _gridWidth * _gridWidth; pi++) // for all plane points + { + if (cvIsNaN(plane2imgX[pi]) == 1) continue; + float x = plane2imgX[pi]; + float y = plane2imgY[pi]; + float colorImage = getInterpolatedElement(image, x, y, wI); + float fac = getInterpolatedElement(vignetteFactor, x, y, wI); + float colorPlane = planeColor[pi]; + if (cvIsNaN(colorPlane) == 1) continue; + if (cvIsNaN(colorImage) == 1) continue; + double residual = (double) ((colorImage - colorPlane * fac) * (colorImage - colorPlane * fac)); + if (abs(residual) > oth2) + { + E += oth2; + R++; + continue; + } + int ix = (int) x; + int iy = (int) y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx * dy; + vignetteFactorTT[ix + iy * wI + 0] += (1 - dx - dy + dxdy) * colorPlane * colorPlane; + vignetteFactorTT[ix + iy * wI + 1] += (dx - dxdy) * colorPlane * colorPlane; + vignetteFactorTT[ix + iy * wI + wI] += (dy - dxdy) * colorPlane * colorPlane; + vignetteFactorTT[ix + iy * wI + 1 + wI] += dxdy * colorPlane * colorPlane; + vignetteFactorCT[ix + iy * wI + 0] += (1 - dx - dy + dxdy) * colorImage * colorPlane; + vignetteFactorCT[ix + iy * wI + 1] += (dx - dxdy) * colorImage * colorPlane; + vignetteFactorCT[ix + iy * wI + wI] += (dy - dxdy) * colorImage * colorPlane; + vignetteFactorCT[ix + iy * wI + 1 + wI] += dxdy * colorImage * colorPlane; + if (cvIsNaN(fac) == 1) continue; + E += residual; + R++; + } + delete[] plane2imgX; + delete[] plane2imgY; + delete[] image; + } + float maxFac = 0; + for (int pi = 0; pi < hI * wI; pi++) // for all plane points + { + if (vignetteFactorTT[pi] < 1) + { + vignetteFactor[pi] = NAN; + } + else + { + vignetteFactor[pi] = vignetteFactorCT[pi] / vignetteFactorTT[pi]; + if (vignetteFactor[pi] > maxFac) maxFac = vignetteFactor[pi]; + } + } + std::cout << R << " residual terms => " << sqrt(E / R) << std::endl; + + // normalize to vignette max. factor 1. + for (int pi = 0; pi < hI * wI; pi++) + { + vignetteFactor[pi] /= maxFac; + } + logFile << it << " " << n << " " << R << " " << sqrt(E / R) << "\n"; + + // dilate & smoothe vignette by 4 pixel for output. + // does not change anything in the optimization; uses vignetteFactorTT and vignetteFactorCT for temporary storing + memcpy(vignetteFactorTT, vignetteFactor, sizeof(float) * hI * wI); + for (int dilit = 0; dilit < 4; dilit++) + { + memcpy(vignetteFactorCT, vignetteFactorTT, sizeof(float) * hI * wI); + for (int y = 0; y < hI; y++) + { + for (int x = 0; x < wI; x++) + { + int idx = x + y * wI; + { + float sum = 0, num = 0; + if (x < wI - 1 && y < hI - 1 && cvIsNaN(vignetteFactorCT[idx + 1 + wI]) != 1) + { + sum += vignetteFactorCT[idx + 1 + wI]; + num++; + } + if (x < wI - 1 && cvIsNaN(vignetteFactorCT[idx + 1]) != 1) + { + sum += vignetteFactorCT[idx + 1]; + num++; + } + if (x < wI - 1 && y > 0 && cvIsNaN(vignetteFactorCT[idx + 1 - wI]) != 1) + { + sum += vignetteFactorCT[idx + 1 - wI]; + num++; + } + + if (y < hI - 1 && cvIsNaN(vignetteFactorCT[idx + wI]) != 1) + { + sum += vignetteFactorCT[idx + wI]; + num++; + } + if (cvIsNaN(vignetteFactorCT[idx]) != 1) + { + sum += vignetteFactorCT[idx]; + num++; + } + if (y > 0 && cvIsNaN(vignetteFactorCT[idx - wI]) != 1) + { + sum += vignetteFactorCT[idx - wI]; + num++; + } + + if (y < hI - 1 && x > 0 && cvIsNaN(vignetteFactorCT[idx - 1 + wI]) != 1) + { + sum += vignetteFactorCT[idx - 1 + wI]; + num++; + } + if (x > 0 && cvIsNaN(vignetteFactorCT[idx - 1]) != 1) + { + sum += vignetteFactorCT[idx - 1]; + num++; + } + if (y > 0 && x > 0 && cvIsNaN(vignetteFactorCT[idx - 1 - wI]) != 1) + { + sum += vignetteFactorCT[idx - 1 - wI]; + num++; + } + + if (num > 0) vignetteFactorTT[idx] = sum / num; + } + } + } + } + + // ================================ Store Vignette Image ======================================= + if (debug) + { + displayImageV(vignetteFactorTT, wI, hI, "VignetteSmoothed"); + } + Mat wrapSmoothed = Mat(hI, wI, CV_32F, vignetteFactorTT) * 254.9 * 254.9; + Mat wrapSmoothed16; + wrapSmoothed.convertTo(wrapSmoothed16, CV_16U, 1, 0); + imwrite("vignetteCalibResult/vignetteSmoothed.png", wrapSmoothed16); + waitKey(50); + + if (debug) + { + displayImageV(vignetteFactor, wI, hI, "VignetteOrg"); + } + Mat wrap = Mat(hI, wI, CV_32F, vignetteFactor) * 254.9 * 254.9; + Mat wrap16; + wrap.convertTo(wrap16, CV_16U, 1, 0); + imwrite("vignetteCalibResult/vignette.png", wrap16); + waitKey(50); + } + + logFile.flush(); + logFile.close(); + + delete[] planeColor; + delete[] planeColorFF; + delete[] planeColorFC; + delete[] vignetteFactor; + delete[] vignetteFactorTT; + delete[] vignetteFactorCT; +} + +void VignetteCalib::calibFast(bool debug) +{ + std::cout<<"Fast mode! This requires large memory (10GB+)!"<getWidth(); + hO = imageReader->getHeight(); + int wI = wO, hI = hO; + + Ptr parameters = aruco::DetectorParameters::create(); + Ptr dictionary = aruco::getPredefinedDictionary(aruco::DICT_ARUCO_ORIGINAL); + + std::vector images; + std::vector p2imgX; + std::vector p2imgY; + + std::cout<<"Preprocessing images..."<getNumImages(); ++i) + { + std::vector markerIds; + std::vector > markerCorners, rejectedCandidates; + + Mat oriImg = imageReader->getImage(i); + Mat undisImg; + undistort(oriImg, undisImg, _cameraMatrix, _distCoeffs); + + aruco::detectMarkers(undisImg, dictionary, markerCorners, markerIds, parameters, rejectedCandidates); + + if (markerCorners.size() != 1) continue; + + std::vector ptsP; + std::vector ptsI; + ptsI.push_back(Point2f(markerCorners[0][0].x, markerCorners[0][0].y)); + ptsI.push_back(Point2f(markerCorners[0][1].x, markerCorners[0][1].y)); + ptsI.push_back(Point2f(markerCorners[0][2].x, markerCorners[0][2].y)); + ptsI.push_back(Point2f(markerCorners[0][3].x, markerCorners[0][3].y)); + ptsP.push_back(Point2f(-0.5, 0.5)); + ptsP.push_back(Point2f(0.5, 0.5)); + ptsP.push_back(Point2f(0.5, -0.5)); + ptsP.push_back(Point2f(-0.5, -0.5)); + + Matx33f H(findHomography(ptsP, ptsI)); + + std::vector imgRaw(wI * hI); + gammaRemover->getUnGammaImageVec(imageReader->getImage(i), imgRaw); + + float *plane2imgX = new float[_gridWidth * _gridHeight]; + float *plane2imgY = new float[_gridWidth * _gridHeight]; + + Matx33f HK = H * _K_p2idx_inverse; + + int idx = 0; + for (int y = 0; y < _gridHeight; y++) + { + for (int x = 0; x < _gridWidth; x++) + { + Vec3f pp = HK * Vec3f((float) x, (float) y, 1); + plane2imgX[idx] = pp[0] / pp[2]; + plane2imgY[idx] = pp[1] / pp[2]; + idx++; + } + } + + float expDuration; + expDuration = (imageReader->getExposureDuration(i) == 0 ? 1 : imageReader->getExposureDuration(i)); + float *image = new float[wI * hI]; + for (int y = 0; y < hI; y++) + { + for (int x = 0; x < wI; x++) + { + image[x + y * wI] = _meanExposure * imgRaw[x + y * wI] / expDuration; + } + } + + for (int y = 2; y < hI - 2; y++) + { + for (int x = 2; x < wI - 2; x++) + { + for (int deltax = -2; deltax < 3; deltax++) + { + for (int deltay = -2; deltay < 3; deltay++) + { + if (fabsf(image[x + y * wI] - image[x + deltax + (y + deltay) * wI]) > _maxAbsGrad) + { + image[x + y * wI] = NAN; + image[x + deltax + (y + deltay) * wI] = NAN; + } + } + } + } + } + + images.push_back(image); + + if (debug) + { + // debug-plot. + Mat dbgImg(hI, wI, CV_8UC3); + for (int j = 0; j < wI * hI; j++) + { + dbgImg.at(j) = Vec3b((uchar) imgRaw[j], (uchar) imgRaw[j], (uchar) imgRaw[j]); + } + + for (int x = 0; x <= _gridWidth; x += 200) + { + for (int y = 0; y <= _gridHeight; y += 10) + { + int idxS = (x < _gridWidth ? x : _gridWidth - 1) + + (y < _gridHeight ? y : _gridHeight - 1) * _gridWidth; + int idxT = (x < _gridWidth ? x : _gridWidth - 1) + + ((y + 10) < _gridHeight ? (y + 10) : _gridHeight - 1) * _gridWidth; + + int u_dS = (int) lround((plane2imgX[idxS] + 0.5)); + int v_dS = (int) lround((plane2imgY[idxS] + 0.5)); + + int u_dT = (int) lround((plane2imgX[idxT] + 0.5)); + int v_dT = (int) lround((plane2imgY[idxT] + 0.5)); + + if (u_dS >= 0 && v_dS >= 0 && u_dS < wI && v_dS < hI && u_dT >= 0 && v_dT >= 0 && u_dT < wI && + v_dT < hI) + { + line(dbgImg, Point(u_dS, v_dS), Point(u_dT, v_dT), Scalar(0, 0, 255), 10, LINE_AA); + } + } + } + + for (int x = 0; x <= _gridWidth; x += 10) + { + for (int y = 0; y <= _gridHeight; y += 200) + { + int idxS = (x < _gridWidth ? x : _gridWidth - 1) + + (y < _gridHeight ? y : _gridHeight - 1) * _gridWidth; + int idxT = ((x + 10) < _gridWidth ? (x + 10) : _gridWidth - 1) + + (y < _gridHeight ? y : _gridHeight - 1) * _gridWidth; + + int u_dS = (int) lround(plane2imgX[idxS] + 0.5); + int v_dS = (int) lround(plane2imgY[idxS] + 0.5); + + int u_dT = (int) lround(plane2imgX[idxT] + 0.5); + int v_dT = (int) lround(plane2imgY[idxT] + 0.5); + + if (u_dS >= 0 && v_dS >= 0 && u_dS < wI && v_dS < hI && u_dT >= 0 && v_dT >= 0 && u_dT < wI && + v_dT < hI) + { + line(dbgImg, Point(u_dS, v_dS), Point(u_dT, v_dT), Scalar(0, 0, 255), 10, LINE_AA); + } + } + } + + for (int x = 0; x < _gridWidth; x++) + { + for (int y = 0; y < _gridHeight; y++) + { + int u_d = (int) lround(plane2imgX[x + y * _gridWidth] + 0.5); + int v_d = (int) lround(plane2imgY[x + y * _gridWidth] + 0.5); + + if (!(u_d > 1 && v_d > 1 && u_d < wI - 2 && v_d < hI - 2)) + { + plane2imgX[x + y * _gridWidth] = NAN; + plane2imgY[x + y * _gridWidth] = NAN; + } + } + } + + imshow("inRaw", dbgImg); + + if (rand() % 40 == 0) + { + char buf[1000]; + snprintf(buf, 1000, "vignetteCalibResult/img%u.png", (unsigned) i); + imwrite(buf, dbgImg); + } + + waitKey(1); + } + + else + { + for (int x = 0; x < _gridWidth; x++) + { + for (int y = 0; y < _gridHeight; y++) + { + int u_d = (int) lround(plane2imgX[x + y * _gridWidth] + 0.5); + int v_d = (int) lround(plane2imgY[x + y * _gridWidth] + 0.5); + + if (!(u_d > 1 && v_d > 1 && u_d < wI - 2 && v_d < hI - 2)) + { + plane2imgX[x + y * _gridWidth] = NAN; + plane2imgY[x + y * _gridWidth] = NAN; + } + } + } + } + + p2imgX.push_back(plane2imgX); + p2imgY.push_back(plane2imgY); + + } + + std::ofstream logFile; + logFile.open("vignetteCalibResult/log.txt", std::ios::trunc | std::ios::out); + logFile.precision(15); + + unsigned long n = imageReader->getNumImages(); + + float *planeColor = new float[_gridWidth * _gridHeight]; + float *planeColorFF = new float[_gridWidth * _gridHeight]; + float *planeColorFC = new float[_gridWidth * _gridHeight]; + float *vignetteFactor = new float[hI * wI]; + float *vignetteFactorTT = new float[hI * wI]; + float *vignetteFactorCT = new float[hI * wI]; + + // initialize vignette factors to 1. + for (int i = 0; i < hI * wI; i++) vignetteFactor[i] = 1; + + double E = 0; + double R = 0; + + for (int it = 0; it < _maxIterations; it++) + { + int oth2 = _outlierTh * _outlierTh; + if (it < _maxIterations / 2) oth2 = 10000 * 10000; + + // ============================ optimize planeColor ================================ + memset(planeColorFF, 0, _gridWidth * _gridHeight * sizeof(float)); + memset(planeColorFC, 0, _gridWidth * _gridHeight * sizeof(float)); + E = 0; + R = 0; + + // for each plane pixel, it's optimum is at sum(CF)/sum(FF) + for (unsigned long img = 0; img < n; img++) // for all images + { + float *plane2imgX = p2imgX[img]; + float *plane2imgY = p2imgY[img]; + float *image = images[img]; + + for (int pi = 0; pi < _gridWidth * _gridHeight; pi++) // for all plane points + { + if (cvIsNaN(plane2imgX[pi]) == 1) continue; + + // get vignetted color at that point, and add to build average. + float color = getInterpolatedElement(image, plane2imgX[pi], plane2imgY[pi], wI); + float fac = getInterpolatedElement(vignetteFactor, plane2imgX[pi], plane2imgY[pi], wI); + + if (cvIsNaN(fac) == 1) continue; + if (cvIsNaN(color) == 1) continue; + + double residual = (double) ((color - planeColor[pi] * fac) * (color - planeColor[pi] * fac)); + if (abs(residual) > oth2) + { + E += oth2; + R++; + continue; + } + + + planeColorFF[pi] += fac * fac; + planeColorFC[pi] += color * fac; + + if (cvIsNaN(planeColor[pi]) == 1) continue; + E += residual; + R++; + } + } + + for (int pi = 0; pi < _gridWidth * _gridWidth; pi++) // for all plane points + { + if (planeColorFF[pi] < 1) + { + planeColor[pi] = NAN; + } + else + { + planeColor[pi] = planeColorFC[pi] / planeColorFF[pi]; + } + } + if (debug) + { + displayImage(planeColor, _gridWidth, _gridWidth, "Plane"); + } + + std::cout << R << " residual terms => " << sqrt(E / R) << std::endl; + + // ================================ optimize vignette ======================================= + memset(vignetteFactorTT, 0, hI * wI * sizeof(float)); + memset(vignetteFactorCT, 0, hI * wI * sizeof(float)); + E = 0; + R = 0; + + for (unsigned long img = 0; img < n; img++) // for all images + { + float *plane2imgX = p2imgX[img]; + float *plane2imgY = p2imgY[img]; + float *image = images[img]; + + for (int pi = 0; pi < _gridWidth * _gridWidth; pi++) // for all plane points + { + if (cvIsNaN(plane2imgX[pi]) == 1) continue; + float x = plane2imgX[pi]; + float y = plane2imgY[pi]; + + float colorImage = getInterpolatedElement(image, x, y, wI); + float fac = getInterpolatedElement(vignetteFactor, x, y, wI); + float colorPlane = planeColor[pi]; + + if (cvIsNaN(colorPlane) == 1) continue; + if (cvIsNaN(colorImage) == 1) continue; + + double residual = (double) ((colorImage - colorPlane * fac) * (colorImage - colorPlane * fac)); + if (abs(residual) > oth2) + { + E += oth2; + R++; + continue; + } + + + int ix = (int) x; + int iy = (int) y; + float dx = x - ix; + float dy = y - iy; + float dxdy = dx * dy; + + vignetteFactorTT[ix + iy * wI + 0] += (1 - dx - dy + dxdy) * colorPlane * colorPlane; + vignetteFactorTT[ix + iy * wI + 1] += (dx - dxdy) * colorPlane * colorPlane; + vignetteFactorTT[ix + iy * wI + wI] += (dy - dxdy) * colorPlane * colorPlane; + vignetteFactorTT[ix + iy * wI + 1 + wI] += dxdy * colorPlane * colorPlane; + + vignetteFactorCT[ix + iy * wI + 0] += (1 - dx - dy + dxdy) * colorImage * colorPlane; + vignetteFactorCT[ix + iy * wI + 1] += (dx - dxdy) * colorImage * colorPlane; + vignetteFactorCT[ix + iy * wI + wI] += (dy - dxdy) * colorImage * colorPlane; + vignetteFactorCT[ix + iy * wI + 1 + wI] += dxdy * colorImage * colorPlane; + + if (cvIsNaN(fac) == 1) continue; + E += residual; + R++; + } + } + + float maxFac = 0; + for (int pi = 0; pi < hI * wI; pi++) // for all plane points + { + if (vignetteFactorTT[pi] < 1) + { + vignetteFactor[pi] = NAN; + } + else + { + vignetteFactor[pi] = vignetteFactorCT[pi] / vignetteFactorTT[pi]; + if (vignetteFactor[pi] > maxFac) maxFac = vignetteFactor[pi]; + } + } + + std::cout << R << " residual terms => " << sqrt(E / R) << std::endl; + + // normalize to vignette max. factor 1. + for (int pi = 0; pi < hI * wI; pi++) + { + vignetteFactor[pi] /= maxFac; + } + + logFile << it << " " << n << " " << R << " " << sqrt(E / R) << "\n"; + + // dilate & smoothe vignette by 4 pixel for output. + // does not change anything in the optimization; uses vignetteFactorTT and vignetteFactorCT for temporary storing + memcpy(vignetteFactorTT, vignetteFactor, sizeof(float) * hI * wI); + for (int dilit = 0; dilit < 4; dilit++) + { + memcpy(vignetteFactorCT, vignetteFactorTT, sizeof(float) * hI * wI); + for (int y = 0; y < hI; y++) + { + for (int x = 0; x < wI; x++) + { + int idx = x + y * wI; + { + float sum = 0, num = 0; + if (x < wI - 1 && y < hI - 1 && cvIsNaN(vignetteFactorCT[idx + 1 + wI]) != 1) + { + sum += vignetteFactorCT[idx + 1 + wI]; + num++; + } + if (x < wI - 1 && cvIsNaN(vignetteFactorCT[idx + 1]) != 1) + { + sum += vignetteFactorCT[idx + 1]; + num++; + } + if (x < wI - 1 && y > 0 && cvIsNaN(vignetteFactorCT[idx + 1 - wI]) != 1) + { + sum += vignetteFactorCT[idx + 1 - wI]; + num++; + } + + if (y < hI - 1 && cvIsNaN(vignetteFactorCT[idx + wI]) != 1) + { + sum += vignetteFactorCT[idx + wI]; + num++; + } + if (cvIsNaN(vignetteFactorCT[idx]) != 1) + { + sum += vignetteFactorCT[idx]; + num++; + } + if (y > 0 && cvIsNaN(vignetteFactorCT[idx - wI]) != 1) + { + sum += vignetteFactorCT[idx - wI]; + num++; + } + + if (y < hI - 1 && x > 0 && cvIsNaN(vignetteFactorCT[idx - 1 + wI]) != 1) + { + sum += vignetteFactorCT[idx - 1 + wI]; + num++; + } + if (x > 0 && cvIsNaN(vignetteFactorCT[idx - 1]) != 1) + { + sum += vignetteFactorCT[idx - 1]; + num++; + } + if (y > 0 && x > 0 && cvIsNaN(vignetteFactorCT[idx - 1 - wI]) != 1) + { + sum += vignetteFactorCT[idx - 1 - wI]; + num++; + } + + if (num > 0) vignetteFactorTT[idx] = sum / num; + } + } + } + } + + // ================================ Store Vignette Image ======================================= + if (debug) + { + displayImageV(vignetteFactorTT, wI, hI, "VignetteSmoothed"); + } + Mat wrapSmoothed = Mat(hI, wI, CV_32F, vignetteFactorTT) * 254.9 * 254.9; + Mat wrapSmoothed16; + wrapSmoothed.convertTo(wrapSmoothed16, CV_16U, 1, 0); + imwrite("vignetteCalibResult/vignetteSmoothed.png", wrapSmoothed16); + waitKey(50); + + if (debug) + { + displayImageV(vignetteFactor, wI, hI, "VignetteOrg"); + } + Mat wrap = Mat(hI, wI, CV_32F, vignetteFactor) * 254.9 * 254.9; + Mat wrap16; + wrap.convertTo(wrap16, CV_16U, 1, 0); + imwrite("vignetteCalibResult/vignette.png", wrap16); + waitKey(50); + } + + logFile.flush(); + logFile.close(); + + delete[] planeColor; + delete[] planeColorFF; + delete[] planeColorFC; + delete[] vignetteFactor; + delete[] vignetteFactorTT; + delete[] vignetteFactorCT; + + for (unsigned long i = 0; i < n; i++) + { + delete[] images[i]; + delete[] p2imgX[i]; + delete[] p2imgY[i]; + } +} + + +VignetteCalib::~VignetteCalib() +{ + delete imageReader; + delete gammaRemover; +} + +} // namespace photometric_calib +} // namespace cv \ No newline at end of file diff --git a/modules/photometric_calib/src/VignetteRemover.cpp b/modules/photometric_calib/src/VignetteRemover.cpp new file mode 100644 index 00000000000..cabee946b51 --- /dev/null +++ b/modules/photometric_calib/src/VignetteRemover.cpp @@ -0,0 +1,105 @@ +// 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. + +#include "precomp.hpp" +#include "opencv2/photometric_calib/VignetteRemover.hpp" +#include "opencv2/photometric_calib/GammaRemover.hpp" + +namespace cv { +namespace photometric_calib { + +VignetteRemover::VignetteRemover(const std::string &vignettePath, const std::string &pcalibPath, int w_, int h_) +{ + CV_Assert(vignettePath != ""); + CV_Assert(pcalibPath != ""); + + validVignette = false; + vignetteMap = 0; + vignetteMapInv = 0; + w = w_; + h = h_; + + + Mat vignetteMat = imread(vignettePath, IMREAD_UNCHANGED); + vignetteMap = new float[w * h]; + vignetteMapInv = new float[w * h]; + + CV_Assert(vignetteMat.rows == h && vignetteMat.cols == w); + + CV_Assert(vignetteMat.type() == CV_8U || vignetteMat.type() == CV_16U); + if (vignetteMat.type() == CV_8U) + { + float maxV = 0; + for (int i = 0; i < w * h; i++) + { + if (vignetteMat.at(i) > maxV) maxV = vignetteMat.at(i); + } + + for (int i = 0; i < w * h; i++) + { + vignetteMap[i] = vignetteMat.at(i) / maxV; + } + } + + else + { + float maxV = 0; + for (int i = 0; i < w * h; i++) + { + if (vignetteMat.at(i) > maxV) maxV = vignetteMat.at(i); + } + + for (int i = 0; i < w * h; i++) + { + vignetteMap[i] = vignetteMat.at(i) / maxV; + } + } + + for (int i = 0; i < w * h; i++) + { + vignetteMapInv[i] = 1.0f / vignetteMap[i]; + } + + _pcalibPath = pcalibPath; + validVignette = true; + +} + +Mat VignetteRemover::getUnVignetteImageMat(Mat oriImMat) +{ + std::vector _outImVec(w * h); + getUnVignetteImageVec(oriImMat, _outImVec); + + Mat _outIm(h, w, CV_32F, &_outImVec[0]); + Mat outIm = _outIm * (1 / 255.0f); + return outIm; +} + +void VignetteRemover::getUnVignetteImageVec(Mat oriImMat, std::vector &outImVec) +{ + CV_Assert(validVignette); + CV_Assert(outImVec.size() == (unsigned long) w * h); + photometric_calib::GammaRemover gammaRemover(_pcalibPath, w, h); + std::vector unGammaImVec(w * h); + gammaRemover.getUnGammaImageVec(oriImMat, unGammaImVec); + for (int i = 0; i < w * h; ++i) + { + outImVec[i] = unGammaImVec[i] * vignetteMapInv[i]; + } +} + +VignetteRemover::~VignetteRemover() +{ + if (vignetteMap != 0) + { + delete[] vignetteMap; + } + if (vignetteMapInv != 0) + { + delete[] vignetteMapInv; + } +} + +} // namespace photometric_calib +} // namespace cv diff --git a/modules/photometric_calib/src/photometric_calib.cpp b/modules/photometric_calib/src/photometric_calib.cpp new file mode 100644 index 00000000000..69f5564d88b --- /dev/null +++ b/modules/photometric_calib/src/photometric_calib.cpp @@ -0,0 +1,10 @@ +// 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. + +#include "precomp.hpp" + +namespace cv { +namespace photometric_calib { +} +} // namespace photometric_calib, cv \ No newline at end of file diff --git a/modules/photometric_calib/src/precomp.hpp b/modules/photometric_calib/src/precomp.hpp new file mode 100644 index 00000000000..7c8ae91d990 --- /dev/null +++ b/modules/photometric_calib/src/precomp.hpp @@ -0,0 +1,15 @@ +// 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. + +#ifndef __OPENCV_PRECOMP_H__ +#define __OPENCV_PRECOMP_H__ + +#include "opencv2/core.hpp" +#include "opencv2/highgui.hpp" +#include "opencv2/imgproc.hpp" +#include "opencv2/aruco.hpp" +#include "opencv2/calib3d.hpp" +#include + +#endif \ No newline at end of file diff --git a/modules/photometric_calib/test/test_main.cpp b/modules/photometric_calib/test/test_main.cpp new file mode 100644 index 00000000000..6f9ac2e0d43 --- /dev/null +++ b/modules/photometric_calib/test/test_main.cpp @@ -0,0 +1,3 @@ +#include "test_precomp.hpp" + +CV_TEST_MAIN("") diff --git a/modules/photometric_calib/test/test_precomp.hpp b/modules/photometric_calib/test/test_precomp.hpp new file mode 100644 index 00000000000..e832810b2c9 --- /dev/null +++ b/modules/photometric_calib/test/test_precomp.hpp @@ -0,0 +1,18 @@ +#ifdef __GNUC__ +# pragma GCC diagnostic ignored "-Wmissing-declarations" +# if defined __clang__ || defined __APPLE__ +# pragma GCC diagnostic ignored "-Wmissing-prototypes" +# pragma GCC diagnostic ignored "-Wextra" +# endif +#endif + +#ifndef __OPENCV_TEST_PRECOMP_HPP__ +#define __OPENCV_TEST_PRECOMP_HPP__ + +#include +#include "opencv2/ts.hpp" +#include "opencv2/imgproc.hpp" +#include "opencv2/photometric_calib.hpp" +#include "opencv2/highgui.hpp" + +#endif