iCub-main
Loading...
Searching...
No Matches
stereoCalibThread.cpp
Go to the documentation of this file.
1#include <utility>
2#include <yarp/cv/Cv.h>
3#include "stereoCalibThread.h"
4
5
6stereoCalibThread::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
42bool 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}
120void 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}
133void 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
259void 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 }
357void 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
372void stereoCalibThread::onStop() {
373 startCalibration=0;
374 imagePortInRight.interrupt();
375 imagePortInLeft.interrupt();
376 outPortLeft.interrupt();
377 outPortRight.interrupt();
378 commandPort->interrupt();
379
380}
381void stereoCalibThread::startCalib() {
382 lock_guard<mutex> lck(mtx);
383 startCalibration=1;
384 }
385
386void stereoCalibThread::stopCalib() {
387 lock_guard<mutex> lck(mtx);
388 startCalibration=0;
389 }
390
391void 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
406bool 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
414void 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
434void 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
445void 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
454bool 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
600void 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
659void 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
826void 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
853void 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
869bool 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
x
x
Definition compute_ekf_sym.m:21
T
T
Definition compute_ekf_sym.m:12
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
LEFT
#define LEFT
Definition stereoCalibThread.h:30
RIGHT
#define RIGHT
Definition stereoCalibThread.h:31
Generated on Wed Dec 18 2024 16:34:49 for iCub-main by doxygen 1.9.8