charles.stelandre
/
SensingLab3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop'

This commit is contained in:
ros 2025-05-19 13:57:42 +02:00
parent 02b9648851 43a9408da2
commit 6588816fd4
8 changed files with 580 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
bin/MonoCameraCalibration Executable file
View File

Binary file not shown.

17
data/camera_calibration_params.xml Normal file
View File

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<opencv_storage>
<camera_matrix type_id="opencv-matrix">
<rows>3</rows>
<cols>3</cols>
<dt>d</dt>
<data>
8.1538658490193018e+02 0. 4.0344686660476424e+02 0.
8.1538658490193018e+02 2.5935115109955393e+02 0. 0. 1.</data></camera_matrix>
<distortion_coefficients type_id="opencv-matrix">
<rows>1</rows>
<cols>5</cols>
<dt>d</dt>
<data>
-3.4454237775633917e-01 7.7306959051926261e-01 0. 0.
-3.5139312654968990e+00</data></distortion_coefficients>
</opencv_storage>

28
include/ReadWriteFunctions.h Normal file
View File

@ -0,0 +1,28 @@
#ifndef _READWRITE_FUNCTIONS_
#define _READWRITE_FUNCTIONS_
#include <opencv2/opencv.hpp>
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<cv::Point3f>& objPoints, std::vector<cv::Point2f>& 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<cv::Point3f> objPoints, std::vector<cv::Point2f> imagePoints, cv::Mat cameraChessboardTransform);
#endif

BIN
lib/MonoCameraCalibration.o Normal file
View File

Binary file not shown.

BIN
lib/ReadWriteFunctions.o Normal file
View File

Binary file not shown.

13
makefile Normal file
View File

@ -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/*

278
src/MonoCameraCalibration.cpp Normal file
View File

@ -0,0 +1,278 @@
#include <opencv2/opencv.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <stdio.h>
#include <iostream>
#include <unistd.h>
#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<cv::Point3f> objp;
for(int i{0}; i<iCheckerBoardHeight; i++)
{
for(int j{0}; j<iCheckerBoardWidth; j++)
objp.push_back(cv::Point3f(j*fSquareSize, i*fSquareSize, 0));
}
// main loop launched every FPS
int iCount = 0;
bool bIsCalibrated = false;
bool bIsUndistort = false;
cv::Mat camMatrix, distCoeffs;
cv::Mat frame, gray, view;
std::vector<std::vector<cv::Point3f> > objpoints; // vector to store vectors of 3D points for each checkerboard image
std::vector<std::vector<cv::Point2f> > 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<cv::Point2f> 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<cv::Point3f> 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;
}

244
src/ReadWriteFunctions.cpp Normal file
View File

@ -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<cv::Point3f>& objPoints, std::vector<cv::Point2f>& 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<cv::Point3f> objPoints, std::vector<cv::Point2f> 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;
}