void DrawAxis::openWebcam()
{
webcam.open(0);
webcam.set(CV_CAP_PROP_FRAME_WIDTH, frameWidth);
webcam.set(CV_CAP_PROP_FRAME_HEIGHT, frameHeight);
rvec = Mat(Size(3, 1), CV_64F);
tvec = Mat(Size(3, 1), CV_64F);
cout << "intrinsinc = " << intrinsic_params << endl;
cout << "dist = " << distortion_params << endl;
cout << "intrinsic.size = " << intrinsic_params.size() << endl;
while (true){
webcam.read(webcamImage);
bool findCorners = findChessboardCorners(webcamImage, boardSize, corners, CALIB_CB_FAST_CHECK);
if (findCorners){
solvePnP(Mat(boardPoints), Mat(corners), intrinsic_params, distortion_params, rvec, tvec, false);
projectPoints(cubePoints, rvec, tvec, intrinsic_params, distortion_params, cubeFramePoints);
projectPoints(framePoints, rvec, tvec, intrinsic_params, distortion_params, imageFramePoints);
drawAxis(webcamImage, color, 3);
drawCube(webcamImage, cubeFramePoints, Scalar(255, 0, 255), 2);
}
namedWindow("OpenCV Webcam", 0);
imshow("OpenCV Webcam", webcamImage);
waitKey(10);
}
}
bool Calibration::findBoard(Mat img, vector<Point2f>& pointBuf, bool refine) {
bool found=false;
if(patternType == CHESSBOARD) {
// no CV_CALIB_CB_FAST_CHECK, because it breaks on dark images (e.g., dark IR images from kinect)
// int chessFlags = CV_CALIB_CB_ADAPTIVE_THRESH;// | CV_CALIB_CB_NORMALIZE_IMAGE;
int chessFlags = CV_CALIB_CB_ADAPTIVE_THRESH;
found = findChessboardCorners(img, patternSize, pointBuf, chessFlags);
// improve corner accuracy
if(found) {
if(img.type() != CV_8UC1) {
cvtColor(img, grayMat, CV_RGB2GRAY);
} else {
grayMat = img;
}
if(refine) {
// the 11x11 dictates the smallest image space square size allowed
// in other words, if your smallest square is 11x11 pixels, then set this to 11x11
cornerSubPix(grayMat, pointBuf, subpixelSize, cv::Size(-1,-1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
}
}
}
//#ifdef USING_OPENCV_2_3
else {
int flags = (patternType == CIRCLES_GRID ? CALIB_CB_SYMMETRIC_GRID : CALIB_CB_ASYMMETRIC_GRID); // + CALIB_CB_CLUSTERING
found = findCirclesGrid(img, patternSize, pointBuf, flags);
}
//#endif
return found;
}
void Camera::calibrate(vector<string> imageList, int boardWidth, int boardHeight) {
vector<vector<Point2f> > imagePoints;
Size boardSize, imageSize;
boardSize.width=boardWidth;
boardSize.height=boardHeight;
int flags=0;
int i;
float squareSize = 1.f, aspectRatio = 1.f;
Mat view, viewGray;
for(i = 0; i<(int)imageList.size();i++)
{
view = cv::imread(imageList[i], 1);
imageSize = view.size();
vector<Point2f> pointbuf;
cvtColor(view, viewGray, CV_BGR2GRAY);
bool found = findChessboardCorners( view, boardSize, pointbuf,
CV_CALIB_CB_ADAPTIVE_THRESH & CV_CALIB_CB_FAST_CHECK & CV_CALIB_CB_NORMALIZE_IMAGE);
if(found)
{
drawChessboardCorners( view, boardSize, Mat(pointbuf), found );
imagePoints.push_back(pointbuf);
}
}
prepareandRunCalibration(imagePoints, imageSize, boardSize, squareSize, aspectRatio, flags, K, Dist);
}
void stereoCalibThread::monoCalibration(const vector<string>& imageList, int boardWidth, int boardHeight, Mat &K, Mat &Dist)
{
vector<vector<Point2f> > imagePoints;
Size boardSize, imageSize;
boardSize.width=boardWidth;
boardSize.height=boardHeight;
int flags=0;
int i;
float squareSize = 1.f, aspectRatio = 1.f;
Mat view, viewGray;
for(i = 0; i<(int)imageList.size();i++)
{
view = cv::imread(imageList[i], 1);
imageSize = view.size();
vector<Point2f> pointbuf;
cvtColor(view, viewGray, CV_BGR2GRAY);
bool found = false;
if(boardType == "CIRCLES_GRID") {
found = findCirclesGridDefault(view, boardSize, pointbuf, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
found = findCirclesGridDefault(view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else {
found = findChessboardCorners( view, boardSize, pointbuf,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
}
if(found)
{
drawChessboardCorners( view, boardSize, Mat(pointbuf), found );
imagePoints.push_back(pointbuf);
}
}
std::vector<Mat> rvecs, tvecs;
std::vector<float> reprojErrs;
double totalAvgErr = 0;
K = Mat::eye(3, 3, CV_64F);
if( flags & CV_CALIB_FIX_ASPECT_RATIO )
K.at<double>(0,0) = aspectRatio;
Dist = Mat::zeros(4, 1, CV_64F);
std::vector<std::vector<Point3f> > objectPoints(1);
calcChessboardCorners(boardSize, squareSize, objectPoints[0]);
objectPoints.resize(imagePoints.size(),objectPoints[0]);
double rms = calibrateCamera(objectPoints, imagePoints, imageSize, K,
Dist, rvecs, tvecs,CV_CALIB_FIX_K3);
printf("RMS error reported by calibrateCamera: %g\n", rms);
cout.flush();
}
bool Fiducial::find(Mat gray, bool accurate =false){
found = findChessboardCorners(gray, size, corners,CV_CALIB_CB_ADAPTIVE_THRESH
+ CV_CALIB_CB_NORMALIZE_IMAGE
+ CV_CALIB_CB_FAST_CHECK);
if(found && accurate){
cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1),TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
}
return found;
}
// Sarkpontok koordinátáinak helyes megkapása:
bool Chessboard::getCornerGoodCoordinates(void)
{
// Tábla megkeresése, sarkok visszaadása
bool found = findChessboardCorners(this->GreyPicture, this->BoardSize, this->CornerPoints,
CV_CALIB_CB_ADAPTIVE_THRESH); ///| CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
// Hibakezelés:
if (!found)
{
return false;
}
else
{
// ----------------------------------------------------------------------------------
// Rendezni kell a sarkpontokat, mert alapvetõen random sorrendben jelennek meg.
// Sarkpont iteráció: keressük a min és max y koordinátájú sarkpontokat:
Point2f FirstPoint = CornerPoints[0];
float MinY = 10000;
float MaxY = 0;
float MaxX = 0;
float MinX = 10000;
// Sarkpontokon (!) végiglépkedve
std::vector<Point2f>::iterator it = CornerPoints.begin();
for (int CornerIdx = 0; CornerIdx < 4; CornerIdx++)
{
// Szélsõ pontok kiválasztása a tábla helyzetének megállapításához
if (it->y > MaxY) MaxY = it->y;
if (it->y < MinY) MinY = it->y;
if (it->x > MaxX) MaxX = it->x;
if (it->x < MinX) MinX = it->x;
// Következõ sarkpontra lépés:
if (CornerIdx % 2 == 0) it += 8;
else if (CornerIdx == 1) it += 64;
}
// Meglévõ sarkpontok transzformációjának 4 esetre bontása:
// Határok meghúzása:
float xBorder = MinX + ((MaxX - MinX) / 2);
float yBorder = MinY + ((MaxY - MinY) / 2);
if (FirstPoint.x <= xBorder && FirstPoint.y <= yBorder) transformCorners90();
else if (FirstPoint.x < xBorder && FirstPoint.y > yBorder); // Ez alapból jó
else if (FirstPoint.x > xBorder && FirstPoint.y < yBorder) transformCorners180();
else if (FirstPoint.x >= xBorder && FirstPoint.y >= yBorder) transformCorners270();
else
{
cout << "Rohadt nagy error a sarkpont koordináta transzformációban!" << endl;
return false;
}
return true;
}
}
void Corners(Size boardSize)
{
//namedWindow( "image left", WINDOW_AUTOSIZE );
//namedWindow( "image right", WINDOW_AUTOSIZE );
bool foundL = findChessboardCorners( left, boardSize, ptvecL, CV_CALIB_CB_ADAPTIVE_THRESH );
bool foundR = findChessboardCorners( right, boardSize, ptvecR, CV_CALIB_CB_ADAPTIVE_THRESH );
/*
std::cout << "foundL = " << foundL << " foundR = " << foundR << std::endl;
std::cout << " L " << ptvecL.size() << " R " << ptvecR.size() << std::endl;
if(!foundL)
{
std::cout << "FoundL not true" << std::endl;
rotate(left, -90, left);
ptvecL.clear();
bool foundL = findChessboardCorners( left, boardSize, ptvecL, CV_CALIB_CB_ADAPTIVE_THRESH );
std::cout << "foundL = " << foundL << " L " << ptvecL.size() << std::endl;
}
else
{
std::cout << "foundL = " << foundL << " L " << ptvecL.size() << std::endl;
}
if(!foundR)
{
std::cout << "FoundR not true" << std::endl;
flip(right,right,1);
ptvecR.clear();
foundR = findChessboardCorners( left, boardSize, ptvecL, CV_CALIB_CB_ADAPTIVE_THRESH );
std::cout << "foundR = " << foundL << " R " << ptvecR.size() << std::endl;
}
else
{
std::cout << "foundR = " << foundR << " R " << ptvecR.size() << std::endl;
}
*/
drawChessboardCorners( left, boardSize,ptvecL, foundL );
drawChessboardCorners( right, boardSize,ptvecR, foundR );
//imshow("image left ",left);
//imshow("image right ",right);
}
bool generate2DPointsFromCheesboard(Mat undistord_image, Size chess_height_width, std::vector<Point2f> &ptvec)
{
//cvtColor(undistord_image,undistord_image,CV_RGB2GRAY);
bool found = findChessboardCorners( undistord_image, chess_height_width, ptvec, CV_CALIB_CB_ADAPTIVE_THRESH );
//cout << ptvec ;
if (found)
drawChessboardCorners(undistord_image,chess_height_width, ptvec, found) ;
else cout<< "ERROR ; chessboard not found" << endl;
//imshow("Cheesboard found", undistord_image);
//waitKey(0);
return found ;
}
bool Chessboard::findCorners(ofPixels &image, vector<ofVec2f> &points) const {
int subPixSearch = image.getWidth() / squaresX / 10.0f;
if (subPixSearch % 2 == subPixSearch)
subPixSearch++; //ensure odd numbers
int chessFlags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK;
bool found;
Mat img = toCv(image);
found = findChessboardCorners(img, cv::Size(squaresX-1, squaresY-1), *(vector<Point2f>*)&points, chessFlags);
return found;
}
//导入标定图片，提取角点
bool CSingleCalib::addChessBoardPoints(String *strImageName)
{
cout << "CSingleCalib::addChessBoardPoints !" << endl;
bool bRet = false;
do
{
vector<Point2f> imageCorners;
vector<Point3f> objectCorners;
for (int i = 0; i < ChessBoardSize_h; ++i)
{
for (int j = 0; j < ChessBoardSize_w; ++j)
{
objectCorners.push_back(Point3f(i*SquareWidth, j*SquareWidth, 0));
}
}
cv::Mat image;
for (int i = 1; i <= NImage; ++i)
{
image = imread(*strImageName);
cvtColor(image, grayImage, CV_BGR2GRAY);
bool found = findChessboardCorners(image, ChessBoardSize, imageCorners);
cornerSubPix(grayImage, imageCorners, Size(5, 5), cv::Size(-1, -1),
cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1));
cout << i << endl;
if (imageCorners.size() != ChessBoardSize.area())
{
cout << "检测的角点数和棋盘格本身的数目不等" << endl;
break;
}
addPoints(imageCorners, objectCorners);
cv::drawChessboardCorners(image, ChessBoardSize, imageCorners, found);
cv::imshow("BoardCorner", image);
cv::waitKey(10);
++strImageName;
}
// cvDestroyWindow("image");
cvDestroyWindow("BoardCorner");
bRet = true;
} while (false);
return bRet;
}
std::vector<pf::Corners> getCornersSamples(size_t index) {
cv::Size numSquares(NUM_HOR_SQUARES, NUM_VER_SQUARES);
cv::VideoCapture capture(index + 1);
if (!capture.isOpened()) {
std::cerr << "Can't open the camera" << std::endl;
std::exit(-1);
}
capture.set(CV_CAP_PROP_FPS, FRAMES_PER_SECOND);
std::vector<pf::Corners> cornersSamples;
bool started = false;
clock_t time = 0;
while (cornersSamples.size() < NUM_FRAMES) {
// Capture frame
cv::Mat frame;
capture >> frame;
// Find chessboard corners
auto found = findChessboardCorners(frame);
if (found.second && started && clock() - time > DELAY_BETWEEN_FRAMES) {
time = clock();
cornersSamples.push_back(found.first);
cv::bitwise_not(frame, frame);
}
// Show image
cv::drawChessboardCorners(frame, numSquares, cv::Mat(found.first), found.second);
cv::imshow("Calibrate", frame);
// Wait for 's' to start
if (cv::waitKey(100) == 's') {
started = true;
}
}
return cornersSamples;
}
void findCorner(vector<Point2f>&corners,vector<vector<Point2f>>&corners_Seq,vector<Mat>&image_Seq){
cout<<"start find corners"<<endl;
int image_count = 25;
Size board_size = Size(9,6);
int successImageNum = 0;
Mat image,imageGray;
int count = 0;
for(int i = 0 ;i<image_count;i++){
image = imread(findFileName(i));
cvtColor(image,imageGray,CV_RGB2GRAY);
bool found = findChessboardCorners(image, board_size, corners,CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE+
CALIB_CB_FAST_CHECK);
if (!found){
cout <<"not found corners!"<< endl;
continue;
}else{
cornerSubPix(imageGray, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1);
Mat temp = image.clone();
drawChessboardCorners( temp, board_size, Mat(corners), found );
string imageFileName;
std::stringstream StrStm;
StrStm<<i+1;
StrStm>>imageFileName;
imageFileName += "_corner.jpg";
imwrite(imageFileName,temp);
cout<<"Frame corner#"<<i+1<<"...end"<<endl;
count = count + corners.size();
successImageNum = successImageNum + 1;
corners_Seq.push_back(corners);
}
image_Seq.push_back(image);
}
cout << " corners find success"<< endl;
}
bool Calibration::findBoard(Mat img, vector<Point2f> &pointBuf, bool refine) {
// no CV_CALIB_CB_FAST_CHECK, because it breaks on dark images (e.g., dark IR images from kinect)
int chessFlags = CV_CALIB_CB_ADAPTIVE_THRESH;// | CV_CALIB_CB_NORMALIZE_IMAGE;
bool found = findChessboardCorners(img, patternSize, pointBuf, chessFlags);
// improve corner accuracy
if(found) {
if(img.type() != CV_8UC1) {
cvtColor(img, grayMat, CV_RGB2GRAY);
} else {
grayMat = img;
}
// the 11x11 dictates the smallest image space square size allowed
// in other words, if your smallest square is 11x11 pixels, then set this to 11x11
cornerSubPix(grayMat, pointBuf, cv::Size(11, 11), cv::Size(-1,-1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
return true;
} else {
return false;
}
}
//#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/xfeatures2d/nonfree.hpp>
int main() {
char key;
int i = 0;
int j = 0;
bool boardFound = false;
cv::Mat currFrame;
std::vector<cv::Point2f> boardCorners;
cv::namedWindow("Camera_Output", 1);
//cv::VideoCapture capture(CV_CAP_ANY); //Capture using any camera connected to your system
cv::VideoCapture capture(1); //Capture using any camera connected to your system
if(!capture.isOpened()) { // check if we succeeded
return -1;
}
while(1) { //Create infinte loop for live streaming
capture >> currFrame;
bool boardFound = findChessboardCorners(currFrame, cv::Size(5,7), boardCorners, CV_CALIB_CB_ADAPTIVE_THRESH );
if (boardFound) {
for(i=0, j=1; j<boardCorners.size(); ++i, ++j) {
cv::line(currFrame, boardCorners[i], boardCorners[j], cv::Scalar(0, 255, 0), 4);
}
}
cv::imshow("Camera_Output", currFrame);
key = cv::waitKey(10); //Capture Keyboard stroke
if ((char(key) == 27) || (char(key) == 'q')){
break; //If you hit ESC key loop will break.
}
}
return 0;
}
int StereoCameraCalibration::handleStereoCalibration(struct StereoCameraCalibration::CalibrationConfigStruct struct_calibrationConfig)
{
//The capture for the image
VideoCapture leftCameraCapture;
VideoCapture rightCameraCapture;
//Used to signify the capture portion is done
int quitCapture = 0;
int lastKey = -1;
int key = -1;
double totalAvgErr;
double rmsErr;
CalibrationStates currentCalibrationState = CALIBRATION_STATE_INIT;
//A vector to hold the points found during calibration
vector< vector<Point2f> >calibPoints[CAMERA_LOCATION_RIGHT+1];
//How many frames we have
int frameCount = 0;
clock_t prevTimeStamp = clock();
Mat currentLeftFrame;
Mat currentRightFrame;
Mat leftCaptureFrame;
Mat rightCaptureFrame;
Mat flippedLeftFrame;
Mat flippedRightFrame;
Mat cameraMatrix[2];
Mat distCoeffs[2];
Mat R, T, R1, R2, P1, P2, Q;
Rect validRoi[2];
Mat rmap[2][2];
while(currentCalibrationState != CALIBRATION_STATE_DONE)
{
key = waitKey(33) & 0xff;
switch(currentCalibrationState)
{
case CALIBRATION_STATE_INIT:
{
//Open the captures
leftCameraCapture.open(struct_calibrationConfig.leftCameraId);
rightCameraCapture.open(struct_calibrationConfig.rightCameraId);
if(!(leftCameraCapture.set(CV_CAP_PROP_FPS, 30.0)))
{
cout << "Left frame rate set failed" << endl;
}
if(!(rightCameraCapture.set(CV_CAP_PROP_FPS, 30.0)))
{
cout << "Right frame rate set failed" << endl;
}
if(!(leftCameraCapture.set(CV_CAP_PROP_FRAME_WIDTH, 640)))
{
cout << "Left frame width set failed" << endl;
}
if(!(leftCameraCapture.set(CV_CAP_PROP_FRAME_HEIGHT, 480)))
{
cout << "Left frame height set failed" << endl;
}
if(!(rightCameraCapture.set(CV_CAP_PROP_FRAME_WIDTH, 640)))
{
cout << "Right frame width set failed" << endl;
}
if(!(rightCameraCapture.set(CV_CAP_PROP_FRAME_HEIGHT, 480)))
{
cout << "Right frame height set failed" << endl;
}
//Named window for calibration
namedWindow("Current Left Calibration Image Raw", 1);
namedWindow("Current Right Calibration Image Raw", 1);
//Named window for calibration
namedWindow("Current Left Calibration Image", 1);
namedWindow("Current Right Calibration Image", 1);
cout << "Starting calibration feature point capture." << endl;
currentCalibrationState = CALIBRATION_STATE_SHOW_IMAGE_BEFORE_CAPTURE;
break;
}
case CALIBRATION_STATE_SHOW_IMAGE_BEFORE_CAPTURE:
{
if(leftCameraCapture.isOpened() && rightCameraCapture.isOpened())
{
leftCameraCapture >> leftCaptureFrame;
rightCameraCapture >> rightCaptureFrame;
leftCaptureFrame.copyTo(currentLeftFrame);
rightCaptureFrame.copyTo(currentRightFrame);
}
//cout << "Left Frame Size" <<currentLeftFrame.rows <<"x" << currentLeftFrame.cols << endl;
//cout << "Right Frame Size" <<currentRightFrame.rows <<"x" << currentRightFrame.cols << endl;
if(!currentLeftFrame.data)
{
cout << "No Frame Data from Left Camera" << endl;
return 2;
}
if(!currentRightFrame.data)
{
cout << "No Frame Data from Right Camera" << endl;
return 2;
}
//currentFrame.copyTo(flippedFrame);
flip(currentLeftFrame, flippedLeftFrame,1);
flip(currentRightFrame, flippedRightFrame,1);
imshow("Current Left Calibration Image Raw", flippedLeftFrame);
imshow("Current Right Calibration Image Raw", flippedRightFrame);
if(key == 's' || key == ' ')
{
prevTimeStamp = clock();
currentCalibrationState = CALIBRATION_STATE_IMAGE_CAPTURE;
}
if(key == 27)
{
currentCalibrationState =CALIBRATION_STATE_ABORT;
}
break;
}
case CALIBRATION_STATE_IMAGE_CAPTURE:
{
if(leftCameraCapture.isOpened() && rightCameraCapture.isOpened())
{
leftCameraCapture >> leftCaptureFrame;
leftCaptureFrame.copyTo(currentLeftFrame);
rightCameraCapture >> rightCaptureFrame;
rightCaptureFrame.copyTo(currentRightFrame);
}
if(!currentLeftFrame.data)
{
cout << "No Frame Data from Left Camera" << endl;
return 2;
}
if(!currentRightFrame.data)
{
cout << "No Frame Data from Right Camera" << endl;
return 2;
}
Mat flippedLeftFrame;
Mat flippedRightFrame;
//currentFrame.copyTo(flippedFrame);
flip(currentLeftFrame, flippedLeftFrame,1);
flip(currentRightFrame, flippedRightFrame,1);
imshow("Current Left Calibration Image Raw", flippedRightFrame);
imshow("Current Right Calibration Image Raw", flippedLeftFrame);
Mat currentFrameGray[CAMERA_LOCATION_RIGHT+1];
cvtColor(currentLeftFrame,currentFrameGray[CAMERA_LOCATION_LEFT],CV_BGR2GRAY);
cvtColor(currentRightFrame,currentFrameGray[CAMERA_LOCATION_RIGHT],CV_BGR2GRAY);
vector<Point2f> currentFramePoints[CAMERA_LOCATION_RIGHT+1];
bool foundPoints[CAMERA_LOCATION_RIGHT+1];
//Find the corners of the Chessboard
foundPoints[CAMERA_LOCATION_LEFT] = findChessboardCorners( currentFrameGray[CAMERA_LOCATION_LEFT], struct_calibrationConfig.boardSize, currentFramePoints[CAMERA_LOCATION_LEFT], CV_CALIB_CB_ADAPTIVE_THRESH & CV_CALIB_CB_FAST_CHECK & CV_CALIB_CB_NORMALIZE_IMAGE);
foundPoints[CAMERA_LOCATION_RIGHT] = findChessboardCorners( currentFrameGray[CAMERA_LOCATION_RIGHT], struct_calibrationConfig.boardSize, currentFramePoints[CAMERA_LOCATION_RIGHT], CV_CALIB_CB_ADAPTIVE_THRESH & CV_CALIB_CB_FAST_CHECK & CV_CALIB_CB_NORMALIZE_IMAGE);
if(foundPoints[CAMERA_LOCATION_LEFT] || foundPoints[CAMERA_LOCATION_RIGHT])
{
if(foundPoints[CAMERA_LOCATION_LEFT])
{
cornerSubPix( currentFrameGray[CAMERA_LOCATION_LEFT], currentFramePoints[CAMERA_LOCATION_LEFT], Size(5,5), Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
}
if(foundPoints[CAMERA_LOCATION_RIGHT])
{
cornerSubPix( currentFrameGray[CAMERA_LOCATION_RIGHT], currentFramePoints[CAMERA_LOCATION_RIGHT], Size(5,5), Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
}
if(foundPoints[CAMERA_LOCATION_LEFT] && foundPoints[CAMERA_LOCATION_RIGHT])
{
if(clock() - prevTimeStamp > struct_calibrationConfig.delay*1e-3*CLOCKS_PER_SEC)
{
prevTimeStamp = clock();
//blink = capture.isOpened();
bitwise_not(currentLeftFrame, currentLeftFrame);
bitwise_not(currentRightFrame, currentRightFrame);
calibPoints[CAMERA_LOCATION_LEFT].push_back(currentFramePoints[CAMERA_LOCATION_LEFT]);
calibPoints[CAMERA_LOCATION_RIGHT].push_back(currentFramePoints[CAMERA_LOCATION_RIGHT]);
frameCount++;
}
}
if(foundPoints[CAMERA_LOCATION_LEFT])
{
drawChessboardCorners( currentLeftFrame, struct_calibrationConfig.boardSize, Mat(currentFramePoints[CAMERA_LOCATION_LEFT]), foundPoints[CAMERA_LOCATION_LEFT] );
}
if(foundPoints[CAMERA_LOCATION_RIGHT])
{
drawChessboardCorners( currentRightFrame, struct_calibrationConfig.boardSize, Mat(currentFramePoints[CAMERA_LOCATION_RIGHT]), foundPoints[CAMERA_LOCATION_RIGHT] );
}
cout << "Good Frames: " << frameCount << endl;
imshow("Current Left Calibration Image", currentLeftFrame);
imshow("Current Right Calibration Image", currentRightFrame);
struct_calibrationConfig.imageSize = currentLeftFrame.size();
}
else
{
imshow("Current Left Calibration Image", currentFrameGray[CAMERA_LOCATION_LEFT]);
imshow("Current Right Calibration Image", currentFrameGray[CAMERA_LOCATION_RIGHT]);
}
if((key == 'd' || key == 'c' || key == 'r' || key == ' ') && frameCount >= 15)
{
currentCalibrationState =CALIBRATION_STATE_RUN;
}
if(key==27)
{
currentCalibrationState =CALIBRATION_STATE_ABORT;
}
break;
}
case CALIBRATION_STATE_RUN:
{
bool ok = runStereoCalibration(calibPoints, struct_calibrationConfig.imageSize, struct_calibrationConfig.boardSize, struct_calibrationConfig.squareSize,
struct_calibrationConfig.aspectRatio, struct_calibrationConfig.flags, cameraMatrix, distCoeffs,
R, T, R1, P1, R2, P2, Q, validRoi, rmsErr, totalAvgErr);
printf("%s. avg reprojection error = %.2f\n",
ok ? "Calibration succeeded" : "Calibration failed",
totalAvgErr);
if(ok)
{
cout << "Moving to save option." << endl;
//Precompute maps for cv::remap()
initUndistortRectifyMap(cameraMatrix[CAMERA_LOCATION_LEFT], distCoeffs[CAMERA_LOCATION_LEFT], R1, P1, struct_calibrationConfig.imageSize, CV_16SC2, rmap[CAMERA_LOCATION_LEFT][CAMERA_REMAP_AXIS_X], rmap[CAMERA_LOCATION_LEFT][CAMERA_REMAP_AXIS_Y]);
initUndistortRectifyMap(cameraMatrix[CAMERA_LOCATION_RIGHT], distCoeffs[CAMERA_LOCATION_RIGHT], R2, P2, struct_calibrationConfig.imageSize, CV_16SC2, rmap[CAMERA_LOCATION_RIGHT][CAMERA_REMAP_AXIS_X], rmap[CAMERA_LOCATION_RIGHT][CAMERA_REMAP_AXIS_Y]);
currentCalibrationState =CALIBRATION_STATE_SAVE;
}
else
{
cout << "Moving to waiting for image capture." << endl;
currentCalibrationState =CALIBRATION_STATE_SHOW_IMAGE_BEFORE_CAPTURE;
}
break;
}
case CALIBRATION_STATE_SAVE:
{
key = displayRemappedStereoImages(leftCameraCapture, rightCameraCapture,validRoi,struct_calibrationConfig.imageSize,rmap);
bool ok = false;
vector<vector<Point2f> > noPoints[2];
if( key == 's' )
{
this->imageSize = struct_calibrationConfig.imageSize;
this->boardSize = struct_calibrationConfig.boardSize;
this->squareSize = struct_calibrationConfig.squareSize;
this ->aspectRatio = struct_calibrationConfig.aspectRatio;
this->flags = struct_calibrationConfig.flags;
this->cameraMatrix[0] = cameraMatrix[0].clone();
this->cameraMatrix[1] = cameraMatrix[1].clone();
this->distCoeffs[0] = distCoeffs[0].clone();
this->distCoeffs[1] = distCoeffs[1].clone();
this->R = R.clone();
this->T = T.clone();
this->R1 = R1.clone();
this->P1 = P1.clone();
this->R2 = R2.clone();
this->P2 = P2.clone();
this->Q = Q.clone();
this->validRoi[0] = validRoi[0];
this->validRoi[1] = validRoi[1];
this->rmsErr = rmsErr;
this->imagePoints[0] = calibPoints[0];
this->imagePoints[1] = calibPoints[1];
this->totalAvgErr = totalAvgErr;
saveStereoCameraParams( struct_calibrationConfig.outputFilename, struct_calibrationConfig.imageSize,
struct_calibrationConfig.boardSize, struct_calibrationConfig.squareSize, struct_calibrationConfig.aspectRatio,
struct_calibrationConfig.flags, cameraMatrix, distCoeffs,
R, T, R1, P1, R2, P2, Q,validRoi,
rmsErr,
struct_calibrationConfig.writePoints ? calibPoints : noPoints,
totalAvgErr );
cout << "Stereo Calibration Data Saved" << endl;
currentCalibrationState =CALIBRATION_STATE_COMPLETE;
}
if(key == 27)
{
cout << "Move to abort" << endl;
currentCalibrationState =CALIBRATION_STATE_ABORT;
}
break;
}
case CALIBRATION_STATE_ABORT:
cout << "Calibration Aborted" << endl;
currentCalibrationState =CALIBRATION_STATE_COMPLETE;
break;
case CALIBRATION_STATE_COMPLETE:
cout << "Calibration Completed" << endl;
currentCalibrationState =CALIBRATION_STATE_DONE;
break;
default:
break;
}//switch
void stereoCalibThread::stereoCalibration(const vector<string>& imagelist, int boardWidth, int boardHeight,float sqsize)
{
Size boardSize;
boardSize.width=boardWidth;
boardSize.height=boardHeight;
if( imagelist.size() % 2 != 0 )
{
cout << "Error: the image list contains odd (non-even) number of elements\n";
return;
}
const int maxScale = 2;
// ARRAY AND VECTOR STORAGE:
std::vector<std::vector<Point2f> > imagePoints[2];
std::vector<std::vector<Point3f> > objectPoints;
Size imageSize;
int i, j, k, nimages = (int)imagelist.size()/2;
imagePoints[0].resize(nimages);
imagePoints[1].resize(nimages);
std::vector<string> goodImageList;
for( i = j = 0; i < nimages; i++ )
{
for( k = 0; k < 2; k++ )
{
const string& filename = imagelist[i*2+k];
Mat img = cv::imread(filename, 0);
if(img.empty())
break;
if( imageSize == Size() )
imageSize = img.size();
else if( img.size() != imageSize )
{
cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n";
break;
}
bool found = false;
std::vector<Point2f>& corners = imagePoints[k][j];
for( int scale = 1; scale <= maxScale; scale++ )
{
Mat timg;
if( scale == 1 )
timg = img;
else
resize(img, timg, Size(), scale, scale);
if(boardType == "CIRCLES_GRID") {
found = findCirclesGridDefault(timg, boardSize, corners, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
found = findCirclesGridDefault(timg, boardSize, corners, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else {
found = findChessboardCorners(timg, boardSize, corners,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
}
if( found )
{
if( scale > 1 )
{
Mat cornersMat(corners);
cornersMat *= 1./scale;
}
break;
}
}
if( !found )
break;
}
if( k == 2 )
{
goodImageList.push_back(imagelist[i*2]);
goodImageList.push_back(imagelist[i*2+1]);
j++;
}
}
fprintf(stdout,"%i pairs have been successfully detected.\n",j);
nimages = j;
if( nimages < 2 )
{
fprintf(stdout,"Error: too few pairs detected \n");
return;
}
imagePoints[0].resize(nimages);
imagePoints[1].resize(nimages);
objectPoints.resize(nimages);
for( i = 0; i < nimages; i++ )
{
for( j = 0; j < boardSize.height; j++ )
for( k = 0; k < boardSize.width; k++ )
objectPoints[i].push_back(Point3f(j*squareSize, k*squareSize, 0));
}
fprintf(stdout,"Running stereo calibration ...\n");
Mat cameraMatrix[2], distCoeffs[2];
Mat E, F;
if(this->Kleft.empty() || this->Kright.empty())
{
double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
this->Kleft, this->DistL,
this->Kright, this->DistR,
imageSize, this->R, this->T, E, F,
TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),
CV_CALIB_FIX_ASPECT_RATIO +
CV_CALIB_ZERO_TANGENT_DIST +
CV_CALIB_SAME_FOCAL_LENGTH +
CV_CALIB_FIX_K3);
fprintf(stdout,"done with RMS error= %f\n",rms);
} else
{
double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
this->Kleft, this->DistL,
this->Kright, this->DistR,
imageSize, this->R, this->T, E, F,
TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_FIX_INTRINSIC + CV_CALIB_FIX_K3);
fprintf(stdout,"done with RMS error= %f\n",rms);
}
// CALIBRATION QUALITY CHECK
cameraMatrix[0] = this->Kleft;
cameraMatrix[1] = this->Kright;
distCoeffs[0]=this->DistL;
distCoeffs[1]=this->DistR;
Mat R, T;
T=this->T;
R=this->R;
double err = 0;
int npoints = 0;
std::vector<Vec3f> lines[2];
for( i = 0; i < nimages; i++ )
{
int npt = (int)imagePoints[0][i].size();
Mat imgpt[2];
for( k = 0; k < 2; k++ )
{
imgpt[k] = Mat(imagePoints[k][i]);
undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
}
for( j = 0; j < npt; j++ )
{
double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
fabs(imagePoints[1][i][j].x*lines[0][j][0] +
imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
err += errij;
}
npoints += npt;
}
fprintf(stdout,"average reprojection err = %f\n",err/npoints);
cout.flush();
}
void Viewer::on_bt_intrinsic()
{
tvStatus->get_buffer()->set_text("Getting intrinsics ...");
Size boardSize, imageSize;
Mat cameraMatrix, distCoeffs;
vector<vector<Point2f> > imagePoints;
// TODO: get this data from user by GUI
boardSize.width = 9;
boardSize.height= 6;
//flag
int flag = 0;
flag |= CV_CALIB_FIX_PRINCIPAL_POINT;
flag |= CV_CALIB_ZERO_TANGENT_DIST;
flag |= CV_CALIB_FIX_ASPECT_RATIO;
if ( !boost::filesystem::exists(pathImage))
{
std::cerr << "[E] Images calibration directory doesn't exist: " << pathImage << std::endl;
return;
}
// List of images
boost::filesystem::directory_iterator end_itr;
for ( boost::filesystem::directory_iterator itr( pathImage ); itr != end_itr; ++itr )
{
if ( itr->path().generic_string().find("jpg") == std::string::npos )
continue;
std::cout << itr->path().generic_string() << std::endl;
Mat view;
view = imread(itr->path().c_str(), CV_LOAD_IMAGE_COLOR);
imageSize = view.size();
vector<Point2f> pointBuf;
bool found = findChessboardCorners( view, boardSize, pointBuf,
CV_CALIB_CB_ADAPTIVE_THRESH |
CV_CALIB_CB_FAST_CHECK |
CV_CALIB_CB_NORMALIZE_IMAGE);
if (found)
{
Mat viewGray;
cvtColor(view, viewGray, CV_BGR2GRAY);
cornerSubPix( viewGray, pointBuf, Size(11,11),
Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
imagePoints.push_back(pointBuf);
// Draw the corners.
drawChessboardCorners( view, boardSize, Mat(pointBuf), found );
//imshow("Image View", view);
}
}
mCalibration->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,
cameraMatrix, distCoeffs, imagePoints);
std::cout << std::endl << cameraMatrix << std::endl;
std::stringstream matrixStr;
matrixStr << "Intrinsic Matrix" << std::endl;
matrixStr << "================" << std::endl << std::endl;
for (int i=0; i<cameraMatrix.cols ; i++)
{
for (int j=0; j<cameraMatrix.rows; j++)
{
matrixStr << cameraMatrix.at<double>(i,j) << "\t\t";
if (i==2 && j==0)
matrixStr << "\t";
}
matrixStr<<std::endl;
tvStatus->get_buffer()->set_text(matrixStr.str().c_str());
}
}
void stereoCalibThread::monoCalibRun()
{
while(imagePortInLeft.getInputCount()==0 && imagePortInRight.getInputCount()==0)
{
fprintf(stdout, "Connect one camera.. \n");
Time::delay(1.0);
if(isStopping())
return;
}
bool left= imagePortInLeft.getInputCount()>0?true:false;
string cameraName;
if(left)
cameraName="LEFT";
else
cameraName="RIGHT";
fprintf(stdout, "CALIBRATING %s CAMERA \n",cameraName.c_str());
int count=1;
Size boardSize, imageSize;
boardSize.width=this->boardWidth;
boardSize.height=this->boardHeight;
while (!isStopping()) {
if(left)
imageL = imagePortInLeft.read(false);
else
imageL = imagePortInRight.read(false);
if(imageL!=NULL){
bool foundL=false;
mutex->wait();
if(startCalibration>0) {
string pathImg=imageDir;
preparePath(pathImg.c_str(), pathL,pathR,count);
string iml(pathL);
imgL= (IplImage*) imageL->getIplImage();
Mat Left(imgL);
std::vector<Point2f> pointbufL;
if(boardType == "CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else {
foundL = findChessboardCorners(Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
}
if(foundL) {
cvCvtColor(imgL,imgL,CV_RGB2BGR);
saveImage(pathImg.c_str(),imgL,count);
imageListL.push_back(iml);
Mat cL(pointbufL);
drawChessboardCorners(Left, boardSize, cL, foundL);
count++;
}
if(count>numOfPairs) {
fprintf(stdout," Running %s Camera Calibration... \n", cameraName.c_str());
monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);
fprintf(stdout," Saving Calibration Results... \n");
if(left)
updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_LEFT");
else
updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_RIGHT");
fprintf(stdout, "Calibration Results Saved in %s \n", camCalibFile.c_str());
startCalibration=0;
count=1;
imageListL.clear();
}
}
mutex->post();
ImageOf<PixelRgb>& outimL=outPortLeft.prepare();
outimL=*imageL;
outPortLeft.write();
ImageOf<PixelRgb>& outimR=outPortRight.prepare();
outimR=*imageL;
outPortRight.write();
if(foundL && startCalibration==1)
Time::delay(2.0);
cout.flush();
}
}
}
void stereoCalibThread::stereoCalibRun()
{
imageL=new ImageOf<PixelRgb>;
imageR=new ImageOf<PixelRgb>;
Stamp TSLeft;
Stamp TSRight;
bool initL=false;
bool initR=false;
int count=1;
Size boardSize, imageSize;
boardSize.width=this->boardWidth;
boardSize.height=this->boardHeight;
while (!isStopping()) {
ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(false);
ImageOf<PixelRgb> *tmpR = imagePortInRight.read(false);
if(tmpL!=NULL)
{
*imageL=*tmpL;
imagePortInLeft.getEnvelope(TSLeft);
initL=true;
}
if(tmpR!=NULL)
{
*imageR=*tmpR;
imagePortInRight.getEnvelope(TSRight);
initR=true;
}
if(initL && initR && checkTS(TSLeft.getTime(),TSRight.getTime())){
bool foundL=false;
bool foundR=false;
mutex->wait();
if(startCalibration>0) {
string pathImg=imageDir;
preparePath(pathImg.c_str(), pathL,pathR,count);
string iml(pathL);
string imr(pathR);
imgL= (IplImage*) imageL->getIplImage();
imgR= (IplImage*) imageR->getIplImage();
Mat Left(imgL);
Mat Right(imgR);
std::vector<Point2f> pointbufL;
std::vector<Point2f> pointbufR;
if(boardType == "CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else {
foundL = findChessboardCorners( Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
foundR = findChessboardCorners( Right, boardSize, pointbufR, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
}
if(foundL && foundR) {
cvCvtColor(imgL,imgL,CV_RGB2BGR);
cvCvtColor(imgR,imgR, CV_RGB2BGR);
saveStereoImage(pathImg.c_str(),imgL,imgR,count);
imageListR.push_back(imr);
imageListL.push_back(iml);
imageListLR.push_back(iml);
imageListLR.push_back(imr);
Mat cL(pointbufL);
Mat cR(pointbufR);
drawChessboardCorners(Left, boardSize, cL, foundL);
drawChessboardCorners(Right, boardSize, cR, foundR);
count++;
}
if(count>numOfPairs) {
fprintf(stdout," Running Left Camera Calibration... \n");
monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);
fprintf(stdout," Running Right Camera Calibration... \n");
monoCalibration(imageListR,this->boardWidth,this->boardHeight,this->Kright,this->DistR);
stereoCalibration(imageListLR, this->boardWidth,this->boardHeight,this->squareSize);
fprintf(stdout," Saving Calibration Results... \n");
updateIntrinsics(imgL->width,imgL->height,Kright.at<double>(0,0),Kright.at<double>(1,1),Kright.at<double>(0,2),Kright.at<double>(1,2),DistR.at<double>(0,0),DistR.at<double>(0,1),DistR.at<double>(0,2),DistR.at<double>(0,3),"CAMERA_CALIBRATION_RIGHT");
updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_LEFT");
Mat Rot=Mat::eye(3,3,CV_64FC1);
Mat Tr=Mat::zeros(3,1,CV_64FC1);
updateExtrinsics(this->R,this->T,"STEREO_DISPARITY");
fprintf(stdout, "Calibration Results Saved in %s \n", camCalibFile.c_str());
startCalibration=0;
count=1;
imageListR.clear();
imageListL.clear();
imageListLR.clear();
}
}
mutex->post();
ImageOf<PixelRgb>& outimL=outPortLeft.prepare();
outimL=*imageL;
outPortLeft.write();
ImageOf<PixelRgb>& outimR=outPortRight.prepare();
outimR=*imageR;
outPortRight.write();
if(foundL && foundR && startCalibration==1)
Time::delay(2.0);
initL=initR=false;
cout.flush();
}
}
delete imageL;
delete imageR;
}
/// it has multithread support under c++ 11
void CameraCalibration::getImagesAndFindPatterns(const string &cameraName)
{
// set mode
mode = CAPTURING;
frameCounter = 0;
imageCounter = 0;
capturedFrame currentImage;
// read current path
currentPath = boost::filesystem::current_path();
resultsPath = currentPath;
cout << "current Results Path" << currentPath << '\n' << endl;
//mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;// check enum type
// Capture only the frames settled in the configuration file
// in the original code it was a endless loop
for (int i = 0;; ++i)
{
Mat view,currentView;
bool blinkOutput = false;
// capture the image
view = s.nextImage();
frameCounter = frameCounter + 1;
//------------------------- Show original distorted image -----------------------------------
Mat originalView = view.clone();
//imshow("original Image", originalView);
//----- If no more image, or got enough, then stop calibration and show result -------------
if (mode == CAPTURING && imagePoints.size() >= (unsigned)s.nrFrames)
{
if (runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints))
mode = CALIBRATED;
else
mode = DETECTION;
}
if (view.empty()) // If no more images then run calibration, save and stop loop.
{
if (imagePoints.size() > 0)
runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints);
break;
}
imageSize = view.size(); // Format input image.
if (s.flipVertical) flip(view, view, 0);
vector<Point2f> pointBuf;
bool found;
switch (s.calibrationPattern) // Find feature points on the input format
{
case Settings::CHESSBOARD:
found = findChessboardCorners(view, s.boardSize, pointBuf,
CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
break;
case Settings::CIRCLES_GRID:
found = findCirclesGrid(view, s.boardSize, pointBuf);
break;
case Settings::ASYMMETRIC_CIRCLES_GRID:
found = findCirclesGrid(view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID);
break;
default:
found = false;
break;
}
if (found) // If done with success,
{
// improve the found corners' coordinate accuracy for chessboard
if (s.calibrationPattern == Settings::CHESSBOARD)
{
Mat viewGray;
cvtColor(view, viewGray, COLOR_BGR2GRAY);
cornerSubPix(viewGray, pointBuf, Size(11, 11),
Size(-1, -1), TermCriteria(TermCriteria::EPS+TermCriteria::MAX_ITER, 30, 0.1));
}
if (mode == CAPTURING && // For camera only take new samples after delay time
(!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC))
{
imagePoints.push_back(pointBuf);
prevTimestamp = clock();
blinkOutput = s.inputCapture.isOpened();
circlePoints = pointBuf; // save ordered circle points
}
// Draw the corners with ID according order criteria distance(x+y) from origin point 1
savedImage = view.clone();
drawChessboardCorners(view, s.boardSize, Mat(pointBuf), found);
// order the points according to d(x+y) from the upper left corner that is used as the origin frame
std::sort(pointBuf.begin(), pointBuf.end(), [](const cv::Point2f &a, const cv::Point2f &b)
{return ((a.x + a.y) < (b.x + b.y)); });
int pointCounter = 1;
for (auto k:pointBuf){
cv::putText(view,std::to_string(pointCounter),cv::Point(k.x,k.y),cv::FONT_HERSHEY_PLAIN,1.0,cv::Scalar(255,0,0),1);
pointCounter = pointCounter + 1;
}
}
//----------------------------- Output Text ------------------------------------------------
string msg = (mode == CAPTURING) ? "100/100" :
mode == CALIBRATED ? "Calibrated" : "the images are being captured";
int baseLine = 0;
Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
Point textOrigin(view.cols - 2 * textSize.width - 10, view.rows - 2 * baseLine - 10);
if (mode == CAPTURING)
{
if (s.showUndistorsed)
msg = format("%d/%d Undist", (int)imagePoints.size(), s.nrFrames);
else
msg = format("%d/%d", (int)imagePoints.size(), s.nrFrames);
}
putText(view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED);
if (blinkOutput){
bitwise_not(view, view);
// save the image used for calibration to disk
imageCounter = imageCounter + 1;
string pathToFile;
string filename;
if (imageCounter <= s.nrFrames)
{
string imageName{ "Image" + string(std::to_string(imageCounter)) + cameraName + ".jpg" };
boost::filesystem::path p{ "/" }; // add a slash to generate a portable string
filename.assign(resultsPath.string() + p.generic_string() + imageName);
vector<int> compression_params;
compression_params.push_back(IMWRITE_JPEG_QUALITY);
compression_params.push_back(100);
// check if the file already exist? if yes, erase it
bool found = getPathForThisFile(imageName,pathToFile);
if (found){
boost::filesystem::remove(pathToFile);
}
// write the new version of the file
cv::imwrite(filename, savedImage);
// save the points use to estimate the scale factor
int currentBufferPosition = imagePoints.size();
int pointCounter = 1;
circlePatternInfo currentCircle;
circlesDataPerImage dataCurrentImage;
vector<circlePatternInfo> circlesInfoFromThisImage;
for (auto k : circlePoints){
currentCircle.circleID = pointCounter;
currentCircle.circlePosition = cv::Point2f(k.x, k.y);
currentCircle.circle3DPosition = circle_Mis_Positions.at(pointCounter - 1);
circlesInfoFromThisImage.push_back(currentCircle);
pointCounter = pointCounter + 1;
}
circlePoints.clear();
dataCurrentImage.imageID= imageCounter;
dataCurrentImage.cameraID = s.cameraID;
dataCurrentImage.circlesData = circlesInfoFromThisImage;
// save all the data from the asymetric circles pattern
DataFromCirclesPattern.push_back(dataCurrentImage);
}
}
//------------------------- Video capture output undistorted ------------------------------
if (mode == CALIBRATED && s.showUndistorsed)
{
Mat temp = view.clone();
undistort(temp, view, cameraMatrix, distCoeffs);
string msgEsckey = "Press 'esc' key to quit";
putText(view, msgEsckey, textOrigin, 1, 1, GREEN, 2);
}
//------------------------------ Show image and check for input commands -------------------
imshow(cameraName, view);
//delay 30ms so that screen can refresh.
//image will not appear without this waitKey() command
cv::waitKey(30);
char c = waitKey(1);
if (c == ESC_KEY) // Escape key
break; // Breaks the capture loop
}
}
void StereoCalib(const vector<string>& imagelist, Size boardSize, bool useCalibrated=true, bool showRectified=true)
{
if( imagelist.size() % 2 != 0 )
{
cout << "Error: the image list contains odd (non-even) number of elements\n";
return;
}
printf("board size: %d x %d", boardSize.width, boardSize.height);
bool displayCorners = true;
const int maxScale = 2;
const float squareSize = 1.f; // Set this to your actual square size
// ARRAY AND VECTOR STORAGE:
vector<vector<Point2f> > imagePoints[2];
vector<vector<Point3f> > objectPoints;
Size imageSize;
int i, j, k, nimages = (int)imagelist.size()/2;
imagePoints[0].resize(nimages);
imagePoints[1].resize(nimages);
vector<string> goodImageList;
for( i = j = 0; i < nimages; i++ )
{
for( k = 0; k < 2; k++ )
{
const string& filename = imagelist[i*2+k];
Mat img = imread(filename, 0);
if(img.empty())
break;
if( imageSize == Size() )
imageSize = img.size();
else if( img.size() != imageSize )
{
cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n";
break;
}
bool found = false;
vector<Point2f>& corners = imagePoints[k][j];
for( int scale = 1; scale <= maxScale; scale++ )
{
Mat timg;
if( scale == 1 )
timg = img;
else
resize(img, timg, Size(), scale, scale);
found = findChessboardCorners(timg, boardSize, corners,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
if( found )
{
if( scale > 1 )
{
Mat cornersMat(corners);
cornersMat *= 1./scale;
}
break;
}
}
if( displayCorners )
{
cout << filename << endl;
Mat cimg, cimg1;
cvtColor(img, cimg, CV_GRAY2BGR);
drawChessboardCorners(cimg, boardSize, corners, found);
double sf = 640./MAX(img.rows, img.cols);
resize(cimg, cimg1, Size(), sf, sf);
imshow("corners", cimg1);
char c = (char)waitKey(500);
if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
exit(-1);
}
else
putchar('.');
if( !found )
break;
cornerSubPix(img, corners, Size(11,11), Size(-1,-1),
TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,
30, 0.01));
}
if( k == 2 )
{
goodImageList.push_back(imagelist[i*2]);
goodImageList.push_back(imagelist[i*2+1]);
j++;
}
}
cout << j << " pairs have been successfully detected.\n";
nimages = j;
if( nimages < 2 )
{
cout << "Error: too little pairs to run the calibration\n";
return;
}
imagePoints[0].resize(nimages);
imagePoints[1].resize(nimages);
objectPoints.resize(nimages);
for( i = 0; i < nimages; i++ )
{
for( j = 0; j < boardSize.height; j++ )
for( k = 0; k < boardSize.width; k++ )
objectPoints[i].push_back(Point3f(j*squareSize, k*squareSize, 0));
}
cout << "Running stereo calibration ...\n";
Mat cameraMatrix[2], distCoeffs[2];
cameraMatrix[0] = Mat::eye(3, 3, CV_64F);
cameraMatrix[1] = Mat::eye(3, 3, CV_64F);
Mat R, T, E, F;
double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
cameraMatrix[0], distCoeffs[0],
cameraMatrix[1], distCoeffs[1],
imageSize, R, T, E, F,
TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),
CV_CALIB_FIX_ASPECT_RATIO +
CV_CALIB_ZERO_TANGENT_DIST +
//CV_CALIB_SAME_FOCAL_LENGTH +
CV_CALIB_RATIONAL_MODEL +
CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5);
cout << "done with RMS error=" << rms << endl;
// CALIBRATION QUALITY CHECK
// because the output fundamental matrix implicitly
// includes all the output information,
// we can check the quality of calibration using the
// epipolar geometry constraint: m2^t*F*m1=0
double err = 0;
int npoints = 0;
vector<Vec3f> lines[2];
for( i = 0; i < nimages; i++ )
{
int npt = (int)imagePoints[0][i].size();
Mat imgpt[2];
for( k = 0; k < 2; k++ )
{
imgpt[k] = Mat(imagePoints[k][i]);
undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
}
for( j = 0; j < npt; j++ )
{
double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
fabs(imagePoints[1][i][j].x*lines[0][j][0] +
imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
err += errij;
}
npoints += npt;
}
cout << "average reprojection err = " << err/npoints << endl;
// save intrinsic parameters
FileStorage fs("calib/intrinsics.yml", CV_STORAGE_WRITE);
if( fs.isOpened() )
{
fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
"M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
fs.release();
}
else
cout << "Error: can not save the intrinsic parameters\n";
Mat R1, R2, P1, P2, Q;
Rect validRoi[2];
stereoRectify(cameraMatrix[0], distCoeffs[0],
cameraMatrix[1], distCoeffs[1],
imageSize, R, T, R1, R2, P1, P2, Q,
CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]);
fs.open("calib/extrinsics.yml", CV_STORAGE_WRITE);
if( fs.isOpened() )
{
fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q;
fs.release();
}
else
cout << "Error: can not save the intrinsic parameters\n";
// OpenCV can handle left-right
// or up-down camera arrangements
bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));
// COMPUTE AND DISPLAY RECTIFICATION
if( !showRectified )
return;
Mat rmap[2][2];
// IF BY CALIBRATED (BOUGUET'S METHOD)
if( useCalibrated )
{
// we already computed everything
}
// OR ELSE HARTLEY'S METHOD
else
// use intrinsic parameters of each camera, but
// compute the rectification transformation directly
// from the fundamental matrix
{
vector<Point2f> allimgpt[2];
for( k = 0; k < 2; k++ )
{
for( i = 0; i < nimages; i++ )
std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k]));
}
F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0);
Mat H1, H2;
stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3);
R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0];
R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1];
P1 = cameraMatrix[0];
P2 = cameraMatrix[1];
}
//Precompute maps for cv::remap()
initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);
Mat canvas;
double sf;
int w, h;
if( !isVerticalStereo )
{
sf = 600./MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width*sf);
h = cvRound(imageSize.height*sf);
canvas.create(h, w*2, CV_8UC3);
}
else
{
sf = 300./MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width*sf);
h = cvRound(imageSize.height*sf);
canvas.create(h*2, w, CV_8UC3);
}
for( i = 0; i < nimages; i++ )
{
for( k = 0; k < 2; k++ )
{
Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg;
remap(img, rimg, rmap[k][0], rmap[k][1], CV_INTER_LINEAR);
cvtColor(rimg, cimg, CV_GRAY2BGR);
Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h));
resize(cimg, canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA);
if( useCalibrated )
{
Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf),
cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf));
rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8);
}
}
if( !isVerticalStereo )
for( j = 0; j < canvas.rows; j += 16 )
line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
else
for( j = 0; j < canvas.cols; j += 16 )
line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8);
imshow("rectified", canvas);
char c = (char)waitKey();
if( c == 27 || c == 'q' || c == 'Q' )
break;
}
}
void CalibrateThread::run()
{
Size boardSize, imageSize;
float squareSize = 1.f, aspectRatio = 1.f;
Mat cameraMatrix, distCoeffs;
//QString of = ui->lineEdit_WorkFolder->text() + '/' + ui->lineEdit_OutputName->text();
QByteArray ba = strFileName.toLatin1();
const char* outputFilename = ba.data();
int i, nframes = 0;
bool writeExtrinsics = true, writePoints = true;
bool undistortImage = false;
int flags = 0;
VideoCapture capture;
bool flipVertical = false;
bool showUndistorted = false;
int delay = 1000;
clock_t prevTimestamp = 0;
int mode = CAPTURING;
vector<vector<Point2f> > imagePoints;
vector<string> imageList;
Pattern pattern = CHESSBOARD;
boardSize.width = m_width;
boardSize.height = m_height;
squareSize = m_squaresize;
//ui->textEdit_Information->append("\nCalibrating... Please wait for a while\n");
if( imgList.size() == 0 )
{
//QMessageBox::warning(NULL, "Error", "Please choose a right folder");
emit popupErrorInformation("Please choose a right folder");
emit closeImageWindow();
return;
}
else
{
nframes = imgList.size();
}
emit appendText("\nCalibrating... Please wait for a while\n");
//namedWindow( "Image View", 1 );
//bDialog->show();
for(i = 0; i < nframes ;i++)
{
//ui->textEdit_Information->append("Processing the image No. " + QString::number(i + 1));
emit appendText("Processing the image No. " + QString::number(i + 1));
Mat view, viewGray;
bool blink = false;
qDebug(imgList.at(i).toLatin1().data());
if( i < (int)imgList.size() )
view = imread(imgList.at(i).toLatin1().data(), 1);
if(!view.data)
{
//QMessageBox::warning(NULL, "Error", );
emit popupErrorInformation("Could not open image files");
return;
}
imageSize = view.size();
if( flipVertical )
flip( view, view, 0 );
vector<Point2f> pointbuf;
cvtColor(view, viewGray, CV_BGR2GRAY);
bool found;
switch( pattern )
{
case CHESSBOARD:
found = findChessboardCorners( view, boardSize, pointbuf,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
break;
case CIRCLES_GRID:
found = findCirclesGrid( view, boardSize, pointbuf );
break;
case ASYMMETRIC_CIRCLES_GRID:
found = findCirclesGrid( view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID );
break;
}
// improve the found corners' coordinate accuracy
if( pattern == CHESSBOARD && found) cornerSubPix( viewGray, pointbuf, Size(11,11),
Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
if( mode == CAPTURING && found &&
(!capture.isOpened() || clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) )
{
imagePoints.push_back(pointbuf);
prevTimestamp = clock();
blink = capture.isOpened();
}
if(found)
drawChessboardCorners( view, boardSize, Mat(pointbuf), found );
string msg = mode == CAPTURING ? "100/100" :
mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
int baseLine = 0;
Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10);
if( mode == CAPTURING )
{
if(undistortImage)
msg = format( "%d/%d Undist", (int)imagePoints.size(), nframes );
else
msg = format( "%d/%d", (int)imagePoints.size(), nframes );
}
putText( view, msg, textOrigin, 1, 1,
mode != CALIBRATED ? Scalar(0,0,255) : Scalar(0,255,0));
if( blink )
bitwise_not(view, view);
if( mode == CALIBRATED && undistortImage )
{
Mat temp = view.clone();
undistort(temp, view, cameraMatrix, distCoeffs);
}
Mat rgb;
cvtColor(view, rgb, CV_BGR2RGB);
QImage image32 = QImage(rgb.cols, rgb.rows, QImage::Format_RGB32);
QRgb value;
for(int r = 0; r < rgb.rows; r++)
{
for(int c = 0; c < rgb.cols; c++)
{
value = qRgb(rgb.ptr<uchar>(0)[r * rgb.cols * 3 + c * 3 + 0], rgb.ptr<uchar>(0)[r * rgb.cols * 3 + c * 3 + 1], rgb.ptr<uchar>(0)[r * rgb.cols * 3 + c * 3 + 2]);
image32.setPixel(c, r, value);
}
}
emit showBitmap(image32);
int key;
if(i < nframes - 1)
{
key = 0xff & waitKey(500);
}
else
{
key = waitKey(500);
}
if( (key & 255) == 27 )
break;
if( key == 'u' && mode == CALIBRATED )
undistortImage = !undistortImage;
}
if(imagePoints.size() > 0)
{
emit appendText("\n" + QString::number(imagePoints.size()) + " out of " + QString::number(nframes) + " images are effective!\n" );
runAndSave(outputFilename, imagePoints, imageSize,
boardSize, pattern, squareSize, aspectRatio,
flags, cameraMatrix, distCoeffs,
writeExtrinsics, writePoints);
}
else
{
emit appendText("Calibrating is not successful! \nPlease change the parameters and try again!");
emit popupErrorInformation("Sorry, no enough points are detected! Please try another group of images!");
emit closeImageWindow();
return;
}
emit appendText("Calibrating Successfully! \nPlease go to the folder to check the out put files!");
emit closeImageWindow();
if( !capture.isOpened() && showUndistorted )
{
Mat view, rview, map1, map2;
initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
imageSize, CV_16SC2, map1, map2);
for( i = 0; i < (int)imageList.size(); i++ )
{
view = imread(imageList[i], 1);
if(!view.data)
continue;
//undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix );
remap(view, rview, map1, map2, INTER_LINEAR);
imshow("Image View", rview);
int c = 0xff & waitKey();
if( (c & 255) == 27 || c == 'q' || c == 'Q' )
break;
}
}
return;
}
void CameraCalibration::StereoCalibration()
{
vector<vector<Point2f> > ImagePoints[2];
vector<vector<Point3f> > ObjectPoints(1);
for(int i=0; i<BoardSize.height; i++)
for(int j=0; j<BoardSize.width; j++)
ObjectPoints.at(0).push_back(Point3f(float( j*SquareSize ),
float( i*SquareSize ),
0));
ObjectPoints.resize(NumFrames, ObjectPoints[0]);
vector<Mat> RVecs[2], TVecs[2];
double rms;
for(int c_idx=0; c_idx<2; c_idx++)
{
for(int i=0; i < NumFrames; i++)
{
Mat img = imread(data_path+"/"+calib_params[c_idx].ImageList.at(i), CV_LOAD_IMAGE_COLOR);
vector<Point2f> pointBuf;
bool found = false;
found = findChessboardCorners(img, BoardSize, pointBuf,
CV_CALIB_CB_ADAPTIVE_THRESH |
CV_CALIB_CB_NORMALIZE_IMAGE);
if(found)
{
Mat viewGray;
cvtColor(img, viewGray, CV_BGR2GRAY);
cornerSubPix(viewGray, pointBuf, Size(11, 11), Size(-1, -1),
TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.01));
//drawChessboardCorners(img, BoardSize, Mat(pointBuf), found);
//namedWindow("Image View", CV_WINDOW_AUTOSIZE);
//imshow("Image View", img);
//waitKey();
}
else
{
cerr << i << "th image cannot be found a pattern." << endl;
exit(EXIT_FAILURE);
}
ImagePoints[c_idx].push_back(pointBuf);
}
calib_params[c_idx].DistCoeffs = Mat::zeros(8, 1, CV_64F);
calib_params[c_idx].CameraMatrix = initCameraMatrix2D(ObjectPoints, ImagePoints[c_idx], ImageSize, 0);
rms = calibrateCamera(ObjectPoints,
ImagePoints[c_idx],
ImageSize,
calib_params[c_idx].CameraMatrix,
calib_params[c_idx].DistCoeffs,
RVecs[c_idx],
TVecs[c_idx],
CV_CALIB_USE_INTRINSIC_GUESS |
CV_CALIB_FIX_K3 |
CV_CALIB_FIX_K4 |
CV_CALIB_FIX_K5 |
CV_CALIB_FIX_K6);
cout << c_idx << " camera re-projection error reported by calibrateCamera: "<< rms << endl;
}
rms = stereoCalibrate(ObjectPoints,
ImagePoints[0],
ImagePoints[1],
calib_params[0].CameraMatrix,
calib_params[0].DistCoeffs,
calib_params[1].CameraMatrix,
calib_params[1].DistCoeffs,
ImageSize,
stereo_params->R,
stereo_params->T,
stereo_params->E,
stereo_params->F,
TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5));
cout << "Stereo re-projection error reported by stereoCalibrate: " << rms << endl;
cout << "Fundamental Matrix reprojection error: " << FundamentalMatrixQuality(ImagePoints[0], ImagePoints[1],
calib_params[0].CameraMatrix, calib_params[1].CameraMatrix,
calib_params[0].DistCoeffs, calib_params[1].DistCoeffs, stereo_params->F) << endl;
// Transfer matrix from OpenCV Mat to Pangolin matrix
CvtCameraExtrins(RVecs, TVecs);
Timer PangolinTimer;
// Stereo rectification
stereoRectify(calib_params[0].CameraMatrix,
calib_params[0].DistCoeffs,
calib_params[1].CameraMatrix,
calib_params[1].DistCoeffs,
ImageSize,
stereo_params->R,
stereo_params->T,
rect_params->LeftRot,
rect_params->RightRot,
rect_params->LeftProj,
rect_params->RightProj,
rect_params->Disp2DepthReProjMat,
CALIB_ZERO_DISPARITY, // test later
1, // test later
ImageSize,
&rect_params->LeftRoi,
&rect_params->RightRoi);
cout << "\nStereo rectification using calibration spent: " << PangolinTimer.getElapsedTimeInMilliSec() << "ms." << endl;
rect_params->isVerticalStereo = fabs(rect_params->RightProj.at<double>(1, 3)) >
fabs(rect_params->RightProj.at<double>(0, 3));
// Get the rectification re-map index
initUndistortRectifyMap(calib_params[0].CameraMatrix, calib_params[0].DistCoeffs,
rect_params->LeftRot, rect_params->LeftProj,
ImageSize, CV_16SC2, rect_params->LeftRMAP[0], rect_params->LeftRMAP[1]);
initUndistortRectifyMap(calib_params[1].CameraMatrix, calib_params[1].DistCoeffs,
rect_params->RightRot, rect_params->RightProj,
ImageSize, CV_16SC2, rect_params->RightRMAP[0], rect_params->RightRMAP[1]);
}
void CameraCalibration::MonoCalibration()
{
vector<vector<Point2f> > ImagePoints;
for(int i=0; i < NumFrames; i++)
{
Mat img = imread(calib_params[0].ImageList.at(i), CV_LOAD_IMAGE_COLOR);
vector<Point2f> pointBuf;
bool found = false;
found = findChessboardCorners(img, BoardSize, pointBuf,
CV_CALIB_CB_ADAPTIVE_THRESH |
CV_CALIB_CB_NORMALIZE_IMAGE);
if(found)
{
Mat viewGray;
cvtColor(img, viewGray, CV_BGR2GRAY);
cornerSubPix(viewGray, pointBuf, Size(11, 11), Size(-1, -1),
TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.01));
// drawChessboardCorners(img_left, calib_params.BoardSize, Mat(left_pointBuf), found_left);
// imshow("Left Image View", img_left);
// drawChessboardCorners(img_right, calib_params.BoardSize, Mat(right_pointBuf), found_right);
// imshow("Right Image View", img_right);
// waitKey();
}
else
{
cerr << i << "th image cannot be found a pattern." << endl;
exit(EXIT_FAILURE);
}
ImagePoints.push_back(pointBuf);
}
vector<vector<Point3f> > ObjectPoints(1);
for(int i=0; i<BoardSize.height; i++)
for(int j=0; j<BoardSize.width; j++)
ObjectPoints.at(0).push_back(Point3f(float( j*SquareSize ),
float( i*SquareSize ),
0));
ObjectPoints.resize(ImagePoints.size(), ObjectPoints[0]);
vector<Mat> RVecs, TVecs;
calib_params->DistCoeffs = Mat::zeros(8, 1, CV_64F);
calib_params->CameraMatrix = initCameraMatrix2D(ObjectPoints, ImagePoints, ImageSize, 0);
double rms = calibrateCamera(ObjectPoints,
ImagePoints,
ImageSize,
calib_params->CameraMatrix,
calib_params->DistCoeffs,
RVecs,
TVecs,
CV_CALIB_USE_INTRINSIC_GUESS |
CV_CALIB_FIX_K3 |
CV_CALIB_FIX_K4 |
CV_CALIB_FIX_K5 |
CV_CALIB_FIX_K6);
cout << "Camera re-projection error reported by calibrateCamera: "<< rms << endl;
// Transfer matrix from OpenCV Mat to Pangolin matrix
CvtCameraExtrins(&RVecs, &TVecs);
}
int main(int argc, char *argv[])
{
// parse configuration file
// get input arguments
OpenCvStereoConfig stereo_config;
string config_file = "";
ConciseArgs parser(argc, argv);
parser.add(config_file, "c", "config", "Configuration file containing camera GUIDs, etc.", true);
parser.add(left_camera_mode, "l", "left-camera", "Calibrate just the left camera.");
parser.add(right_camera_mode, "r", "right-camera", "Calibrate just the right camera.");
parser.add(force_brightness, "b", "brightness", "set brightness to this level");
parser.add(force_exposure, "e", "exposure", "set exposure to this level");
parser.parse();
// parse the config file
if (ParseConfigFile(config_file, &stereo_config) != true)
{
fprintf(stderr, "Failed to parse configuration file, quitting.\n");
return -1;
}
if (left_camera_mode || right_camera_mode)
{
stereo_mode = false;
}
uint64 guid = stereo_config.guidLeft;
uint64 guid2 = stereo_config.guidRight;
dc1394_t *d;
dc1394camera_t *camera;
dc1394error_t err;
Mat frame_array_left[MAX_FRAMES];
Mat frame_array_right[MAX_FRAMES];
int numFrames = 0;
// ----- cam 2 -----
dc1394_t *d2;
dc1394camera_t *camera2;
dc1394error_t err2;
d = dc1394_new ();
if (!d)
g_critical("Could not create dc1394 context");
d2 = dc1394_new ();
if (!d2)
g_critical("Could not create dc1394 context for camera 2");
camera = dc1394_camera_new (d, guid);
if (!camera)
g_critical("Could not create dc1394 camera");
camera2 = dc1394_camera_new (d2, guid2);
if (!camera2)
g_critical("Could not create dc1394 camera for camera 2");
// setup
err = setup_gray_capture(camera, DC1394_VIDEO_MODE_FORMAT7_1);
DC1394_ERR_CLN_RTN(err, cleanup_and_exit(camera), "Could not setup camera");
err2 = setup_gray_capture(camera2, DC1394_VIDEO_MODE_FORMAT7_1);
DC1394_ERR_CLN_RTN(err2, cleanup_and_exit(camera2), "Could not setup camera number 2");
// enable auto-exposure
// turn on the auto exposure feature
err = dc1394_feature_set_power(camera, DC1394_FEATURE_EXPOSURE, DC1394_ON);
DC1394_ERR_RTN(err,"Could not turn on the exposure feature");
err = dc1394_feature_set_mode(camera, DC1394_FEATURE_EXPOSURE, DC1394_FEATURE_MODE_ONE_PUSH_AUTO);
DC1394_ERR_RTN(err,"Could not turn on Auto-exposure");
// enable auto-exposure
// turn on the auto exposure feature
err = dc1394_feature_set_power(camera2, DC1394_FEATURE_EXPOSURE, DC1394_ON);
DC1394_ERR_RTN(err,"Could not turn on the exposure feature for cam2");
err = dc1394_feature_set_mode(camera2, DC1394_FEATURE_EXPOSURE, DC1394_FEATURE_MODE_ONE_PUSH_AUTO);
DC1394_ERR_RTN(err,"Could not turn on Auto-exposure for cam2");
// enable camera
err = dc1394_video_set_transmission(camera, DC1394_ON);
DC1394_ERR_CLN_RTN(err, cleanup_and_exit(camera), "Could not start camera iso transmission");
err2 = dc1394_video_set_transmission(camera2, DC1394_ON);
DC1394_ERR_CLN_RTN(err2, cleanup_and_exit(camera2), "Could not start camera iso transmission for camera number 2");
if (left_camera_mode || stereo_mode)
{
InitBrightnessSettings(camera, camera2);
MatchBrightnessSettings(camera, camera2, true, force_brightness, force_exposure);
} else {
// use the right camera as the master for brightness
// since we're calibrating that one
InitBrightnessSettings(camera2, camera);
MatchBrightnessSettings(camera2, camera, true);
}
// make opencv windows
if (left_camera_mode || stereo_mode)
{
namedWindow("Input Left", CV_WINDOW_AUTOSIZE);
moveWindow("Input Left", 100, 100);
}
if (right_camera_mode || stereo_mode)
{
namedWindow("Input Right", CV_WINDOW_AUTOSIZE);
moveWindow("Input Right", 478, 100);
}
CvSize size;
Mat cornersL, cornersR;
int i;
while (numFrames < MAX_FRAMES) {
Mat chessL, chessR;
// each loop dump a bunch of frames to clear the buffer
MatchBrightnessSettings(camera, camera2, true, force_brightness, force_exposure);
for (i=0;i<10;i++)
{
if (left_camera_mode || stereo_mode)
{
chessL = GetFrameFormat7(camera);
}
if (right_camera_mode || stereo_mode)
{
chessR = GetFrameFormat7(camera2);
}
}
// copy the images for drawing/display
size = chessL.size();
Mat chessLc;
chessLc.create(size, CV_32FC3);
Mat chessRc;
chessRc.create(size, CV_32FC3);
// attempt checkerboard matching
bool foundPattern = true; // set to true so we can do an OR
// later if we're only using one
// camera
if (left_camera_mode || stereo_mode)
{
foundPattern = findChessboardCorners(chessL, Size(CHESS_X, CHESS_Y), cornersL);
}
if (right_camera_mode || stereo_mode)
{
foundPattern = foundPattern & findChessboardCorners(chessR, Size(CHESS_X, CHESS_Y), cornersR);
}
if (left_camera_mode || stereo_mode)
{
cvtColor( chessL, chessLc, CV_GRAY2BGR );
drawChessboardCorners(chessLc, Size(CHESS_X, CHESS_Y), cornersL, foundPattern);
imshow("Input Left", chessLc);
}
if (right_camera_mode || stereo_mode)
{
cvtColor(chessR, chessRc, CV_GRAY2BGR);
drawChessboardCorners(chessRc, Size(CHESS_X, CHESS_Y), cornersR, foundPattern);
imshow("Input Right", chessRc);
}
// key codes:
// page up: 654365
// page down: 65366
// b: 98
char key = waitKey();
//printf("%d\n", (int)key);
if (key == 98)
{
break;
} else if (key == 86){
if (foundPattern)
{
// this was a good one -- save it
frame_array_left[numFrames] = chessL;
frame_array_right[numFrames] = chessR;
// give the user some guidence on the number
// of frames they should be using
if (stereo_mode)
{
printf("Saved frame %d / about 10\n", numFrames);
} else {
printf("Saved frame %d / about 20-30\n", numFrames);
}
numFrames ++;
} else {
printf("Not saving frame since did not find a checkboard.\n");
}
} else if (key == 'W') {
force_brightness +=20;
cout << "Brightness: " << force_brightness << "\n";
} else if (key == 'w') {
force_brightness -=20;
cout << "Brightness: " << force_brightness << "\n";
} else if (key == 'E') {
force_exposure +=20;
cout << "Exposure: " << force_exposure << "\n";
} else if (key == 'e') {
force_exposure -=20;
cout << "Exposure: " << force_exposure << "\n";
}
}
printf("\n\n");
// clear out the calibration directory
printf("Deleting old images...\nrm calibrationImages/*.ppm\n");
int retval = system("rm calibrationImages/*.ppm");
if (retval != 0) {
printf("Warning: Deleting images may have failed.\n");
}
printf("done.\n");
char filename[1000];
for (i=0;i<numFrames;i++)
{
if (left_camera_mode || stereo_mode)
{
sprintf(filename, "calibrationImages/cam1-%05d.ppm", i+1);
imwrite(filename, frame_array_left[i]);
}
if (right_camera_mode || stereo_mode)
{
sprintf(filename, "calibrationImages/cam2-%05d.ppm", i+1);
imwrite(filename, frame_array_right[i]);
}
printf("Writing frame %d\n", i);
}
printf("\n\n");
destroyWindow("Input Left");
destroyWindow("Input Right");
// stop data transmission
err = dc1394_video_set_transmission(camera, DC1394_OFF);
DC1394_ERR_CLN_RTN(err,cleanup_and_exit(camera),"Could not stop the camera");
err2 = dc1394_video_set_transmission(camera2, DC1394_OFF);
DC1394_ERR_CLN_RTN(err2,cleanup_and_exit(camera2),"Could not stop the camera 2");
// close camera
cleanup_and_exit(camera);
cleanup_and_exit(camera2);
dc1394_free (d);
dc1394_free (d2);
return 0;
}
void CameraCalibration::calibrate()
{
const string inputSettingsFile = "default.xml";
// Read the settings
FileStorage fs( inputSettingsFile, FileStorage::READ );
if ( !fs.isOpened() ) {
FileStorage fs( inputSettingsFile, FileStorage::WRITE );
fs.release();
cerr << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
return;
}
else {
s.read( fs["Settings"] );
// close Settings file
fs.release();
}
if ( !s.goodInput ) {
cerr << "Invalid input detected. Application stopping." << endl;
return;
}
vector<vector<Point2f> > imagePoints;
Mat distCoeffs;
Size imageSize;
int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
clock_t prevTimestamp = 0;
const Scalar RED( 0, 0, 255 ), GREEN( 0, 255, 0 );
const char ESC_KEY = 27;
for ( int i = 0; ; ++i ) {
Mat view;
bool blinkOutput = false;
view = s.nextImage();
//----- If no more image, or got enough, then stop calibration and show result -------------
if ( mode == CAPTURING && imagePoints.size() >= (unsigned)s.nrFrames ) {
if ( runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints ) ) {
mode = CALIBRATED;
}
else {
mode = DETECTION;
}
}
// If no more images then run calibration, save and stop loop.
if ( view.empty() ) {
if ( imagePoints.size() > 0 ) {
runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints);
}
break;
}
imageSize = view.size(); // Format input image.
if ( s.flipVertical ) {
flip( view, view, 0 );
}
vector<Point2f> pointBuf;
bool found;
// Find feature points on the input format
switch ( s.calibrationPattern ) {
case Settings::CHESSBOARD:
found = findChessboardCorners( view, s.boardSize, pointBuf,
CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
break;
case Settings::CIRCLES_GRID:
found = findCirclesGrid( view, s.boardSize, pointBuf );
break;
case Settings::ASYMMETRIC_CIRCLES_GRID:
found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID );
break;
default:
found = false;
break;
}
// If done with success, improve the found corners' coordinate accuracy for chessboard
if ( found ) {
if ( s.calibrationPattern == Settings::CHESSBOARD ) {
Mat viewGray;
cvtColor( view, viewGray, COLOR_BGR2GRAY );
cornerSubPix( viewGray, pointBuf, Size( 11,11 ), Size(-1,-1), TermCriteria( TermCriteria::EPS + TermCriteria::MAX_ITER, 30, 0.1 ) );
}
// For camera only take new samples after delay time
if ( mode == CAPTURING && (!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) ) {
imagePoints.push_back( pointBuf );
prevTimestamp = clock();
blinkOutput = s.inputCapture.isOpened();
}
// Draw the corners.
drawChessboardCorners( view, s.boardSize, Mat( pointBuf ), found );
}
//----------------------------- Output Text ------------------------------------------------
string msg = ( mode == CAPTURING ) ? "100/100" :
mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
int baseLine = 0;
Size textSize = getTextSize( msg, 1, 1, 1, &baseLine );
Point textOrigin( view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10 );
if ( mode == CAPTURING ) {
if ( s.showUndistorsed ) {
msg = format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames );
}
else {
msg = format( "%d/%d", (int)imagePoints.size(), s.nrFrames );
}
}
putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED );
if ( blinkOutput ) {
bitwise_not( view, view );
}
//------------------------- Video capture output undistorted ------------------------------
if ( mode == CALIBRATED && s.showUndistorsed ) {
Mat temp = view.clone();
undistort( temp, view, cameraMatrix, distCoeffs );
}
//------------------------------ Show image and check for input commands -------------------
imshow( "Image View", view );
char key = (char)waitKey( s.inputCapture.isOpened() ? 50 : s.delay );
if ( key == ESC_KEY ) {
break;
}
if ( key == 'u' && mode == CALIBRATED ) {
s.showUndistorsed = !s.showUndistorsed;
}
if ( s.inputCapture.isOpened() && key == 'g' ) {
mode = CAPTURING;
imagePoints.clear();
}
}
// -----------------------Show the undistorted image for the image list ------------------------
if ( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed ) {
Mat view, rview, map1, map2;
initUndistortRectifyMap( cameraMatrix, distCoeffs, Mat(),
getOptimalNewCameraMatrix( cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0 ),
imageSize, CV_16SC2, map1, map2 );
for ( int i = 0; i < (int)s.imageList.size(); i++ ) {
view = imread( s.imageList[i], 1 );
if ( view.empty() ) {
continue;
}
remap( view, rview, map1, map2, INTER_LINEAR );
imshow( "Image View", rview );
char c = (char)waitKey();
if ( c == ESC_KEY || c == 'q' || c == 'Q' ) {
break;
}
}
}
}
std::vector<Point> KinectCalibrator::findCalibrationSquare(Mat rgbMatrix){
Size chessboardSize(5,4);
bool test4 = findChessboardCorners(rgbMatrix, chessboardSize, _pointVector);
return _pointVector;
}
int main_file(Parameter *pParam)
{
char *file_list = pParam->image_list;//"/home/sean/Pictures/calib_test1/dir.txt";
Size boardSize(8,6);
Size imageSize;
int flags = CV_CALIB_FIX_ASPECT_RATIO;
float squareSize = pParam->square_size;
float aspectRatio = 1.f;
Mat cameraMatrix;
Mat distCoeffs;
int result = 0;
// read frames from data file
vector<vector<Point2f> > imagePointsSet;
vector<Point2f> imagePoints;
vector<string> fileNames;
fileNames.clear();
imagePointsSet.clear();
getFileName(file_list, fileNames);
for(unsigned int i = 0; i < fileNames.size(); i++)
{
Mat framecv;
int found = 0;
framecv = imread(fileNames[i].c_str(), 0);
if(framecv.cols <= 0 || framecv.rows <= 0 || framecv.data == NULL )
{
printf("finish chess board detection \n");
break;
}
imagePoints.clear();
imageSize.width = framecv.cols;
imageSize.height = framecv.rows;
found = findChessboardCorners(
framecv,
boardSize,
imagePoints,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
if(found)
{
cornerSubPix(
framecv,
imagePoints,
Size(11,11),
Size(-1,-1),
TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
if(found)
{
drawChessboardCorners( framecv, boardSize, Mat(imagePoints), found);
imshow("framecv_xx", framecv);
waitKey(10);
}
imagePointsSet.push_back(imagePoints);
}
}
// calibrate cameras
if(1)
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
result = runCalibration(imagePointsSet, imageSize, boardSize, CHESSBOARD, squareSize,
aspectRatio, flags, cameraMatrix, distCoeffs,
rvecs, tvecs, reprojErrs, totalAvgErr);
}
// test calibrate
if(1)
{
Mat view, rview, map1, map2;
int i;
Size imageSize2;
imageSize2.width = 2 * imageSize.width;
imageSize2.height = 2 * imageSize.height;
initUndistortRectifyMap(cameraMatrix,
distCoeffs,
Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
imageSize, CV_16SC2, map1, map2);
for(i = 0; i < fileNames.size(); i++)
{
view = imread(fileNames[i].c_str());
remap(view, rview, map1, map2, INTER_LINEAR);
imshow("rview", rview);
waitKey(0);
}
}
// save
if(result == 0 )
{
save_result(pParam->output_path, cameraMatrix, distCoeffs);
}
}
int main_camera(Parameter *pParam)
{
Size boardSize(8,6);
Size imageSize;
int flags = CV_CALIB_FIX_ASPECT_RATIO;
float squareSize = pParam->square_size;
float aspectRatio = 1.f;
Mat cameraMatrix;
Mat distCoeffs;
Mat frame;
VideoCapture video;
int flag_finish = 0;
int result = 0;
// read frames from data file
vector<vector<Point2f> > imagePointsSet;
vector<Point2f> imagePoints;
vector<string> fileNames;
fileNames.clear();
imagePointsSet.clear();
video.open(pParam->camera_index);
if(video.isOpened() != true)
{
printf("fail to open camera %d\n", pParam->camera_index);
video.open(-1);
if(video.isOpened() != true)
{
printf("fail to open the default camera, please make sure an accessible camera is connected \n");
return -1;
}
else
{
printf("open the default camera\n");
}
}
while(flag_finish == 0)
{
Mat framecv;
int found = 0;
video >> frame;
cvtColor(frame, framecv, CV_RGB2GRAY);
imshow("framecv", framecv); // for oberserving input
waitKey(10);
if(framecv.cols <= 0 || framecv.rows <= 0 || framecv.data == NULL )
{
printf("finish chess board detection \n");
break;
}
imagePoints.clear();
imageSize.width = framecv.cols;
imageSize.height = framecv.rows;
found = findChessboardCorners(
framecv,
boardSize,
imagePoints,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
if(found)
{
cornerSubPix(
framecv,
imagePoints,
Size(11,11),
Size(-1,-1),
TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
if(found)
{
char key = 0;
drawChessboardCorners( framecv, boardSize, Mat(imagePoints), found);
imshow("framecv_xx", framecv);
key = waitKey(0);
if(key == 'c' || key == 'C') // not correct
continue;
else if(key == 'q' || key == 'Q')
return 0;
else if(key == 's' || key == 'S')
flag_finish = 1;
}
printf("get a new chess board input\n");
imagePointsSet.push_back(imagePoints);
}
else
{
printf("no found usable chess board\n");
}
}
// calibrate cameras
if(1)
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
result = runCalibration(imagePointsSet, imageSize, boardSize, CHESSBOARD, squareSize,
aspectRatio, flags, cameraMatrix, distCoeffs,
rvecs, tvecs, reprojErrs, totalAvgErr);
}
// test calibrate
if(1)
{
Mat view, rview, map1, map2;
int i;
Size imageSize2;
imageSize2.width = 2 * imageSize.width;
imageSize2.height = 2 * imageSize.height;
initUndistortRectifyMap(cameraMatrix,
distCoeffs,
Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
imageSize, CV_16SC2, map1, map2);
while(1)
{
char key = 0;
video>>view;
remap(view, rview, map1, map2, INTER_LINEAR);
imshow("rview", rview);
key = waitKey(0);
if(key == 's')
break;
else if(key == 'q')
break;
}
}
/// Calculates the corner pixel locations as detected by each camera
/// In: s: board settings, includes size, square size and the number of corners
/// inputCapture1: video capture for camera 1
/// inputCapture2: video capture for camera 2
/// iterations: number of chessboard images to take
/// Out: imagePoints1: pixel coordinates of chessboard corners for camera 1
/// imagePoints2: pixel coordinates of chessboard corners for camera 2
bool RetrieveChessboardCorners(vector<vector<Point2f> >& imagePoints1, vector<vector<Point2f> >& imagePoints2, BoardSettings s, VideoCapture inputCapture1,VideoCapture inputCapture2, int iterations)
{
destroyAllWindows();
Mat image1,image2;
vector<Point2f> pointBuffer1;
vector<Point2f> pointBuffer2;
clock_t prevTimeStamp = 0;
bool found1,found2;
int count = 0;
while (count != iterations){
char c = waitKey(15);
if (c == 's'){
cerr << "Calibration stopped" << endl;
return false;
}
// try find chessboard corners
else if(c == 'c'){
// ADAPTIVE_THRESH -> use adaptive thresholding to convert image to B&W
// FAST_CHECK -> Terminates call earlier if no chessboard in image
// NORMALIZE_IMAGE -> normalize image gamma before thresholding
// FILTER_QUADS -> uses additional criteria to filter out false quads
found1 = findChessboardCorners(image1, s.boardSize, pointBuffer1,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK |
CV_CALIB_CB_NORMALIZE_IMAGE | CV_CALIB_CB_FILTER_QUADS);
found2 = findChessboardCorners(image2, s.boardSize, pointBuffer2,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK |
CV_CALIB_CB_NORMALIZE_IMAGE | CV_CALIB_CB_FILTER_QUADS);
if (found1 && found2 && (pointBuffer1.size() >= s.cornerNum) && (pointBuffer2.size() >= s.cornerNum)){
// if time delay passed refine accuracy and store
if ((clock() - prevTimeStamp) > CAPTURE_DELAY * 1e-3*CLOCKS_PER_SEC){
Mat imageGray1, imageGray2;
cvtColor(image1, imageGray1, COLOR_BGR2GRAY);
cvtColor(image2, imageGray2, COLOR_BGR2GRAY);
// refines corner locations
// Size(11,11) -> size of the search window
// Size(-1,-1) -> indicates no dead zone in search size
// TermCriteria -> max 1000 iteration, to get acuraccy of 0.01
cornerSubPix(imageGray1, pointBuffer1, Size(5,5), Size(-1, -1),
TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 1000, 0.01));
cornerSubPix(imageGray2, pointBuffer2, Size(5,5), Size(-1, -1),
TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 1000, 0.01));
drawChessboardCorners(image1, s.boardSize, Mat(pointBuffer1), found1);
imshow("Image View1", image1);
drawChessboardCorners(image2, s.boardSize, Mat(pointBuffer2), found2);
imshow("Image View2", image2);
// user verifies the correct corners have been found
c = waitKey(0);
if (c == 's'){
return false;
}
if (c == 'y'){
// store the points and store time stamp
imagePoints1.push_back(pointBuffer1);
imagePoints2.push_back(pointBuffer2);
prevTimeStamp = clock();
count++;
cerr << "Count: " << count << endl;
}
}
}
}
inputCapture1.read(image1);
inputCapture2.read(image2);
imshow("Image View1", image1);
imshow("Image View2", image2);
}
// found all corners
return true;
}