/**
* Prints out the board to standard out.
* For testing purposes.
*/
void printBoard() {
int i, j;
// fprintf(stderr, "From input:\n");
//
// for (i = 0; i < columns - PADDING; i++) {
// for (j = 0; j < rows - PADDING; j++) {
// printf("%c", pieces[getPiece(j,i)]);
// }
// }
// fprintf(stderr, "\nTop Pieces:\n");
// for (i = 0; i < columns - PADDING; i++) {
// printf("%d ", columnHeight[i]);
// }
// fprintf(stderr, "\n\n");
// for (i = rows - 1; i >= 0; i--) {
// for (j = 0; j < columns; j++) {
// printf("%c ", pieces[board[i + j * rows]]);
// }
// printf("\n");
// }
// printf("\n\n");
for (i = rows - 7; i >= 0; i--) {
for (j = 0; j < boardSize(columns); j++) {
fprintf(stderr, "%c ", pieces[getPiece(i,j)]);
}
fprintf(stderr, "\n");
}
fprintf(stderr, "\n\n");
}
Board::Board(String boardString) {
for(auto i_str = boardString.find(LL('\n')); i_str != String::npos; i_str = boardString.find(LL('\n'))) {
boardString.replace(i_str, 1, LL(""));
}
board = boardString;
size = boardSize();
xyl = LengthToXY(size);
}
void UiProto::on_btnChessboard_clicked()
{
SterioMats Smats(orginalL,orginalR);
Size boardSize(6,9);
Smats.Corners(boardSize);
QImage QorginalL = matToQImage(Smats.left); // convert original and Canny images to QImage
ui->picL->setPixmap(QPixmap::fromImage(QorginalL)); // show original and Canny images on labels
QImage QorginalR = matToQImage(Smats.right); // convert original and Canny images to QImage
ui->picR->setPixmap(QPixmap::fromImage(QorginalR)); // show original and Canny images on labels
}
void KBounceGameWidget::resizeEvent( QResizeEvent* ev )
{
kDebug() << "Size" << ev->size();
m_renderer.setBackgroundSize( ev->size() );
renderBackground();
QSize boardSize( ev->size().width() - 30, ev->size().height() - 30 );
m_board->resize( boardSize );
m_board->moveTo( ( ev->size().width() - boardSize.width() ) / 2, ( ev->size().height() - boardSize.height() ) / 2 );
redraw();
}
int main(){
// connect to the cameras
VideoCapture leftCam(1);
VideoCapture rightCam(2);
char response[4];
// if needed: Capture calibration images and save to disk
printf("Would you like to capture new calibration images? (y/n): ");
fgets(response, sizeof response, stdin);
bool recapture = (response[0] == 'y');
if (recapture) {
calibCapture( leftCam, rightCam );
}
// if needed: calibrate the cameras
printf("Would you like to recalibrate the cameras? (y/n): ");
fgets(response, sizeof response, stdin);
bool recalibrate = (response[0] == 'y');
if (recalibrate) {
vector<string> imageList;
bool read_ok = readStringList("calib/image_list.xml", imageList);
if(!read_ok || imageList.empty()) {
cout << "Failed to read calibration image list (calib/image_list.xml)" << endl;
return 1;
}
FileStorage fs("calib/image_list.xml", FileStorage::READ);
int boardSizeX = (int)fs["boardSizeX"];
int boardSizeY = (int)fs["boardSizeY"];
Size boardSize(boardSizeX, boardSizeY);
fs.release();
cout << "calibrating cameras...";
StereoCalib(imageList, boardSize, true, true);
}
StereoMatcher sm("calib/intrinsics.yml", "calib/extrinsics.yml");
Mat left, right, disp;
left = imread("calib/left1.bmp");
right = imread("calib/right1.bmp");
sm.getDisp(left, right, disp);
namedWindow("Disparity");
imshow("Disparity", disp);
waitKey(-1);
while(1){
// grab images from left and right cameras
// rectify images
// calculate disparity
// show depth map
// break if exit condition
break;
}
return 0;
}
int main()
{
cv::namedWindow("Image");
cv::Mat image;
std::vector<std::string> filelist;
// generate list of chessboard image filename
for (int i=1; i<=20; i++) {
std::stringstream str;
str << "../chessboards/chessboard" << std::setw(2) << std::setfill('0') << i << ".jpg";
std::cout << str.str() << std::endl;
filelist.push_back(str.str());
image= cv::imread(str.str(),0);
cv::imshow("Image",image);
cv::waitKey(100);
}
// Create calibrator object
CameraCalibrator cameraCalibrator;
// add the corners from the chessboard
cv::Size boardSize(6,4);
cameraCalibrator.addChessboardPoints(
filelist, // filenames of chessboard image
boardSize); // size of chessboard
// calibrate the camera
// cameraCalibrator.setCalibrationFlag(true,true);
cameraCalibrator.calibrate(image.size());
// Image Undistortion
image = cv::imread(filelist[6]);
cv::Mat uImage= cameraCalibrator.remap(image);
// display camera matrix
cv::Mat cameraMatrix= cameraCalibrator.getCameraMatrix();
std::cout << " Camera intrinsic: " << cameraMatrix.rows << "x" << cameraMatrix.cols << std::endl;
std::cout << cameraMatrix.at<double>(0,0) << " " << cameraMatrix.at<double>(0,1) << " " << cameraMatrix.at<double>(0,2) << std::endl;
std::cout << cameraMatrix.at<double>(1,0) << " " << cameraMatrix.at<double>(1,1) << " " << cameraMatrix.at<double>(1,2) << std::endl;
std::cout << cameraMatrix.at<double>(2,0) << " " << cameraMatrix.at<double>(2,1) << " " << cameraMatrix.at<double>(2,2) << std::endl;
imshow("Original Image", image);
imshow("Undistorted Image", uImage);
cv::waitKey();
return 0;
}
int main(){
vector<string> filelist;
filelist.push_back("../data/chessboard/left01.jpg");
filelist.push_back("../data/chessboard/left02.jpg");
filelist.push_back("../data/chessboard/left03.jpg");
filelist.push_back("../data/chessboard/left04.jpg");
filelist.push_back("../data/chessboard/left05.jpg");
filelist.push_back("../data/chessboard/left06.jpg");
filelist.push_back("../data/chessboard/left07.jpg");
filelist.push_back("../data/chessboard/left08.jpg");
filelist.push_back("../data/chessboard/left09.jpg");
filelist.push_back("../data/chessboard/left11.jpg");
filelist.push_back("../data/chessboard/left12.jpg");
filelist.push_back("../data/chessboard/left13.jpg");
filelist.push_back("../data/chessboard/left14.jpg");
filelist.push_back("../data/chessboard/right01.jpg");
filelist.push_back("../data/chessboard/right02.jpg");
filelist.push_back("../data/chessboard/right03.jpg");
filelist.push_back("../data/chessboard/right04.jpg");
filelist.push_back("../data/chessboard/right05.jpg");
filelist.push_back("../data/chessboard/right06.jpg");
filelist.push_back("../data/chessboard/right07.jpg");
filelist.push_back("../data/chessboard/right08.jpg");
filelist.push_back("../data/chessboard/right09.jpg");
filelist.push_back("../data/chessboard/right11.jpg");
filelist.push_back("../data/chessboard/right12.jpg");
filelist.push_back("../data/chessboard/right13.jpg");
filelist.push_back("../data/chessboard/right14.jpg");
cv::Size boardSize(9,6);
CameraCalibrator ccalibrator;
ccalibrator.addChessboardPoints(filelist, boardSize);
cv::Size imageSize(640, 480);
ccalibrator.calibrate(imageSize);
ccalibrator.print();
Mat distorted = imread(filelist[0], cv::IMREAD_GRAYSCALE);
imshow("distorted image", distorted);
imshow("undistorted image", ccalibrator.remap(distorted));
waitKey();
return 0;
}
void DestructibleBridgeUtil::createBridge(hkArray<hkpConvexShape*>& shapes)
{
hkpBoxShape* beamShape;
{
hkVector4 beamSize(BRIDGE_BEAM_SIZE_X*0.5f, BRIDGE_BEAM_SIZE_Y*0.5f, BRIDGE_BEAM_SIZE_Z*0.5f);
beamShape = new hkpBoxShape(beamSize, 0);
}
hkpBoxShape* boardShape;
{
hkVector4 boardSize(BRIDGE_BOARD_SIZE_X*0.5f, BRIDGE_BOARD_SIZE_Y*0.5f, BRIDGE_BOARD_SIZE_Z*0.5f);
boardShape = new hkpBoxShape(boardSize, 0);
}
hkVector4 position(0.0f, 0.0f, 0.0f);
createBridgeElementLevel0(beamShape, boardShape, position, shapes);
beamShape->removeReference();
boardShape->removeReference();
}
void MainWindow::startNewGame()
{
if(Settings::playSounds())
m_soundStart->start();
const KgDifficultyLevel::StandardLevel l = Kg::difficultyLevel();
bool isWrapped = (l == KgDifficultyLevel::VeryHard);
if (Kg::difficultyLevel() == KgDifficultyLevel::Custom)
isWrapped = Settings::wrapping();
const QSize size = boardSize();
m_view->startNewGame(size.width(), size.height(), (Wrapping)isWrapped);
m_clickCount = -m_view->minimumMoves();
m_gameClock->restart();
if(m_pauseAction->isChecked())
{
m_pauseAction->setChecked(false);
}
Kg::difficulty()->setGameRunning(true);
updateStatusBar();
}
int getMovesCommands(board Game, Piece piece, Move *movesTable, Point point) {
int n = 1;
Move result = moveBuilder(point, piece, Game);
int board_Size = boardSize(Game);
int x = point->x;
int y = point->y;
result->command = MOVE;
for (int j=-1; j<=1; j++) {
for (int i=-1; i<=1; i++) {
if ( (j != 0 ||
i != 0) &&
validatePoint(makePoint(x + i, y + j), board_Size)) {
Piece piece1 = pieceOnPosition(Game, x + i, y + j);
if (piece1 != NULL &&
piece1->owner == piece->owner) {
continue;
}
constructMoveCommand(x+i, y+j, result, Game);
movesTable[n] = newMove();
copyMoveData(movesTable[n], result);
n++;
}
}
}
free(result);
return n;
}
Calib::Calib()
{
Size boardSize(6,5); // Chessboard' size in corners with both color (nb of squares -1)
int widthSquare = 40; // Width of a square in mm
int heightSquare = 27;
vector <Mat> images;
// Getting the four images of the chessboard
string imageFileName = "../src/mire1.jpg";
images.push_back(imread(imageFileName, 1));
imageFileName = "../src/mire2.jpg";
images.push_back(imread(imageFileName, 1));
imageFileName = "../src/mire3.jpg";
images.push_back(imread(imageFileName, 1));
imageFileName = "../src/mire4.jpg";
images.push_back(imread(imageFileName, 1));
Size imageSize = images.at(0).size();
// Find chessboard's corners in the scene for the 4 images
vector<vector<Point2f> > cornersScene(1);
vector<Mat> imagesGray;
imagesGray.resize(4);
for (int i=0; i<4; i++)
{
if(images.at(i).empty())
{
cerr << "Image not read correctly!" << endl;
exit(-1);
}
bool patternFound = findChessboardCorners(images.at(i), boardSize, cornersScene[0]);
if(!patternFound)
{
cerr << "Could not find chess board!" << endl;
exit(-1);
}
// Improve corner's coordinate accuracy
cvtColor(images.at(i), imagesGray.at(i), CV_RGB2GRAY);
cornerSubPix(imagesGray.at(i), cornersScene[0], Size(3,2), Size(-1,-1), TermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));
// Drawing the corners
drawChessboardCorners(images.at(i), boardSize, Mat(cornersScene[0]), patternFound );
imshow("Corners find", images.at(i));
int keyPressed;
/*do
{
keyPressed = waitKey(0);
} while (keyPressed != 27);*/
}
// Getting the chessboard's corners on the mire's image
vector<vector<Point3f> > cornersMire(1);
for( int y = 0; y < boardSize.height; y++ )
{
for( int x = 0; x < boardSize.width; x++ )
{
cornersMire[0].push_back(cv::Point3f(float(x*widthSquare),
float(y*heightSquare), 0));
}
}
// Getting the camera's parameters
Mat distortionCoefficients = Mat::zeros(8, 1, CV_64F);
Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
calibrateCamera(cornersMire, cornersScene, imageSize, cameraMatrix,
distortionCoefficients, rotationVectors, translationVectors);
//cout << "Camera matrix: " << cameraMatrix << endl;
//cout << "Distortion _coefficients: " << distortionCoefficients << endl;
cout << rotationVectors.at(0) << endl;
cout << translationVectors.at(0) << endl;
}
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;
}
}
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 getPossibleMoves(board Game, Piece piece, Move *movesTable, Point point) {
int n = 0;
movesTable[n] = newMove();
n++;
if (piece->lastMove == currentTurn(Game)) {
return n;
}
n = getMovesCommands(Game, piece, movesTable, point);
Move result = moveBuilder(point, piece, Game);
int board_Size = boardSize(Game);
int x = point->x;
int y = point->y;
if (toupper(piece->letter) == 'C' &&
currentTurn(Game) - piece->lastMove >=3 ) {
for (int j=-1; j<=1; j++) {
for (int i=-1; i<=1; i++) {
if ( (j != 0 ||
i != 0) &&
validatePoint(makePoint(x + i, y + j), board_Size) &&
isPositionFree(Game, x + i, y + j)) {
result->xTo = x + i;
result->yTo = y + j;
result->isPieceRemoved= NULL;
char letter = piece->letter;
Piece pieceCreated = createNewPiece(piece->owner, letter,
currentTurn(Game));
result->isPieceCreated = pieceCreated;
result->command = PRODUCE_PEASANT;
movesTable[n] = newMove();
copyMoveData(movesTable[n], result);
n++;
free(result->isPieceCreated);
letter = matchingKnightLetter(letter);
pieceCreated = createNewPiece(piece->owner, letter,
currentTurn(Game) - 1);
result->isPieceCreated = pieceCreated;
result->command = PRODUCE_KNIGHT;
movesTable[n] = newMove();
copyMoveData(movesTable[n], result);
n++;
free(result->isPieceCreated);
}
}
}
}
free(result);
return n;
}
void CalibrationDialog::calibrate()
{
processingData_ = true;
savedCalibration_ = false;
QMessageBox mb(QMessageBox::Information,
tr("Calibrating..."),
tr("Operation in progress..."));
mb.show();
QApplication::processEvents();
uSleep(100); // hack make sure the text in the QMessageBox is shown...
QApplication::processEvents();
std::vector<std::vector<cv::Point3f> > objectPoints(1);
cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
float squareSize = ui_->doubleSpinBox_squareSize->value();
// compute board corner positions
for( int i = 0; i < boardSize.height; ++i )
for( int j = 0; j < boardSize.width; ++j )
objectPoints[0].push_back(cv::Point3f(float( j*squareSize ), float( i*squareSize ), 0));
for(int id=0; id<(stereo_?2:1); ++id)
{
UINFO("Calibrating camera %d (samples=%d)", id, (int)imagePoints_[id].size());
objectPoints.resize(imagePoints_[id].size(), objectPoints[0]);
//calibrate
std::vector<cv::Mat> rvecs, tvecs;
std::vector<float> reprojErrs;
cv::Mat K, D;
K = cv::Mat::eye(3,3,CV_64FC1);
UINFO("calibrate!");
//Find intrinsic and extrinsic camera parameters
double rms = cv::calibrateCamera(objectPoints,
imagePoints_[id],
imageSize_[id],
K,
D,
rvecs,
tvecs);
UINFO("Re-projection error reported by calibrateCamera: %f", rms);
// compute reprojection errors
std::vector<cv::Point2f> imagePoints2;
int i, totalPoints = 0;
double totalErr = 0, err;
reprojErrs.resize(objectPoints.size());
for( i = 0; i < (int)objectPoints.size(); ++i )
{
cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
err = cv::norm(cv::Mat(imagePoints_[id][i]), cv::Mat(imagePoints2), CV_L2);
int n = (int)objectPoints[i].size();
reprojErrs[i] = (float) std::sqrt(err*err/n);
totalErr += err*err;
totalPoints += n;
}
double totalAvgErr = std::sqrt(totalErr/totalPoints);
UINFO("avg re projection error = %f", totalAvgErr);
cv::Mat P(3,4,CV_64FC1);
P.at<double>(2,3) = 1;
K.copyTo(P.colRange(0,3).rowRange(0,3));
std::cout << "cameraMatrix = " << K << std::endl;
std::cout << "distCoeffs = " << D << std::endl;
std::cout << "width = " << imageSize_[id].width << std::endl;
std::cout << "height = " << imageSize_[id].height << std::endl;
models_[id] = CameraModel(cameraName_.toStdString(), imageSize_[id], K, D, cv::Mat::eye(3,3,CV_64FC1), P);
if(id == 0)
{
ui_->label_fx->setNum(models_[id].fx());
ui_->label_fy->setNum(models_[id].fy());
ui_->label_cx->setNum(models_[id].cx());
ui_->label_cy->setNum(models_[id].cy());
ui_->label_error->setNum(totalAvgErr);
std::stringstream strK, strD, strR, strP;
strK << models_[id].K();
strD << models_[id].D();
strR << models_[id].R();
strP << models_[id].P();
ui_->lineEdit_K->setText(strK.str().c_str());
ui_->lineEdit_D->setText(strD.str().c_str());
ui_->lineEdit_R->setText(strR.str().c_str());
ui_->lineEdit_P->setText(strP.str().c_str());
}
else
{
ui_->label_fx_2->setNum(models_[id].fx());
ui_->label_fy_2->setNum(models_[id].fy());
ui_->label_cx_2->setNum(models_[id].cx());
ui_->label_cy_2->setNum(models_[id].cy());
ui_->label_error_2->setNum(totalAvgErr);
std::stringstream strK, strD, strR, strP;
strK << models_[id].K();
strD << models_[id].D();
strR << models_[id].R();
strP << models_[id].P();
ui_->lineEdit_K_2->setText(strK.str().c_str());
ui_->lineEdit_D_2->setText(strD.str().c_str());
ui_->lineEdit_R_2->setText(strR.str().c_str());
ui_->lineEdit_P_2->setText(strP.str().c_str());
}
}
if(stereo_ && models_[0].isValid() && models_[1].isValid())
{
UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
cv::Size imageSize = imageSize_[0].width > imageSize_[1].width?imageSize_[0]:imageSize_[1];
cv::Mat R, T, E, F;
std::vector<std::vector<cv::Point3f> > objectPoints(1);
cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
float squareSize = ui_->doubleSpinBox_squareSize->value();
// compute board corner positions
for( int i = 0; i < boardSize.height; ++i )
for( int j = 0; j < boardSize.width; ++j )
objectPoints[0].push_back(cv::Point3f(float( j*squareSize ), float( i*squareSize ), 0));
objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
// calibrate extrinsic
#if CV_MAJOR_VERSION < 3
double rms = cv::stereoCalibrate(
objectPoints,
stereoImagePoints_[0],
stereoImagePoints_[1],
models_[0].K(), models_[0].D(),
models_[1].K(), models_[1].D(),
imageSize, R, T, E, F,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
cv::CALIB_FIX_INTRINSIC);
#else
double rms = cv::stereoCalibrate(
objectPoints,
stereoImagePoints_[0],
stereoImagePoints_[1],
models_[0].K(), models_[0].D(),
models_[1].K(), models_[1].D(),
imageSize, R, T, E, F,
cv::CALIB_FIX_INTRINSIC,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
#endif
UINFO("stereo calibration... done with RMS error=%f", rms);
double err = 0;
int npoints = 0;
std::vector<cv::Vec3f> lines[2];
UINFO("Computing avg re-projection error...");
for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
{
int npt = (int)stereoImagePoints_[0][i].size();
cv::Mat imgpt[2];
for(int k = 0; k < 2; k++ )
{
imgpt[k] = cv::Mat(stereoImagePoints_[k][i]);
cv::undistortPoints(imgpt[k], imgpt[k], models_[k].K(), models_[k].D(), cv::Mat(), models_[k].K());
computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
}
for(int j = 0; j < npt; j++ )
{
double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
err += errij;
}
npoints += npt;
}
double totalAvgErr = err/(double)npoints;
UINFO("stereo avg re projection error = %f", totalAvgErr);
cv::Mat R1, R2, P1, P2, Q;
cv::Rect validRoi[2];
cv::stereoRectify(models_[0].K(), models_[0].D(),
models_[1].K(), models_[1].D(),
imageSize, R, T, R1, R2, P1, P2, Q,
cv::CALIB_ZERO_DISPARITY, 0, imageSize, &validRoi[0], &validRoi[1]);
UINFO("Valid ROI1 = %d,%d,%d,%d ROI2 = %d,%d,%d,%d newImageSize=%d/%d",
validRoi[0].x, validRoi[0].y, validRoi[0].width, validRoi[0].height,
validRoi[1].x, validRoi[1].y, validRoi[1].width, validRoi[1].height,
imageSize.width, imageSize.height);
if(imageSize_[0].width == imageSize_[1].width)
{
//Stereo, keep new extrinsic projection matrix
stereoModel_ = StereoCameraModel(
cameraName_.toStdString(),
imageSize_[0], models_[0].K(), models_[0].D(), R1, P1,
imageSize_[1], models_[1].K(), models_[1].D(), R2, P2,
R, T, E, F);
}
else
{
//Kinect
stereoModel_ = StereoCameraModel(
cameraName_.toStdString(),
imageSize_[0], models_[0].K(), models_[0].D(), cv::Mat::eye(3,3,CV_64FC1), models_[0].P(),
imageSize_[1], models_[1].K(), models_[1].D(), cv::Mat::eye(3,3,CV_64FC1), models_[1].P(),
R, T, E, F);
}
std::stringstream strR1, strP1, strR2, strP2;
strR1 << stereoModel_.left().R();
strP1 << stereoModel_.left().P();
strR2 << stereoModel_.right().R();
strP2 << stereoModel_.right().P();
ui_->lineEdit_R->setText(strR1.str().c_str());
ui_->lineEdit_P->setText(strP1.str().c_str());
ui_->lineEdit_R_2->setText(strR2.str().c_str());
ui_->lineEdit_P_2->setText(strP2.str().c_str());
ui_->label_baseline->setNum(stereoModel_.baseline());
//ui_->label_error_stereo->setNum(totalAvgErr);
}
if(stereo_ &&
stereoModel_.left().isValid() &&
stereoModel_.right().isValid()&&
(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))
{
ui_->radioButton_rectified->setEnabled(true);
ui_->radioButton_stereoRectified->setEnabled(true);
ui_->radioButton_stereoRectified->setChecked(true);
ui_->pushButton_save->setEnabled(true);
}
else if(models_[0].isValid())
{
ui_->radioButton_rectified->setEnabled(true);
ui_->radioButton_rectified->setChecked(true);
ui_->pushButton_save->setEnabled(!stereo_);
}
UINFO("End calibration");
processingData_ = false;
}
void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)
{
processingData_ = true;
if(cameraName_.isEmpty())
{
cameraName_ = "0000";
if(!cameraName.isEmpty())
{
cameraName_ = cameraName;
}
}
if(ui_->label_serial->text().isEmpty())
{
ui_->label_serial->setText(cameraName_);
}
std::vector<cv::Mat> inputRawImages(2);
if(ui_->checkBox_switchImages->isChecked())
{
inputRawImages[0] = imageRight;
inputRawImages[1] = imageLeft;
}
else
{
inputRawImages[0] = imageLeft;
inputRawImages[1] = imageRight;
}
std::vector<cv::Mat> images(2);
images[0] = inputRawImages[0];
images[1] = inputRawImages[1];
imageSize_[0] = images[0].size();
imageSize_[1] = images[1].size();
bool boardFound[2] = {false};
bool boardAccepted[2] = {false};
bool readyToCalibrate[2] = {false};
std::vector<std::vector<cv::Point2f> > pointBuf(2);
bool depthDetected = false;
for(int id=0; id<(stereo_?2:1); ++id)
{
cv::Mat viewGray;
if(!images[id].empty())
{
if(images[id].type() == CV_16UC1)
{
depthDetected = true;
//assume IR image: convert to gray scaled
const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1);
for(int i=0; i<images[id].rows; ++i)
{
for(int j=0; j<images[id].cols; ++j)
{
viewGray.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(images[id].at<unsigned short>(i,j) - minIrs_[id], 0)) * factor, 255.0f);
}
}
cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
}
else if(images[id].channels() == 3)
{
cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
}
else
{
viewGray = images[id];
cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
}
}
else
{
UERROR("Image %d is empty!! Should not!", id);
}
minIrs_[id] = 0;
maxIrs_[id] = 0x7FFF;
//Dot it only if not yet calibrated
if(!ui_->pushButton_save->isEnabled())
{
cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
if(!viewGray.empty())
{
int flags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;
if(!viewGray.empty())
{
int maxScale = viewGray.cols < 640?2:1;
for( int scale = 1; scale <= maxScale; scale++ )
{
cv::Mat timg;
if( scale == 1 )
timg = viewGray;
else
cv::resize(viewGray, timg, cv::Size(), scale, scale, CV_INTER_CUBIC);
boardFound[id] = cv::findChessboardCorners(timg, boardSize, pointBuf[id], flags);
if(boardFound[id])
{
if( scale > 1 )
{
cv::Mat cornersMat(pointBuf[id]);
cornersMat *= 1./scale;
}
break;
}
}
}
}
if(boardFound[id]) // If done with success,
{
// improve the found corners' coordinate accuracy for chessboard
float minSquareDistance = -1.0f;
for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
{
float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
if(minSquareDistance == -1.0f || minSquareDistance > d)
{
minSquareDistance = d;
}
}
float radius = minSquareDistance/2.0f +0.5f;
cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
// Draw the corners.
cv::drawChessboardCorners(images[id], boardSize, cv::Mat(pointBuf[id]), boardFound[id]);
std::vector<float> params(4,0);
getParams(pointBuf[id], boardSize, imageSize_[id], params[0], params[1], params[2], params[3]);
bool addSample = true;
for(unsigned int i=0; i<imageParams_[id].size(); ++i)
{
if(fabs(params[0] - imageParams_[id][i].at(0)) < 0.1 && // x
fabs(params[1] - imageParams_[id][i].at(1)) < 0.1 && // y
fabs(params[2] - imageParams_[id][i].at(2)) < 0.05 && // size
fabs(params[3] - imageParams_[id][i].at(3)) < 0.1) // skew
{
addSample = false;
}
}
if(addSample)
{
boardAccepted[id] = true;
imagePoints_[id].push_back(pointBuf[id]);
imageParams_[id].push_back(params);
UINFO("[%d] Added board, total=%d. (x=%f, y=%f, size=%f, skew=%f)", id, (int)imagePoints_[id].size(), params[0], params[1], params[2], params[3]);
}
// update statistics
std::vector<float> xRange(2, imageParams_[id][0].at(0));
std::vector<float> yRange(2, imageParams_[id][0].at(1));
std::vector<float> sizeRange(2, imageParams_[id][0].at(2));
std::vector<float> skewRange(2, imageParams_[id][0].at(3));
for(unsigned int i=1; i<imageParams_[id].size(); ++i)
{
xRange[0] = imageParams_[id][i].at(0) < xRange[0] ? imageParams_[id][i].at(0) : xRange[0];
xRange[1] = imageParams_[id][i].at(0) > xRange[1] ? imageParams_[id][i].at(0) : xRange[1];
yRange[0] = imageParams_[id][i].at(1) < yRange[0] ? imageParams_[id][i].at(1) : yRange[0];
yRange[1] = imageParams_[id][i].at(1) > yRange[1] ? imageParams_[id][i].at(1) : yRange[1];
sizeRange[0] = imageParams_[id][i].at(2) < sizeRange[0] ? imageParams_[id][i].at(2) : sizeRange[0];
sizeRange[1] = imageParams_[id][i].at(2) > sizeRange[1] ? imageParams_[id][i].at(2) : sizeRange[1];
skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
}
//UINFO("Stats [%d]:", id);
//UINFO(" Count = %d", (int)imagePoints_[id].size());
//UINFO(" x = [%f -> %f]", xRange[0], xRange[1]);
//UINFO(" y = [%f -> %f]", yRange[0], yRange[1]);
//UINFO(" size = [%f -> %f]", sizeRange[0], sizeRange[1]);
//UINFO(" skew = [%f -> %f]", skewRange[0], skewRange[1]);
float xGood = xRange[1] - xRange[0];
float yGood = yRange[1] - yRange[0];
float sizeGood = sizeRange[1] - sizeRange[0];
float skewGood = skewRange[1] - skewRange[0];
if(id == 0)
{
ui_->progressBar_x->setValue(xGood*100);
ui_->progressBar_y->setValue(yGood*100);
ui_->progressBar_size->setValue(sizeGood*100);
ui_->progressBar_skew->setValue(skewGood*100);
if((int)imagePoints_[id].size() > ui_->progressBar_count->maximum())
{
ui_->progressBar_count->setMaximum((int)imagePoints_[id].size());
}
ui_->progressBar_count->setValue((int)imagePoints_[id].size());
}
else
{
ui_->progressBar_x_2->setValue(xGood*100);
ui_->progressBar_y_2->setValue(yGood*100);
ui_->progressBar_size_2->setValue(sizeGood*100);
ui_->progressBar_skew_2->setValue(skewGood*100);
if((int)imagePoints_[id].size() > ui_->progressBar_count_2->maximum())
{
ui_->progressBar_count_2->setMaximum((int)imagePoints_[id].size());
}
ui_->progressBar_count_2->setValue((int)imagePoints_[id].size());
}
if(imagePoints_[id].size() >= COUNT_MIN && xGood > 0.5 && yGood > 0.5 && sizeGood > 0.4 && skewGood > 0.5)
{
readyToCalibrate[id] = true;
}
//update IR values
if(inputRawImages[id].type() == CV_16UC1)
{
//update min max IR if the chessboard was found
minIrs_[id] = 0xFFFF;
maxIrs_[id] = 0;
for(size_t i = 0; i < pointBuf[id].size(); ++i)
{
const cv::Point2f &p = pointBuf[id][i];
cv::Rect roi(std::max(0, (int)p.x - 3), std::max(0, (int)p.y - 3), 6, 6);
roi.width = std::min(roi.width, inputRawImages[id].cols - roi.x);
roi.height = std::min(roi.height, inputRawImages[id].rows - roi.y);
//find minMax in the roi
double min, max;
cv::minMaxLoc(inputRawImages[id](roi), &min, &max);
if(min < minIrs_[id])
{
minIrs_[id] = min;
}
if(max > maxIrs_[id])
{
maxIrs_[id] = max;
}
}
}
}
}
}
ui_->label_baseline->setVisible(!depthDetected);
ui_->label_baseline_name->setVisible(!depthDetected);
if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0])))
{
stereoImagePoints_[0].push_back(pointBuf[0]);
stereoImagePoints_[1].push_back(pointBuf[1]);
UINFO("Add stereo image points (size=%d)", (int)stereoImagePoints_[0].size());
}
if(!stereo_ && readyToCalibrate[0])
{
ui_->pushButton_calibrate->setEnabled(true);
}
else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
{
ui_->pushButton_calibrate->setEnabled(true);
}
if(ui_->radioButton_rectified->isChecked())
{
if(models_[0].isValid())
{
images[0] = models_[0].rectifyImage(images[0]);
}
if(models_[1].isValid())
{
images[1] = models_[1].rectifyImage(images[1]);
}
}
else if(ui_->radioButton_stereoRectified->isChecked() &&
(stereoModel_.left().isValid() &&
stereoModel_.right().isValid()&&
(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
{
images[0] = stereoModel_.left().rectifyImage(images[0]);
images[1] = stereoModel_.right().rectifyImage(images[1]);
}
if(ui_->checkBox_showHorizontalLines->isChecked())
{
for(int id=0; id<(stereo_?2:1); ++id)
{
int step = imageSize_[id].height/16;
for(int i=step; i<imageSize_[id].height; i+=step)
{
cv::line(images[id], cv::Point(0, i), cv::Point(imageSize_[id].width, i), CV_RGB(0,255,0));
}
}
}
ui_->label_left->setText(tr("%1x%2").arg(images[0].cols).arg(images[0].rows));
//show frame
ui_->image_view->setImage(uCvMat2QImage(images[0]).mirrored(ui_->checkBox_mirror->isChecked(), false));
if(stereo_)
{
ui_->label_right->setText(tr("%1x%2").arg(images[1].cols).arg(images[1].rows));
ui_->image_view_2->setImage(uCvMat2QImage(images[1]).mirrored(ui_->checkBox_mirror->isChecked(), false));
}
processingData_ = false;
}
void OnStartCalib(class Fl_Button* b, void*)
{
CalibUI* ui = reinterpret_cast<CalibUI*>(b->window()->user_data());
if (ui != nullptr)
{
ui->calibImageBox->SetImagePoints(nullptr);
cv::Size imageSize;
cv::Size boardSize(static_cast<int>(ui->vertCornersInput->value()),
static_cast<int>(ui->horizCornersInput->value()));
ui->appData->imagesPoints.clear();
ui->appData->imagesPointsMap.clear();
ui->calibTextResult->buffer(ui->appData->resultTextBuff.get());
ui->appData->resultTextBuff->text("Calibration in progress ...");
ui->calibTextResult->redraw();
Fl::wait();
for (int i = 1; i <= ui->calibImageBrowser->size(); ++i)
{
Fl::wait();
const char* fileName = ui->calibImageBrowser->text(i);
Fl_Shared_Image *img = Fl_Shared_Image::get(fileName);
cv::Mat mat = FLImageToMat(img);
imageSize = mat.size();
std::vector<cv::Point2f> pointbuf;
bool found = cv::findChessboardCorners(mat, boardSize, pointbuf, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
Fl::wait();
cv::Mat matGray;
cv::cvtColor(mat, matGray, CV_BGR2GRAY);
if(found)
{
cv::cornerSubPix(matGray, pointbuf, cv::Size(11,11),
cv::Size(-1,-1), cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
ui->appData->imagesPoints.push_back(pointbuf);
ui->appData->imagesPointsMap.insert(std::make_pair(fileName, ui->appData->imagesPoints.size() - 1));
}
img->release();
}
if (!ui->appData->imagesPoints.empty())
{
double squareSize = ui->cellSizeInput->value();
bool ok = runCalibration(ui->appData->imagesPoints, imageSize, boardSize, squareSize,
ui->appData->cameraMatrix, ui->appData->distCoeffs);
if (ok)
{
std::stringstream buf;
double pixelSize = 0;
buf << "Focal length X (pixel rel. units) : " << ui->appData->cameraMatrix.at<double>(0,0) << "\n";
buf << "Focal length Y (pixel rel. units) : " << ui->appData->cameraMatrix.at<double>(1,1) << "\n";
buf << "Principal point x : " << ui->appData->cameraMatrix.at<double>(0,2) << "\n";
buf << "Principal point y : " << ui->appData->cameraMatrix.at<double>(1,2) << "\n";
buf << "K1 : " << ui->appData->distCoeffs.at<double>(0, 0) << "\n";
buf << "K2 : " << ui->appData->distCoeffs.at<double>(1, 0) << "\n";
buf << "K3 : " << ui->appData->distCoeffs.at<double>(4, 0) << "\n";
buf << "P1 : " << ui->appData->distCoeffs.at<double>(2, 0) << "\n";
buf << "P2 : " << ui->appData->distCoeffs.at<double>(3, 0) << "\n";
ui->appData->resultTextBuff->text(buf.str().c_str());
OnCalibImageSelected(ui->calibImageBrowser, nullptr);
return;
}
}
}
ui->appData->resultTextBuff->text("Calibration failed!");
}
