diff --git a/bin/MonoCameraCalibration b/bin/MonoCameraCalibration new file mode 100755 index 0000000..3f17af7 Binary files /dev/null and b/bin/MonoCameraCalibration differ diff --git a/data/camera_calibration_params.xml b/data/camera_calibration_params.xml new file mode 100644 index 0000000..3dc9fc6 --- /dev/null +++ b/data/camera_calibration_params.xml @@ -0,0 +1,17 @@ + + + + 3 + 3 +
d
+ + 8.1538658490193018e+02 0. 4.0344686660476424e+02 0. + 8.1538658490193018e+02 2.5935115109955393e+02 0. 0. 1. + + 1 + 5 +
d
+ + -3.4454237775633917e-01 7.7306959051926261e-01 0. 0. + -3.5139312654968990e+00 + diff --git a/include/ReadWriteFunctions.h b/include/ReadWriteFunctions.h new file mode 100644 index 0000000..446cbfe --- /dev/null +++ b/include/ReadWriteFunctions.h @@ -0,0 +1,28 @@ +#ifndef _READWRITE_FUNCTIONS_ +#define _READWRITE_FUNCTIONS_ + + +#include + +bool readMonoCameraCalibParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs); + +bool readStereoCameraParameters(std::string filename, cv::Mat &camLMatrix, cv::Mat & distLCoeffs, cv::Mat &camRMatrix, cv::Mat & distRCoeffs, cv::Mat &stereoMapL_x, cv::Mat &stereoMapL_y, cv::Mat &stereoMapR_x, cv::Mat &stereoMapR_y, cv::Mat &Q); + +bool readDisparityParameters(std::string filename, int &numDisparities, int &blockSize , int &preFilterType, int &preFilterSize, int &preFilterCap, int &minDisparity , int &textureThreshold, int &uniquenessRatio, int &speckleRange, int &speckleWindowSize, int &disp12MaxDiff); + +bool readColorParameters(std::string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV); + +bool readCameraChessboardCalibrationParameters(std::string filename, std::vector& objPoints, std::vector& imagePoints, cv::Mat& cameraChessboardTransform); + +bool writeMonoCameraParameters(std::string filename, cv::Mat camMatrix, cv::Mat distCoeffs); + +bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV); + +bool writeStereoCameraParameters(std::string filename, cv::Mat camLMatrix, cv::Mat distLCoeffs, cv::Mat camRMatrix, cv::Mat distRCoeffs, cv::Mat stereoMapL_x, cv::Mat stereoMapL_y, cv::Mat stereoMapR_x, cv::Mat stereoMapR_y, cv::Mat Q); + +bool writeDisparityParameters(std::string filename, int numDisparities, int blockSize , int preFilterType, int preFilterSize, int preFilterCap, int minDisparity , int textureThreshold, int uniquenessRatio, int speckleRange, int speckleWindowSize, int disp12MaxDiff); + +bool writeCamRobotCalibrationParameters(std::string filename, cv::Mat robotCamTransform); + +bool writeCameraChessboardCalibrationParameters(std::string filename, std::vector objPoints, std::vector imagePoints, cv::Mat cameraChessboardTransform); +#endif \ No newline at end of file diff --git a/lib/MonoCameraCalibration.o b/lib/MonoCameraCalibration.o new file mode 100644 index 0000000..9aabd68 Binary files /dev/null and b/lib/MonoCameraCalibration.o differ diff --git a/lib/ReadWriteFunctions.o b/lib/ReadWriteFunctions.o new file mode 100644 index 0000000..abe62d1 Binary files /dev/null and b/lib/ReadWriteFunctions.o differ diff --git a/makefile b/makefile new file mode 100644 index 0000000..858950b --- /dev/null +++ b/makefile @@ -0,0 +1,13 @@ +all: ReadWrite MonoCamCalib + g++ lib/MonoCameraCalibration.o lib/ReadWriteFunctions.o -o bin/MonoCameraCalibration -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4` + +MonoCamCalib: src/MonoCameraCalibration.cpp + g++ -c src/MonoCameraCalibration.cpp -o lib/MonoCameraCalibration.o -I./include -I/usr/include/opencv4 + +ReadWrite: src/ReadWriteFunctions.cpp + g++ -c src/ReadWriteFunctions.cpp -o lib/ReadWriteFunctions.o -I./include -I/usr/include/opencv4 + + +clean: + rm lib/*.o + rm bin/* \ No newline at end of file diff --git a/src/MonoCameraCalibration.cpp b/src/MonoCameraCalibration.cpp new file mode 100644 index 0000000..2c35539 --- /dev/null +++ b/src/MonoCameraCalibration.cpp @@ -0,0 +1,278 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "ReadWriteFunctions.h" + +#define MONO_CALIB_PARAMS_FILENAME "./data/camera_calibration_params.xml" +#define FPS 20 +#define CAM_INDEX 0 +#define NB_FRAMES 25 +#define SQUARE_SIZE 23.4 +#define CHECKERBOARD_WIDTH 6 +#define CHECKERBOARD_HEIGHT 9 +#define RESOLUTION_MAX 800 + + +int main( int argc, char** argv ) +{ + // initializes main parameters + float fFPS = FPS; + int iCamIndex = CAM_INDEX; + int iNbFrames = NB_FRAMES; + float fSquareSize = SQUARE_SIZE; // in mm + int iCheckerBoardWidth = CHECKERBOARD_WIDTH; + int iCheckerBoardHeight = CHECKERBOARD_HEIGHT; + std::string sMonoCalibParamFilename = MONO_CALIB_PARAMS_FILENAME; + int iMaxVideoResolution = RESOLUTION_MAX; + + + // updates main parameters from arguments + int opt; + while ((opt = getopt (argc, argv, ":o:f:c:n:s:w:h:i:j:r:")) != -1) + { + switch (opt) + { + case 'o': + sMonoCalibParamFilename = optarg; + break; + case 'f': + fFPS = atof(optarg); + break; + case 'c': + iCamIndex = atoi(optarg); + break; + case 'n': + iNbFrames = atoi(optarg); + break; + case 's': + fSquareSize = atof(optarg); + break; + case 'w': + iCheckerBoardWidth = atoi(optarg); + break; + case 'h': + iCheckerBoardHeight = atoi(optarg); + break; + case 'r': + iMaxVideoResolution = atoi(optarg); + break; + case '?': + if (optopt == 'o' || optopt == 'f' || optopt == 'c' || optopt == 'n' || optopt == 's' || optopt == 'w' || optopt == 'h'|| optopt == 'i'|| optopt == 'j'|| optopt == 'r') + fprintf (stderr, "Option -%c requires an argument.\n", optopt); + else if (isprint (optopt)) + fprintf (stderr, "Unknown option `-%c'.\n", optopt); + else + fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt); + return 1; + default: + abort (); + } + } + + // creates camera grabbers for the left and right cameras + std::cout << "[INFO] Opening camera videostreams..."; + cv::VideoCapture cam(iCamIndex, cv::CAP_V4L2); + + // changes image resolution to maximum (e.g. 1920x1080 if possible) + cam.set(cv::CAP_PROP_FRAME_HEIGHT, iMaxVideoResolution); cam.set(cv::CAP_PROP_FRAME_WIDTH, iMaxVideoResolution); + + // checks if the camera was successfully opened + if (!cam.isOpened()) // if not success, exit program + { + std::cout << std::endl; + std::cout << "[ERROR] Could not open the camera!" << std::endl; + return 1; + } + std::cout << "OK!" << std::endl; + + // gets image resolution for info + std::cout << "[INFO] Left camera resolution: " << cam.get(cv::CAP_PROP_FRAME_WIDTH) << "x" << cam.get(cv::CAP_PROP_FRAME_HEIGHT) << std::endl; + + // inits display + cv::namedWindow("Frames", cv::WINDOW_NORMAL); + cv::resizeWindow("Frames", 640, 480); + + // defines the world coordinates for 3D points (in mm) + std::vector objp; + for(int i{0}; i > objpoints; // vector to store vectors of 3D points for each checkerboard image + std::vector > imgpoints; // vector to store vectors of 2D points for each checkerboard image + + while (true) + { + // reads a new frame from left & right cameras + bool bCamSuccess = cam.read(frame); + + // checks if a new frame was grabbed + if (!bCamSuccess) //if not success, break loop + { + std::cout << "[WARNING] Could not read a frame from video stream" << std::endl; + break; + } + + // gets a copy for display + view = frame.clone(); + + // converts frames to grayscale + cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); + + // looks for chessboard in the frames + std::vector corners; + cv::Size board = cv::Size(iCheckerBoardWidth, iCheckerBoardHeight); + bool found= cv::findChessboardCorners(gray, board, corners, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FILTER_QUADS); + + // checks if chessboard was found + if (found && !bIsCalibrated) + { + cv::TermCriteria criteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, 30, 0.001); + + // refines pixel coordinates for given 2d points + cv::cornerSubPix(gray, corners, cv::Size(11,11), cv::Size(-1,-1), criteria); + + // displays the detected corner points on the checkerboard + cv::drawChessboardCorners(view, cv::Size(iCheckerBoardWidth, iCheckerBoardHeight), corners, found); + + cv::putText(view, "Press g to grab the frame", cv::Point(50, 50), 2, 1, cv::Scalar(0, 200, 0), 2); + } + + if (!bIsCalibrated) + { + // adds frame number and displays it + std::string sCounter = std::to_string(iCount) + "/" + std::to_string(iNbFrames); + cv::putText(view, sCounter, cv::Point(50, 100), 2, 1, cv::Scalar(0, 0, 200), 2); + } + + // displays the current frame + cv::imshow("Frames", view); + + // waits for awhile depending on the FPS value + char key = (char)cv::waitKey(1000.0/fFPS); + + // checks if ESC was pressed to exit + if (key == 27) // if 'esc' key is pressed, break loop + { + std::cout << "[INFO] Esc key is pressed by user -> Shutting down!" << std::endl; + return 3; + } + + // checks if 'g' was pressed to save the current frame + if (key == 'g' && found) // if 'g' key is pressed, save the frame + { + std::cout << "[INFO] Checkerboard information is saved: " << iCount << "/" << iNbFrames << std::endl; + + // stores 3D and corresponding 2D points + objpoints.push_back(objp); + imgpoints.push_back(corners); + + // increments the counter + iCount++; + + // creates a blink effect + bitwise_not(view, view); + cv::imshow("Frames", view); + cv::waitKey(200); + } + + // checks if 'u' was pressed to undistort the frame when calibration is done + if (key == 'u' && bIsCalibrated) + { + bIsUndistort = !bIsUndistort; + view = frame.clone(); + + if (bIsUndistort) + { + std::cout << "[INFO] Undistort frame" << std::endl; + undistort(frame, view, camMatrix, distCoeffs); + } + else + std::cout << "[INFO] Original frame" << std::endl; + + cv::imshow("Frames", view); + } + + // checks if 's' was pressed to save the matrix intrinsic and distortion information to a yaml file + if (key == 's' && bIsCalibrated) + { + // saves the calibration params in a yaml file + std::cout << "[INFO] Save the result in a yaml file... "; + bool isMonoCalibParamsSaved = writeMonoCameraParameters(sMonoCalibParamFilename, camMatrix, distCoeffs); + + // checks if the stereo camera parameters were successfully read + if (!isMonoCalibParamsSaved) + { + std::cout << std::endl; + std::cout << "\t[ERROR] Mono camera parameters could not be saved!" << std::endl; + return 4; + } + std::cout << "OK!" << std::endl; + } + + // performes calibration if number of frames is sufficient + if (iCount >= iNbFrames && !bIsCalibrated) + { + view = frame.clone(); + cv::putText(view, "Calibration in progress...", cv::Point(50, 50), 2, 1, cv::Scalar(0, 200, 0), 2); + cv::imshow("Frames", view); + cv::waitKey(200); + + /* + * Performing camera calibration by passing the value of known 3D points (objpoints) + * and corresponding pixel coordinates of the detected corners (imgpoints) + */ + cv::Mat R, T; + std::vector newObjPoints; + int iFixedPoint = -1; + iFixedPoint = iCheckerBoardHeight - 1; + int flag = 0; + flag |= cv::CALIB_ZERO_TANGENT_DIST; + flag |= cv::CALIB_FIX_ASPECT_RATIO; + + // calibrates camera + std::cout << "[INFO] Calibrate the camera" << std::endl; + double rms = cv::calibrateCameraRO(objpoints, imgpoints, gray.size(), iFixedPoint, camMatrix, distCoeffs, R, T, newObjPoints, flag | cv::CALIB_USE_LU); + std::cout << "--> RMS reprojection error = " << rms << std::endl; + + bIsCalibrated = true; + + view = frame.clone(); + cv::putText(view, "Calibration done", cv::Point(50, 50), 2, 1, cv::Scalar(0, 200, 0), 2); + cv::putText(view, "Press u to visualize undistort frame", cv::Point(50, 100), 2, 1, cv::Scalar(0, 200, 0), 2); + cv::putText(view, "Press s to save the intrinsic/distortion params", cv::Point(50, 150), 2, 1, cv::Scalar(0, 200, 0), 2); + cv::imshow("Frames", view); + cv::waitKey(-1); + } + } + + // releases video stream + cam.release(); + + // destroys all windows + cv::destroyAllWindows(); + + return 0; +} + + + + + + + \ No newline at end of file diff --git a/src/ReadWriteFunctions.cpp b/src/ReadWriteFunctions.cpp new file mode 100644 index 0000000..2057414 --- /dev/null +++ b/src/ReadWriteFunctions.cpp @@ -0,0 +1,244 @@ +#include "ReadWriteFunctions.h" + + + +bool readMonoCameraCalibParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (READ) the mono camera calibration parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs["camera_matrix"] >> camMatrix; + fs["distortion_coefficients"] >> distCoeffs; + + // releases the reader + fs.release(); + + return true; +} + +bool readStereoCameraParameters(std::string filename, cv::Mat &camLMatrix, cv::Mat & distLCoeffs, cv::Mat &camRMatrix, cv::Mat & distRCoeffs, cv::Mat &stereoMapL_x, cv::Mat &stereoMapL_y, cv::Mat &stereoMapR_x, cv::Mat &stereoMapR_y, cv::Mat &Q) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (READ) the stereo camera parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs["Left_Camera_Matrix"] >> camLMatrix; + fs["Left_Camera_Distortion"] >> distLCoeffs; + fs["Right_Camera_Matrix"] >> camRMatrix; + fs["Right_Camera_Distortion"] >> distRCoeffs; + fs["Left_Stereo_Map_x"] >> stereoMapL_x; + fs["Left_Stereo_Map_y"] >> stereoMapL_y; + fs["Right_Stereo_Map_x"] >> stereoMapR_x; + fs["Right_Stereo_Map_y"] >> stereoMapR_y; + fs["Q"] >> Q; + + // releases the reader + fs.release(); + + return true; +} + +bool readDisparityParameters(std::string filename, int &numDisparities, int &blockSize , int &preFilterType, int &preFilterSize, int &preFilterCap, int &minDisparity , int &textureThreshold, int &uniquenessRatio, int &speckleRange, int &speckleWindowSize, int &disp12MaxDiff) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (READ) the disparity parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs["numDisparities"] >> numDisparities; + fs["blockSize"] >> blockSize; + fs["preFilterType"] >> preFilterType; + fs["preFilterSize"] >> preFilterSize; + fs["preFilterCap"] >> preFilterCap; + fs["minDisparity"] >> minDisparity; + fs["textureThreshold"] >> textureThreshold; + fs["uniquenessRatio"] >> uniquenessRatio; + fs["speckleRange"] >> speckleRange; + fs["speckleWindowSize"] >> speckleWindowSize; + fs["disp12MaxDiff"] >> disp12MaxDiff; + + // releases the reader + fs.release(); + + return true; +} + +bool readColorParameters(std::string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (READ) the color parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs["lowH"] >> iLowH; + fs["highH"] >> iHighH; + fs["lowS"] >> iLowS; + fs["highS"] >> iHighS; + fs["lowV"] >> iLowV; + fs["highV"] >> iHighV; + + // releases the reader + fs.release(); + + return true; +} + +bool readCameraChessboardCalibrationParameters(std::string filename, std::vector& objPoints, std::vector& imagePoints, cv::Mat& cameraChessboardTransform) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (READ) the camera/chessboard calibration parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs["objPoints"] >> objPoints; + fs["imagePoints"] >> imagePoints; + fs["cameraChessboardTransform"] >> cameraChessboardTransform; + + // releases the reader + fs.release(); + + return true; +} + +bool writeMonoCameraParameters(std::string filename, cv::Mat camMatrix, cv::Mat distCoeffs) +{ + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (WRITE) the mono camera parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs << "camera_matrix" << camMatrix; + fs << "distortion_coefficients" << distCoeffs; + + // releases the writer + fs.release(); + + return true; +} + +bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV) +{ + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (WRITE) the color parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs << "lowH" << iLowH; + fs << "highH" << iHighH; + fs << "lowS" << iLowS; + fs << "highS" << iHighS; + fs << "lowV" << iLowV; + fs << "highV" << iHighV; + + // releases the writer + fs.release(); + + return true; +} + +bool writeStereoCameraParameters(std::string filename, cv::Mat camLMatrix, cv::Mat distLCoeffs, cv::Mat camRMatrix, cv::Mat distRCoeffs, cv::Mat stereoMapL_x, cv::Mat stereoMapL_y, cv::Mat stereoMapR_x, cv::Mat stereoMapR_y, cv::Mat Q) +{ + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (WRITE) the stereo camera parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs << "Left_Camera_Matrix" << camLMatrix; + fs << "Left_Camera_Distortion" << distLCoeffs; + fs << "Right_Camera_Matrix" << camRMatrix; + fs << "Right_Camera_Distortion" << distRCoeffs; + fs << "Left_Stereo_Map_x" << stereoMapL_x; + fs << "Left_Stereo_Map_y" << stereoMapL_y; + fs << "Right_Stereo_Map_x" << stereoMapR_x; + fs << "Right_Stereo_Map_y" << stereoMapR_y; + fs << "Q" << Q; + + // releases the writer + fs.release(); + + return true; +} + +bool writeDisparityParameters(std::string filename, int numDisparities, int blockSize , int preFilterType, int preFilterSize, int preFilterCap, int textureThreshold, int uniquenessRatio, int speckleRange, int speckleWindowSize, int disp12MaxDiff, int minDisparity ) +{ + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (WRITE) the disparity parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs << "numDisparities" << numDisparities; + fs << "blockSize" << blockSize; + fs << "preFilterType" << preFilterType; + fs << "preFilterSize" << preFilterSize; + fs << "preFilterCap" << preFilterCap; + fs << "minDisparity" << minDisparity; + fs << "textureThreshold" << textureThreshold; + fs << "uniquenessRatio" << uniquenessRatio; + fs << "speckleRange" << speckleRange; + fs << "speckleWindowSize" << speckleWindowSize; + fs << "disp12MaxDiff" << disp12MaxDiff; + + // releases the writer + fs.release(); + + return true; +} + + +bool writeCamRobotCalibrationParameters(std::string filename, cv::Mat robotCamTransform) +{ + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (WRITE) the camera/robot calibration parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs << "robotCamTransform" << robotCamTransform; + + // releases the writer + fs.release(); + + return true; +} + + +bool writeCameraChessboardCalibrationParameters(std::string filename, std::vector objPoints, std::vector imagePoints, cv::Mat cameraChessboardTransform) +{ + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open (WRITE) the camera/chessboard calibration parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs << "objPoints" << objPoints; + fs << "imagePoints" << imagePoints; + fs << "cameraChessboardTransform" << cameraChessboardTransform; + + // releases the writer + fs.release(); + + return true; +} \ No newline at end of file