iCub-main
stereoCalibThread.cpp
Go to the documentation of this file.
1 #include <utility>
2 #include <yarp/cv/Cv.h>
3 #include "stereoCalibThread.h"
4 
5 
6 stereoCalibThread::stereoCalibThread(ResourceFinder &rf, Port* commPort, const char *imageDir)
7 {
8  moduleName=rf.check("name", Value("stereoCalib"),"module name (string)").asString().c_str();
9  robotName=rf.check("robotName",Value("icub"), "module name (string)").asString().c_str();
10 
11  this->inputLeftPortName = "/"+moduleName;
12  this->inputLeftPortName +=rf.check("imgLeft",Value("/cam/left:i"),"Input image port (string)").asString().c_str();
13 
14  this->inputRightPortName = "/"+moduleName;
15  this->inputRightPortName += rf.check("imgRight", Value("/cam/right:i"),"Input image port (string)").asString().c_str();
16 
17  this->outNameRight = "/"+moduleName;
18  this->outNameRight += rf.check("outRight",Value("/cam/right:o"),"Output image port (string)").asString().c_str();
19 
20  this->outNameLeft = "/"+moduleName;
21  this->outNameLeft +=rf.check("outLeft",Value("/cam/left:o"),"Output image port (string)").asString().c_str();
22 
23  Bottle stereoCalibOpts=rf.findGroup("STEREO_CALIBRATION_CONFIGURATION");
24  this->boardWidth= stereoCalibOpts.check("boardWidth", Value(8)).asInt32();
25  this->boardHeight= stereoCalibOpts.check("boardHeight", Value(6)).asInt32();
26  this->numOfPairs= stereoCalibOpts.check("numberOfPairs", Value(30)).asInt32();
27  this->squareSize= (float)stereoCalibOpts.check("boardSize", Value(0.09241)).asFloat64();
28  this->boardType= stereoCalibOpts.check("boardType", Value("CHESSBOARD")).asString();
29  this->commandPort=commPort;
30  this->imageDir=imageDir;
31  this->startCalibration=0;
32  this->currentPathDir=rf.getHomeContextPath().c_str();
33  int tmp=stereoCalibOpts.check("MonoCalib", Value(0)).asInt32();
34  this->stereo= tmp?false:true;
35  this->camCalibFile=rf.getHomeContextPath().c_str();
36  this->standalone = rf.check("standalone");
37  string fileName= "outputCalib.ini"; //rf.find("from").asString().c_str();
38 
39  this->camCalibFile=this->camCalibFile+"/"+fileName.c_str();
40 }
41 
42 bool stereoCalibThread::threadInit()
43 {
44  if (!imagePortInLeft.open(inputLeftPortName.c_str())) {
45  cout << ": unable to open port " << inputLeftPortName << endl;
46  return false;
47  }
48 
49  if (!imagePortInRight.open(inputRightPortName.c_str())) {
50  cout << ": unable to open port " << inputRightPortName << endl;
51  return false;
52  }
53 
54  if (!outPortLeft.open(outNameLeft.c_str())) {
55  cout << ": unable to open port " << outNameLeft << endl;
56  return false;
57  }
58 
59  if (!outPortRight.open(outNameRight.c_str())) {
60  cout << ": unable to open port " << outNameRight << endl;
61  return false;
62  }
63 
64  //mono calibration does not need the joint positions initialised below
65  if(!stereo || standalone) return true;
66 
67  Property optHead;
68  optHead.put("device","remote_controlboard");
69  optHead.put("remote",("/"+robotName+"/head").c_str());
70  optHead.put("local","/"+moduleName+"/client/head");
71  if (polyHead.open(optHead))
72  polyHead.view(posHead);
73  else
74  {
75  cout<<"Devices not available"<<endl;
76  return false;
77  }
78 
79  Property optTorso;
80  optTorso.put("device","remote_controlboard");
81  optTorso.put("remote",("/"+robotName+"/torso").c_str());
82  optTorso.put("local","/"+moduleName+"/client/torso");
83 
84  bool useTorso=true;
85  if (polyTorso.open(optTorso))
86  polyTorso.view(posTorso);
87  else
88  {
89  yWarning("Unable to connect to torso! Continuing without...");
90  useTorso=false;
91  }
92 
93  yarp::sig::Vector head_angles(6,0.0);
94  posHead->getEncoders(head_angles.data());
95 
96  yarp::sig::Vector torso_angles(3,0.0);
97  if (useTorso)
98  posTorso->getEncoders(torso_angles.data());
99 
100  qL.resize(torso_angles.length()+head_angles.length()-1);
101  for(size_t i=0; i<torso_angles.length(); i++)
102  qL[i]=torso_angles[torso_angles.length()-i-1];
103 
104  for(size_t i=0; i<head_angles.length()-2; i++)
105  qL[i+torso_angles.length()]=head_angles[i];
106  qL[7]=head_angles[4]+(0.5-(LEFT))*head_angles[5];
107  qL=iCub::ctrl::CTRL_DEG2RAD*qL;
108 
109  qR.resize(torso_angles.length()+head_angles.length()-1);
110  for(size_t i=0; i<torso_angles.length(); i++)
111  qR[i]=torso_angles[torso_angles.length()-i-1];
112 
113  for(size_t i=0; i<head_angles.length()-2; i++)
114  qR[i+torso_angles.length()]=head_angles[i];
115  qR[7]=head_angles[4]+(0.5-(RIGHT))*head_angles[5];
116  qR=iCub::ctrl::CTRL_DEG2RAD*qR;
117 
118  return true;
119 }
120 void stereoCalibThread::run(){
121 
122  if(stereo)
123  {
124  yInfo("Running Stereo Calibration Mode... \n");
125  stereoCalibRun();
126  }
127  else
128  {
129  yInfo("Running Mono Calibration Mode... Connect only one eye \n");
130  monoCalibRun();
131  }
132 }
133 void stereoCalibThread::stereoCalibRun()
134 {
135  imageL=new ImageOf<PixelRgb>;
136  imageR=new ImageOf<PixelRgb>;
137 
138  Stamp TSLeft;
139  Stamp TSRight;
140 
141  bool initL=false;
142  bool initR=false;
143 
144  int count=1;
145  Size boardSize, imageSize;
146  boardSize.width=this->boardWidth;
147  boardSize.height=this->boardHeight;
148 
149  while (!isStopping()) {
150  ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(false);
151  ImageOf<PixelRgb> *tmpR = imagePortInRight.read(false);
152 
153  if(tmpL!=NULL)
154  {
155  *imageL=*tmpL;
156  imagePortInLeft.getEnvelope(TSLeft);
157  initL=true;
158  }
159  if(tmpR!=NULL)
160  {
161  *imageR=*tmpR;
162  imagePortInRight.getEnvelope(TSRight);
163  initR=true;
164  }
165 
166  if(initL && initR && checkTS(TSLeft.getTime(),TSRight.getTime())){
167 
168  bool foundL=false;
169  bool foundR=false;
170 
171  mtx.lock();
172  if(startCalibration>0) {
173 
174  string pathImg=imageDir;
175  preparePath(pathImg.c_str(),pathL,pathR,count);
176  string iml(pathL);
177  string imr(pathR);
178  Left=yarp::cv::toCvMat(*imageL);
179  Right=yarp::cv::toCvMat(*imageR);
180 
181  std::vector<Point2f> pointbufL;
182  std::vector<Point2f> pointbufR;
183  if(boardType == "CIRCLES_GRID") {
184  foundL = findCirclesGrid(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
185  foundR = findCirclesGrid(Right, boardSize, pointbufR, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
186  } else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
187  foundL = findCirclesGrid(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
188  foundR = findCirclesGrid(Right, boardSize, pointbufR, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
189  } else {
190  foundL = findChessboardCorners(Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
191  foundR = findChessboardCorners(Right, boardSize, pointbufR, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
192  }
193 
194  if(foundL && foundR) {
195  cvtColor(Left,Left,CV_RGB2BGR);
196  cvtColor(Right,Right,CV_RGB2BGR);
197  saveStereoImage(pathImg.c_str(),Left,Right,count);
198 
199  imageListR.push_back(imr);
200  imageListL.push_back(iml);
201  imageListLR.push_back(iml);
202  imageListLR.push_back(imr);
203  Mat cL(pointbufL);
204  Mat cR(pointbufR);
205  drawChessboardCorners(Left, boardSize, cL, foundL);
206  drawChessboardCorners(Right, boardSize, cR, foundR);
207  count++;
208  }
209 
210  if(count>numOfPairs) {
211  yInfo(" Running Left Camera Calibration... \n");
212  monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);
213 
214  yInfo(" Running Right Camera Calibration... \n");
215  monoCalibration(imageListR,this->boardWidth,this->boardHeight,this->Kright,this->DistR);
216 
217  stereoCalibration(imageListLR, this->boardWidth,this->boardHeight,this->squareSize);
218 
219  yInfo(" Saving Calibration Results... \n");
220  updateIntrinsics(Right.cols,Right.rows,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");
221  updateIntrinsics(Left.cols,Left.rows,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");
222 
223  Mat Rot=Mat::eye(3,3,CV_64FC1);
224  Mat Tr=Mat::zeros(3,1,CV_64FC1);
225 
226  updateExtrinsics(this->R,this->T,"STEREO_DISPARITY");
227 
228  yInfo("Calibration Results Saved in %s \n", camCalibFile.c_str());
229 
230  startCalibration=0;
231  count=1;
232  imageListR.clear();
233  imageListL.clear();
234  imageListLR.clear();
235  }
236  }
237  mtx.unlock();
238  ImageOf<PixelRgb>& outimL=outPortLeft.prepare();
239  outimL=*imageL;
240  outPortLeft.write();
241 
242  ImageOf<PixelRgb>& outimR=outPortRight.prepare();
243  outimR=*imageR;
244  outPortRight.write();
245 
246  if(foundL && foundR && startCalibration==1)
247  Time::delay(2.0);
248  initL=initR=false;
249  cout.flush();
250  }
251  }
252 
253  delete imageL;
254  delete imageR;
255 }
256 
257 
258 
259 void stereoCalibThread::monoCalibRun()
260 {
261  while(imagePortInLeft.getInputCount()==0 && imagePortInRight.getInputCount()==0)
262  {
263  yInfo("Connect one camera.. \n");
264  Time::delay(1.0);
265 
266  if(isStopping())
267  return;
268 
269  }
270 
271  bool left= imagePortInLeft.getInputCount()>0?true:false;
272 
273  string cameraName;
274 
275  if(left)
276  cameraName="LEFT";
277  else
278  cameraName="RIGHT";
279 
280  yInfo("CALIBRATING %s CAMERA \n",cameraName.c_str());
281 
282 
283  int count=1;
284  Size boardSize, imageSize;
285  boardSize.width=this->boardWidth;
286  boardSize.height=this->boardHeight;
287 
288 
289 
290  while (!isStopping()) {
291  if(left)
292  imageL = imagePortInLeft.read(false);
293  else
294  imageL = imagePortInRight.read(false);
295 
296  if(imageL!=NULL){
297  bool foundL=false;
298  mtx.lock();
299  if(startCalibration>0) {
300 
301  string pathImg=imageDir;
302  preparePath(pathImg.c_str(),pathL,pathR,count);
303  string iml(pathL);
304  Left=yarp::cv::toCvMat(*imageL);
305  std::vector<Point2f> pointbufL;
306 
307  if(boardType == "CIRCLES_GRID") {
308  foundL = findCirclesGrid(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
309  } else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
310  foundL = findCirclesGrid(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
311  } else {
312  foundL = findChessboardCorners(Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
313  }
314 
315  if(foundL) {
316  cvtColor(Left,Left,CV_RGB2BGR);
317  saveImage(pathImg.c_str(),Left,count);
318  imageListL.push_back(iml);
319  Mat cL(pointbufL);
320  drawChessboardCorners(Left, boardSize, cL, foundL);
321  count++;
322  }
323 
324  if(count>numOfPairs) {
325  yInfo(" Running %s Camera Calibration... \n", cameraName.c_str());
326  monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);
327 
328  yInfo(" Saving Calibration Results... \n");
329  updateIntrinsics(Left.cols,Left.rows,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),
330  Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),
331  DistL.at<double>(0,3),left?"CAMERA_CALIBRATION_LEFT":"CAMERA_CALIBRATION_RIGHT");
332  yInfo("Calibration Results Saved in %s \n", camCalibFile.c_str());
333 
334  startCalibration=0;
335  count=1;
336  imageListL.clear();
337  }
338  }
339  mtx.unlock();
340  ImageOf<PixelRgb>& outimL=outPortLeft.prepare();
341  outimL=*imageL;
342  outPortLeft.write();
343 
344  ImageOf<PixelRgb>& outimR=outPortRight.prepare();
345  outimR=*imageL;
346  outPortRight.write();
347 
348  if(foundL && startCalibration==1)
349  Time::delay(2.0);
350  cout.flush();
351 
352  }
353  }
354 
355 
356  }
357 void stereoCalibThread::threadRelease()
358 {
359  imagePortInRight.close();
360  imagePortInLeft.close();
361  outPortLeft.close();
362  outPortRight.close();
363  commandPort->close();
364 
365  if (polyHead.isValid())
366  polyHead.close();
367 
368  if (polyTorso.isValid())
369  polyTorso.close();
370 }
371 
372 void stereoCalibThread::onStop() {
373  startCalibration=0;
374  imagePortInRight.interrupt();
375  imagePortInLeft.interrupt();
376  outPortLeft.interrupt();
377  outPortRight.interrupt();
378  commandPort->interrupt();
379 
380 }
381 void stereoCalibThread::startCalib() {
382  lock_guard<mutex> lck(mtx);
383  startCalibration=1;
384  }
385 
386 void stereoCalibThread::stopCalib() {
387  lock_guard<mutex> lck(mtx);
388  startCalibration=0;
389  }
390 
391 void stereoCalibThread::printMatrix(Mat &matrix) {
392  int row=matrix.rows;
393  int col =matrix.cols;
394  cout << endl;
395  for(int i = 0; i < matrix.rows; i++)
396  {
397  const double* Mi = matrix.ptr<double>(i);
398  for(int j = 0; j < matrix.cols; j++)
399  cout << Mi[j] << " ";
400  cout << endl;
401  }
402  cout << endl;
403 }
404 
405 
406 bool stereoCalibThread::checkTS(double TSLeft, double TSRight, double th) {
407  double diff=fabs(TSLeft-TSRight);
408  if(diff <th)
409  return true;
410  else return false;
411 
412 }
413 
414 void stereoCalibThread::preparePath(const char * imageDir, char* pathL, char* pathR, int count) {
415  char num[5];
416  sprintf(num, "%i", count);
417 
418 
419  strncpy(pathL,imageDir, strlen(imageDir));
420  pathL[strlen(imageDir)]='\0';
421  strcat(pathL,"left");
422  strcat(pathL,num);
423  strcat(pathL,".png");
424 
425  strncpy(pathR,imageDir, strlen(imageDir));
426  pathR[strlen(imageDir)]='\0';
427  strcat(pathR,"right");
428  strcat(pathR,num);
429  strcat(pathR,".png");
430 
431 }
432 
433 
434 void stereoCalibThread::saveStereoImage(const char * imageDir, const Mat& left, const Mat& right, int num) {
435  char pathL[256];
436  char pathR[256];
437  preparePath(imageDir, pathL,pathR,num);
438 
439  yInfo("Saving images number %d \n",num);
440 
441  imwrite(pathL,left);
442  imwrite(pathR,right);
443 }
444 
445 void stereoCalibThread::saveImage(const char * imageDir, const Mat& left, int num) {
446  char pathL[256];
447  preparePath(imageDir, pathL,pathR,num);
448 
449  yInfo("Saving images number %d \n",num);
450 
451  imwrite(pathL,left);
452 }
453 
454 bool stereoCalibThread::updateIntrinsics(int width, int height, double fx, double fy,double cx, double cy, double k1, double k2, double p1, double p2, const string& groupname){
455 
456  std::vector<string> lines;
457 
458  bool append = false;
459 
460  ifstream in;
461  in.open(camCalibFile.c_str()); //camCalibFile.c_str());
462 
463  if(in.is_open()){
464  // file exists
465  string line;
466  bool sectionFound = false;
467  bool sectionClosed = false;
468 
469  // process lines
470  while(std::getline(in, line)){
471  // check if we left calibration section
472  if (sectionFound == true && line.find("[", 0) != string::npos)
473  sectionClosed = true; // also valid if no groupname specified
474  // check if we enter calibration section
475  if (line.find(string("[") + groupname + string("]"), 0) != string::npos)
476  sectionFound = true;
477  // if no groupname specified
478  if (groupname == "")
479  sectionFound = true;
480  // if we are in calibration section (or no section/group specified)
481  if (sectionFound == true && sectionClosed == false){
482  // replace w line
483  if (line.find("w",0) ==0){
484  stringstream ss;
485  ss << width;
486  line = "w " + string(ss.str());
487  }
488  // replace h line
489  if (line.find("h",0) ==0){
490  stringstream ss;
491  ss << height;
492  line = "h " + string(ss.str());
493  }
494  // replace fx line
495  if (line.find("fx",0) != string::npos){
496  stringstream ss;
497  ss << fx;
498  line = "fx " + string(ss.str());
499  }
500  // replace fy line
501  if (line.find("fy",0) != string::npos){
502  stringstream ss;
503  ss << fy;
504  line = "fy " + string(ss.str());
505  }
506  // replace cx line
507  if (line.find("cx",0) != string::npos){
508  stringstream ss;
509  ss << cx;
510  line = "cx " + string(ss.str());
511  }
512  // replace cy line
513  if (line.find("cy",0) != string::npos){
514  stringstream ss;
515  ss << cy;
516  line = "cy " + string(ss.str());
517  }
518  // replace k1 line
519  if (line.find("k1",0) != string::npos){
520  stringstream ss;
521  ss << k1;
522  line = "k1 " + string(ss.str());
523  }
524  // replace k2 line
525  if (line.find("k2",0) != string::npos){
526  stringstream ss;
527  ss << k2;
528  line = "k2 " + string(ss.str());
529  }
530  // replace p1 line
531  if (line.find("p1",0) != string::npos){
532  stringstream ss;
533  ss << p1;
534  line = "p1 " + string(ss.str());
535  }
536  // replace p2 line
537  if (line.find("p2",0) != string::npos){
538  stringstream ss;
539  ss << p2;
540  line = "p2 " + string(ss.str());
541  }
542 
543  }
544  // buffer line
545  lines.push_back(line);
546  }
547 
548  in.close();
549 
550  // rewrite file
551  if (!sectionFound){
552  append = true;
553  cout << "Camera calibration parameter section " + string("[") + groupname + string("]") + " not found in file " << camCalibFile << ". Adding group..." << endl;
554  }
555  else{
556  // rewrite file
557  ofstream out;
558  out.open(camCalibFile.c_str(), ios::trunc);
559  if (out.is_open()){
560  for (int i = 0; i < (int)lines.size(); i++)
561  out << lines[i] << endl;
562  out.close();
563  }
564  else
565  return false;
566  }
567 
568  }
569  else{
570  append = true;
571  }
572 
573  if (append){
574  // file doesn't exist or section is appended
575  ofstream out;
576  out.open(camCalibFile.c_str(), ios::app);
577  if (out.is_open()){
578  out << string("[") + groupname + string("]") << endl;
579  out << endl;
580  out << "w " << width << endl;
581  out << "h " << height << endl;
582  out << "fx " << fx << endl;
583  out << "fy " << fy << endl;
584  out << "cx " << cx << endl;
585  out << "cy " << cy << endl;
586  out << "k1 " << k1 << endl;
587  out << "k2 " << k2 << endl;
588  out << "p1 " << p1 << endl;
589  out << "p2 " << p2 << endl;
590  out << endl;
591  out.close();
592  }
593  else
594  return false;
595  }
596 
597  return true;
598 }
599 
600 void stereoCalibThread::monoCalibration(const vector<string>& imageList, int boardWidth, int boardHeight, Mat &K, Mat &Dist)
601 {
602  vector<vector<Point2f> > imagePoints;
603  Size boardSize, imageSize;
604  boardSize.width=boardWidth;
605  boardSize.height=boardHeight;
606  int flags=0;
607  int i;
608 
609  float squareSize = 1.f, aspectRatio = 1.f;
610 
611  Mat view, viewGray;
612 
613  for(i = 0; i<(int)imageList.size();i++)
614  {
615 
616  view = cv::imread(imageList[i], 1);
617  imageSize = view.size();
618  vector<Point2f> pointbuf;
619  cvtColor(view, viewGray, CV_BGR2GRAY);
620 
621  bool found = false;
622  if(boardType == "CIRCLES_GRID") {
623  found = findCirclesGrid(view, boardSize, pointbuf, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
624  } else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
625  found = findCirclesGrid(view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
626  } else {
627  found = findChessboardCorners( view, boardSize, pointbuf,
628  CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
629  }
630 
631  if(found)
632  {
633  drawChessboardCorners( view, boardSize, Mat(pointbuf), found );
634  imagePoints.push_back(pointbuf);
635  }
636 
637  }
638  std::vector<Mat> rvecs, tvecs;
639  std::vector<float> reprojErrs;
640  double totalAvgErr = 0;
641 
642  K = Mat::eye(3, 3, CV_64F);
643  if( flags & CV_CALIB_FIX_ASPECT_RATIO )
644  K.at<double>(0,0) = aspectRatio;
645 
646  Dist = Mat::zeros(4, 1, CV_64F);
647 
648  std::vector<std::vector<Point3f> > objectPoints(1);
649  calcChessboardCorners(boardSize, squareSize, objectPoints[0]);
650  objectPoints.resize(imagePoints.size(),objectPoints[0]);
651 
652  double rms = calibrateCamera(objectPoints, imagePoints, imageSize, K,
653  Dist, rvecs, tvecs,CV_CALIB_FIX_K3);
654  yInfo("RMS error reported by calibrateCamera: %g\n", rms);
655  cout.flush();
656 }
657 
658 
659 void stereoCalibThread::stereoCalibration(const vector<string>& imagelist, int boardWidth, int boardHeight,float sqsize)
660 {
661  Size boardSize;
662  boardSize.width=boardWidth;
663  boardSize.height=boardHeight;
664  if( imagelist.size() % 2 != 0 )
665  {
666  cout << "Error: the image list contains odd (non-even) number of elements\n";
667  return;
668  }
669 
670  const int maxScale = 2;
671  // ARRAY AND VECTOR STORAGE:
672 
673  std::vector<std::vector<Point2f> > imagePoints[2];
674  Size imageSize;
675 
676  int i, j, k, nimages = (int)imagelist.size()/2;
677 
678  imagePoints[0].resize(nimages);
679  imagePoints[1].resize(nimages);
680  std::vector<string> goodImageList;
681  bool differentSizes = false;
682 
683  for( i = j = 0; i < nimages; i++ )
684  {
685  for( k = 0; k < 2; k++ )
686  {
687  const string& filename = imagelist[i*2+k];
688  Mat img = cv::imread(filename, 0);
689  if(img.empty())
690  break;
691  if( imageSize == Size() )
692  imageSize = img.size();
693  else if( img.size() != imageSize )
694  {
695  yWarning() <<"The image " << filename << " has the size different from the first image size.\n";
696  differentSizes = true;
697  }
698  bool found = false;
699  std::vector<Point2f>& corners = imagePoints[k][j];
700  for( int scale = 1; scale <= maxScale; scale++ )
701  {
702  Mat timg;
703  if( scale == 1 )
704  timg = img;
705  else
706  resize(img, timg, Size(), scale, scale);
707 
708  if(boardType == "CIRCLES_GRID") {
709  found = findCirclesGrid(timg, boardSize, corners, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
710  } else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
711  found = findCirclesGrid(timg, boardSize, corners, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
712  } else {
713  found = findChessboardCorners(timg, boardSize, corners,
714  CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
715  }
716 
717  if( found )
718  {
719  if( scale > 1 )
720  {
721  Mat cornersMat(corners);
722  cornersMat *= 1./scale;
723  }
724  break;
725  }
726  }
727  if( !found )
728  break;
729  }
730  if( k == 2 )
731  {
732  goodImageList.push_back(imagelist[i*2]);
733  goodImageList.push_back(imagelist[i*2+1]);
734  j++;
735  }
736  }
737  yInfo("%i pairs have been successfully detected.\n",j);
738  nimages = j;
739  if( nimages < 2 )
740  {
741  yError("Error: too few pairs detected \n");
742  return;
743  }
744 
745  imagePoints[0].resize(nimages);
746  imagePoints[1].resize(nimages);
747 
748  std::vector<std::vector<Point3f> > objectPoints(1);
749  calcChessboardCorners(boardSize, squareSize, objectPoints[0]);
750  objectPoints.resize(nimages, objectPoints[0]);
751 
752  yInfo("Running stereo calibration ...\n");
753 
754  Mat cameraMatrix[2], distCoeffs[2];
755  Mat E, F;
756  if(this->Kleft.empty() || this->Kright.empty())
757  {
758  if (differentSizes){
759  yError("Images have different sizes. Please make sure to compute intrinsic parameters before running stereo calibration. Quitting...");
760  exit (-1);
761  }
762  double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
763  this->Kleft, this->DistL,
764  this->Kright, this->DistR,
765  imageSize, this->R, this->T, E, F,
766  #ifdef OPENCV_GREATER_2
767  CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH + CV_CALIB_FIX_K3,
768  TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 100, 1e-5));
769  #else
770  TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 100, 1e-5),
771  CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH + CV_CALIB_FIX_K3);
772  #endif
773  yInfo("done with RMS error= %f\n",rms);
774  } else
775  {
776  yInfo("Using precomputed intrinsic parameters");
777  double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
778  this->Kleft, this->DistL,
779  this->Kright, this->DistR,
780  imageSize, this->R, this->T, E, F,
781  #ifdef OPENCV_GREATER_2
782  CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_FIX_INTRINSIC + CV_CALIB_FIX_K3);
783  TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 100, 1e-5),
784  #else
785  TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 100, 1e-5),
786  CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_FIX_INTRINSIC + CV_CALIB_FIX_K3);
787  #endif
788  yInfo("done with RMS error= %f\n",rms);
789  }
790 // CALIBRATION QUALITY CHECK
791  cameraMatrix[0] = this->Kleft;
792  cameraMatrix[1] = this->Kright;
793  distCoeffs[0]=this->DistL;
794  distCoeffs[1]=this->DistR;
795  Mat R, T;
796  T=this->T;
797  R=this->R;
798  double err = 0;
799  int npoints = 0;
800  std::vector<Vec3f> lines[2];
801  for( i = 0; i < nimages; i++ )
802  {
803  int npt = (int)imagePoints[0][i].size();
804  Mat imgpt[2];
805  for( k = 0; k < 2; k++ )
806  {
807  imgpt[k] = Mat(imagePoints[k][i]);
808  undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
809  computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
810  }
811  for( j = 0; j < npt; j++ )
812  {
813  double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
814  imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
815  fabs(imagePoints[1][i][j].x*lines[0][j][0] +
816  imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
817  err += errij;
818  }
819  npoints += npt;
820  }
821  yInfo("average reprojection err = %f\n",err/npoints);
822  cout.flush();
823 }
824 
825 
826 void stereoCalibThread::saveCalibration(const string& extrinsicFilePath, const string& intrinsicFilePath){
827 
828  if( Kleft.empty() || Kright.empty() || DistL.empty() || DistR.empty() || R.empty() || T.empty()) {
829  cout << "Error: cameras are not calibrated! Run the calibration or set intrinsic and extrinsic parameters \n";
830  return;
831  }
832 
833  FileStorage fs(intrinsicFilePath+".yml", cv::FileStorage::Mode::WRITE);
834  if( fs.isOpened() )
835  {
836  fs << "M1" << Kleft << "D1" << DistL << "M2" << Kright << "D2" << DistR;
837  fs.release();
838  }
839  else
840  cout << "Error: can not save the intrinsic parameters\n";
841 
842  fs.open(extrinsicFilePath+".yml", cv::FileStorage::Mode::WRITE);
843  if( fs.isOpened() )
844  {
845  fs << "R" << R << "T" << T <<"Q" << Q;
846  fs.release();
847  }
848  else
849  cout << "Error: can not save the intrinsic parameters\n";
850 
851 }
852 
853 void stereoCalibThread::calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)
854 {
855  corners.resize(0);
856 
857  if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
858  for( int i = 0; i < boardSize.height; i++ )
859  for( int j = 0; j < boardSize.width; j++ )
860  corners.push_back(Point3f(float((2*j + i % 2)*squareSize), float(i*squareSize), 0));
861  } else {
862  for( int i = 0; i < boardSize.height; i++ )
863  for( int j = 0; j < boardSize.width; j++ )
864  corners.push_back(Point3f(float(j*squareSize),
865  float(i*squareSize), 0));
866  }
867 }
868 
869 bool stereoCalibThread::updateExtrinsics(Mat Rot, Mat Tr, const string& groupname)
870 {
871  std::vector<string> lines;
872  bool append = false;
873 
874  ifstream in;
875  in.open(camCalibFile.c_str()); //camCalibFile.c_str());
876 
877  if(in.is_open()){
878  // file exists
879  string line;
880  bool sectionFound = false;
881  bool sectionClosed = false;
882 
883  // process lines
884  while(std::getline(in, line)){
885  // check if we left calibration section
886  if (sectionFound == true && line.find("[", 0) != string::npos)
887  sectionClosed = true; // also valid if no groupname specified
888  // check if we enter calibration section
889  if (line.find(string("[") + groupname + string("]"), 0) != string::npos)
890  sectionFound = true;
891  // if no groupname specified
892  if (groupname == "")
893  sectionFound = true;
894  // if we are in calibration section (or no section/group specified)
895  if (sectionFound == true && sectionClosed == false){
896  // replace w line
897  if (line.find("HN",0) != string::npos){
898  stringstream ss;
899  ss << " (" << Rot.at<double>(0,0) << " " << Rot.at<double>(0,1) << " " << Rot.at<double>(0,2) << " " << Tr.at<double>(0,0) << " "
900  << Rot.at<double>(1,0) << " " << Rot.at<double>(1,1) << " " << Rot.at<double>(1,2) << " " << Tr.at<double>(1,0) << " "
901  << Rot.at<double>(2,0) << " " << Rot.at<double>(2,1) << " " << Rot.at<double>(2,2) << " " << Tr.at<double>(2,0) << " "
902  << 0.0 << " " << 0.0 << " " << 0.0 << " " << 1.0 << ")";
903  line = "HN" + string(ss.str());
904  }
905 
906  if (!standalone) {
907  if (line.find("QL", 0) != string::npos) {
908  line = "QL (" + string(qL.toString().c_str()) + ")";
909  }
910  if (line.find("QR", 0) != string::npos) {
911  line = "QR (" + string(qR.toString().c_str()) + ")";
912  }
913  }
914  }
915  // buffer line
916  lines.push_back(line);
917  }
918 
919  in.close();
920 
921  // rewrite file
922  if (!sectionFound){
923  append = true;
924  cout << "Camera calibration parameter section " + string("[") + groupname + string("]") + " not found in file " << camCalibFile << ". Adding group..." << endl;
925  }
926  else{
927  // rewrite file
928  ofstream out;
929  out.open(camCalibFile.c_str(), ios::trunc);
930  if (out.is_open()){
931  for (int i = 0; i < (int)lines.size(); i++)
932  out << lines[i] << endl;
933  out.close();
934  }
935  else
936  return false;
937  }
938 
939  }
940  else{
941  append = true;
942  }
943 
944  if (append){
945  // file doesn't exist or section is appended
946  ofstream out;
947  out.open(camCalibFile.c_str(), ios::app);
948  if (out.is_open()){
949  out << endl;
950  out << string("[") + groupname + string("]") << endl;
951  out << "HN (" << Rot.at<double>(0,0) << " " << Rot.at<double>(0,1) << " " << Rot.at<double>(0,2) << " " << Tr.at<double>(0,0) << " "
952  << Rot.at<double>(1,0) << " " << Rot.at<double>(1,1) << " " << Rot.at<double>(1,2) << " " << Tr.at<double>(1,0) << " "
953  << Rot.at<double>(2,0) << " " << Rot.at<double>(2,1) << " " << Rot.at<double>(2,2) << " " << Tr.at<double>(2,0) << " "
954  << 0.0 << " " << 0.0 << " " << 0.0 << " " << 1.0 << ")";
955  out << endl;
956  out << "QL (" << qL.toString().c_str() << ")" << endl;
957  out << "QR (" << qR.toString().c_str() << ")" << endl;
958  out.close();
959  }
960  else
961  return false;
962  }
963 
964  return true;
965 }
stereoCalibThread::stereoCalibThread
stereoCalibThread(ResourceFinder &rf, Port *commPort, const char *imageDir)
Definition: stereoCalibThread.cpp:6
e
e
Definition: compute_ekf_fast.m:13
F
F
Definition: compute_ekf_fast.m:22
zeros
zeros(2, 2) eye(2
x
x
Definition: compute_ekf_sym.m:21
T
T
Definition: compute_ekf_sym.m:12
LEFT
#define LEFT
Definition: utils.h:33
RIGHT
#define RIGHT
Definition: utils.h:34
iCub::ctrl::dbscan::append
void append(const size_t index, shared_ptr< Epsilon_neighbours_t > en)
Definition: clustering.cpp:65
iCub::ctrl::CTRL_DEG2RAD
constexpr double CTRL_DEG2RAD
PI/180.
Definition: math.h:66
y
y
Definition: show_eyes_axes.m:21
view
view(3)
out
out
Definition: sine.m:8
Generated on Sun Apr 28 2024 20:27:48 for iCub-main by doxygen 1.9.1