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;
}
int main(int argc, char** argv){
int status,i;
Ice::CommunicatorPtr ic;
Ice::PropertiesPtr prop;
cv::Size boardSize;
Pattern pattern = CHESSBOARD;
float squareSize;
float aspectRatio;
int nframes;
bool writeExtrinsics = false;
bool writePoints = false;
int delay;
clock_t prevTimestamp = 0;
int mode = DETECTION;
bool undistortImage = false;
bool globalCalibrated=false;
std::vector<cameraData> cameras;
try{
ic = Ice::initialize(argc,argv);
prop = ic->getProperties();
}catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
return 1;
}
catch (const char* msg) {
std::cerr <<"Error :" << msg << std::endl;
return 1;
}
std::string componentPrefix("cameraCalibrator");
std::cout << "Ncameras: " << prop->getPropertyAsIntWithDefault(componentPrefix+".nCameras",1) << std::endl;
for (int i=0; i< prop->getPropertyAsIntWithDefault(componentPrefix+".nCameras",1); i++){
cameraData cam;
std::stringstream ss;
ss << componentPrefix << ".camera." <<i <<".";
cam.outputFilename=prop->getProperty(ss.str()+"outFile");
cam.proxy = new jderobot::cameraClient(ic,ss.str());
if (cam.proxy != NULL){
cam.proxy->start();
}
else{
throw "rgbdViewer: failed to load RGB Camera";
}
cam.calibrated=false;
cameras.push_back(cam);
}
//parsing calibration parameteres
boardSize.height=prop->getPropertyAsIntWithDefault(componentPrefix+".pattern.height",6);
boardSize.width=prop->getPropertyAsIntWithDefault(componentPrefix+".pattern.width",9);
std::string patterType =prop->getProperty(componentPrefix+".pattern.type");
if( !strcmp( patterType.c_str(), "circles" ) )
pattern = CIRCLES_GRID;
else if( !strcmp( patterType.c_str(), "acircles" ) )
pattern = ASYMMETRIC_CIRCLES_GRID;
else if( !strcmp( patterType.c_str(), "chessboard" ) )
pattern = CHESSBOARD;
else
return fprintf( stderr, "Invalid pattern type: must be chessboard or circles\n" ), -1;
squareSize=atof(prop->getProperty(componentPrefix+".pattern.size").c_str());
aspectRatio=atof(prop->getProperty(componentPrefix+".pattern.ratio").c_str());
nframes=prop->getPropertyAsIntWithDefault(componentPrefix+".frames",10);
writePoints=boost::lexical_cast<bool>(prop->getProperty(componentPrefix+".writePoints"));
writeExtrinsics=boost::lexical_cast<bool>(prop->getProperty(componentPrefix+".writeEstrinsics"));
delay=prop->getPropertyAsIntWithDefault(componentPrefix+".delay",1000);
//init the poling
while (1){
cv::Mat frame;
for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){
while (it->frame.cols ==0){
it->proxy->getImage(it->frame);
}
it->imageSize=it->frame.size();
}
break;
}
std::cout << "Poling init done" << std::endl;
while (1){
for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){
it->proxy->getImage(it->frame);
it->frame.copyTo(it->raw_frame);
it->pointbuf.resize(0);
if (it->frame.channels() ==3)
cv::cvtColor(it->frame, it->viewGray, cv::COLOR_BGR2GRAY);
else
it->frame.copyTo(it->viewGray);
it->found=false;
switch( pattern )
{
case CHESSBOARD:
it->found = cv::findChessboardCorners( it->frame, boardSize, it->pointbuf,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
break;
case CIRCLES_GRID:
it->found = cv::findCirclesGrid( it->frame, boardSize, it->pointbuf );
break;
case ASYMMETRIC_CIRCLES_GRID:
it->found = cv::findCirclesGrid( it->frame, boardSize, it->pointbuf, cv::CALIB_CB_ASYMMETRIC_GRID );
break;
default:
return fprintf( stderr, "Unknown pattern type\n" ), -1;
}
if( pattern == CHESSBOARD && it->found)
cv::cornerSubPix( it->viewGray, it->pointbuf, cv::Size(11,11), cv::Size(-1,-1), cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
if(it->found)
cv::drawChessboardCorners( it->frame, boardSize, cv::Mat(it->pointbuf), it->found );
}
bool globalFound=true;
for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){
globalFound=globalFound && it->found;
}
if( mode == CAPTURING && globalFound && (clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) )
{
for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){
cv::Mat temp;
it->raw_frame.copyTo(temp);
it->images.push_back(temp);
it->imagePoints.push_back(it->pointbuf);
}
prevTimestamp = clock();
}
for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){
std::string msg = mode == CAPTURING ? "100/100" : mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
int baseLine = 0;
cv::Size textSize = cv::getTextSize(msg, 1, 1, 1, &baseLine);
cv::Point textOrigin(cameras[0].frame.cols - 2*textSize.width - 10, cameras[0].frame.rows - 2*baseLine - 10);
if( mode == CAPTURING )
{
if(undistortImage)
msg = cv::format( "%d/%d Undist", (int)it->imagePoints.size(), nframes );
else
msg = cv::format( "%d/%d", (int)it->imagePoints.size(), nframes );
}
cv::putText( it->frame, msg, textOrigin, 1, 1, mode != CALIBRATED ? cv::Scalar(0,0,255) : cv::Scalar(0,255,0));
if( mode == CALIBRATED && undistortImage )
{
cv::Mat temp = it->frame.clone();
undistort(temp, it->frame, it->cameraMatrix, it->distCoeffs);
}
std::stringstream ss;
ss << "Image " << it-cameras.begin();
cv::imshow(ss.str(),it->frame);
int key=0xff & cv::waitKey(1);
if( (key & 255) == 27 )
break;
if( key == 'u' && mode == CALIBRATED )
undistortImage = !undistortImage;
if( key == 'g' )
{
mode = CAPTURING;
it->imagePoints.clear();
}
if( !globalCalibrated && it->imagePoints.size() >= (unsigned)nframes )
{
if( runAndSave(it->outputFilename, it->imagePoints, it->imageSize,
boardSize, pattern, squareSize, aspectRatio,
0, it->cameraMatrix, it->distCoeffs,
writeExtrinsics, writePoints)){
mode = CALIBRATED;
it->calibrated=true;;
}
else
mode = DETECTION;
//save pictures
int cameraID = std::distance(cameras.begin(),it);
for ( std::vector<cv::Mat>::iterator itImage = it->images.begin(); itImage != it->images.end(); itImage++){
std::stringstream ss;
ss << "camera-" << cameraID << "-" << std::distance(it->images.begin(), itImage) << ".png";
cv::imwrite(ss.str(),*itImage);
}
}
}
for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){
if (it == cameras.begin()){
globalCalibrated=it->calibrated;
}
else
globalCalibrated=globalCalibrated && it->calibrated;
}
}
}
