1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
|
#include <cv.h>
#include <highgui.h>
#include "opencv2/core/core.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "opencv2/contrib/contrib.hpp"
#include <stdio.h>
using namespace cv;
using namespace std;
int main()
{
int numBoards = 0;
int numCornersHor;
int numCornersVer;
int cmpt = 1;
bool calibration = false;
bool flagCalibration = false;
bool cameraCalibre = false;
bool pause = false;
printf("Entrez le nombre de coins pour la ligne horizontale: "); // nombre de coins sur la ligne horizontale
scanf("%d", &numCornersHor);
printf("Entrez le nombre de coins pour la ligne verticale: "); // nombre de coins sur la ligne verticale
scanf("%d", &numCornersVer);
printf("Entrez le nombre de cliches a prendre: "); // nombre de clichés (de calibration) à prendre
scanf("%d", &numBoards);
int numSquares = numCornersHor * numCornersVer; // nombre de carrés du damier, selon le nombre de coins
Size board_sz = Size(numCornersHor, numCornersVer);
string filename1 = "C:\\Users\\Jordan\\Downloads\\OpenCV(ComputerVision)\\videos\\damier1.avi"; // lecture vidéo
string filename2 = "C:\\Users\\Jordan\\Downloads\\OpenCV(ComputerVision)\\videos\\damier1.avi"; // lecture vidéo
VideoCapture capture1, capture2;// = VideoCapture(filename);
vector<vector<Point3f>> object_points; // projection des points 3D
vector<vector<Point2f>> image_points1, image_points2; // projection des points 2D
vector<Point2f> corners1, corners2; // nombre de coins
vector<Mat> rvecs, tvecs; // vecteurs de matrices pour les translations de points
Mat fenetreLecture1, fenetreLecture2, fenetreCalibration1, fenetreCalibration2, ImageCalibre; // matrices pour chaque vidéos
Mat intrinsic1, intrinsic2, distCoeffs; // matrices pour le calibrage
Mat D1, D2;
Mat R, T, E, F;
Mat R1, R2, P1, P2, Q;
Mat map1x, map1y, map2x, map2y;
Mat imgU1, imgU2;
int successes = 0; // nombre de clichés pris avec succès
int action = 0;
// bouclage de la vidéo
while (1)
{
capture1 = VideoCapture(filename1); // lecture de la vidéo tant qu'on ne sort pas de la boucle
capture2 = VideoCapture(filename2); // lecture de la vidéo tant qu'on ne sort pas de la boucle
if (!capture1.isOpened())// si impossible d'ouvrir la vidéo 1
{
cout << "\nImpossible d'ouvrir la video 1!\n"; // message d'erreur
getchar(); // attends une action quelconque
return -1;
}
if (!capture2.isOpened())// si impossible d'ouvrir la vidéo 2
{
cout << "\nImpossible d'ouvrir la video 2!\n"; // message d'erreur
getchar(); // attends une action quelconque
return -1;
}
// lecture de chaque image des vidéos
while ( (capture1.get(CV_CAP_PROP_POS_FRAMES)<capture1.get(CV_CAP_PROP_FRAME_COUNT)-1) ) // inutile de mettre la condition pour la vidéo 2
{
capture1 >> fenetreLecture1; // envoie de la vidéo 1 dans la fenêtre 1
capture2 >> fenetreLecture2; // envoie de la vidéo 2 dans la fenêtre 2
imshow("Fenêtre de lecture 1", fenetreLecture1); // affichage de celle-ci
imshow("Fenêtre de lecture 2", fenetreLecture2); // affichage de celle-ci
// passage en mode calibration (touche 'c')
if (calibration)
{
vector<Point3f> obj;
for (int j = 0; j<numSquares; j++)
obj.push_back(Point3f(j / numCornersHor, j%numCornersHor, 0.0f));
while (successes<numBoards) // tant que le nombre de clichés pris avec succès est inférieur au nombre de clichés max, boucler
{
cvtColor(fenetreLecture1, fenetreCalibration1, CV_BGR2GRAY);
cvtColor(fenetreLecture2, fenetreCalibration2, CV_BGR2GRAY);
bool found1 = findChessboardCorners(fenetreLecture1, board_sz, corners1, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
bool found2 = findChessboardCorners(fenetreLecture2, board_sz, corners2, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
if (found1)
{
cornerSubPix(fenetreCalibration1, corners1, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
drawChessboardCorners(fenetreCalibration1, board_sz, corners1, found1);
}
if (found2)
{
cornerSubPix(fenetreCalibration1, corners2, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
drawChessboardCorners(fenetreCalibration2, board_sz, corners2, found2);
}
imshow("Fenêtre de calibration 1", fenetreCalibration1);
imshow("Fenêtre de calibration 2", fenetreCalibration2);
capture1 >> fenetreLecture1;
capture2 >> fenetreLecture2;
int key = waitKey(20);
if (key == 27) // 'esc'
return 0;
if ( key == ' ' && (found1 != 0) && (found2 != 0) ) // si : clique sur "espace" ET damier reconnu alors : image de calibration prise
{
image_points1.push_back(corners1);
image_points2.push_back(corners2);
object_points.push_back(obj);
printf("Cliche %d pris avec succes !\n", cmpt);
successes++;
cmpt++;
if (successes >= numBoards)
break;
}
}
// traitement des fenêtres calibrées
intrinsic1 = Mat(3, 3, CV_32FC1);
intrinsic2 = Mat(3, 3, CV_32FC1);
stereoCalibrate(object_points, image_points1, image_points2,
intrinsic1, D1, intrinsic2, D2, fenetreLecture1.size(), R, T, E, F,
cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5),
CV_CALIB_SAME_FOCAL_LENGTH | CV_CALIB_ZERO_TANGENT_DIST);
calibration = false; // changement d'état du mode de calibration
flagCalibration = true; // le mode calibration ne sera plus valide
cameraCalibre = true; // la caméra est maintenant calibrée
destroyWindow("Fenêtre de calibration 1");
destroyWindow("Fenêtre de calibration 2");
FileStorage fs1("mystereocalib.yml", FileStorage::WRITE);
fs1 << "CM1" << intrinsic1;
fs1 << "CM2" << intrinsic2;
fs1 << "D1" << D1;
fs1 << "D2" << D2;
fs1 << "R" << R;
fs1 << "T" << T;
fs1 << "E" << E;
fs1 << "F" << F;
printf("Done Calibration\n");
printf("Starting Rectification\n");
stereoRectify(intrinsic1, D1, intrinsic2, D2, fenetreLecture1.size(), R, T, R1, R2, P1, P2, Q);
fs1 << "R1" << R1;
fs1 << "R2" << R2;
fs1 << "P1" << P1;
fs1 << "P2" << P2;
fs1 << "Q" << Q;
printf("Done Rectification\n");
printf("Applying Undistort\n");
initUndistortRectifyMap(intrinsic1, D1, R1, P1, fenetreLecture1.size(), CV_32FC1, map1x, map1y);
initUndistortRectifyMap(intrinsic2, D2, R2, P2, fenetreLecture2.size(), CV_32FC1, map2x, map2y);
printf("Undistort complete\n");
} // fin calibration
// apres calibration de la caméra
if (cameraCalibre)
{
capture1 >> fenetreLecture1;
capture2 >> fenetreLecture2;
remap(fenetreLecture1, imgU1, map1x, map1y, INTER_LINEAR, BORDER_CONSTANT, Scalar());
remap(fenetreLecture2, imgU2, map2x, map2y, INTER_LINEAR, BORDER_CONSTANT, Scalar());
imshow("FENÊTRE CALIBREE 1", imgU1);
imshow("FENÊTRE CALIBREE 2", imgU2);
// différentes actions en entrée standard
switch (waitKey(40))
{
case 27: // 'escape' : sort du programme
return -1;
case 'c':
calibration = true; // changement d'état, en mode 'calibration'
cout << "Passage en mode 'Calibration'. Appyez sur 'espace' pour prendre chaque cliche." << endl;
break;
case 112: // 'p' : met ou sort de pause
pause = true; // changement d'état, en mode 'pause'
if (pause){
cout << "Mode pause. Appuyez 'p' pour reprendre !" << endl;
while (pause){ // boucle à l'infini
switch (waitKey()){ // attend sans rien faire
case 112:
pause = false; // changement d'état, sort de 'pause'
cout << "Finie la pause. Reprenons !" << endl;
break;
}
}
}
default:
break;
}
}
// différentes actions en entrée standard LORSQUE LA CAMERA N'EST PAS CALIBREE
switch (waitKey(40))
{
case 27: // 'escape' : sort du programme
return -1;
case 'c':
if (!flagCalibration) // la calibration ne peut être exécutée qu'une seule fois
{
calibration = true; // changement d'état, en mode 'calibration'
cout << "Passage en mode 'Calibration'." << endl;
}
break;
case 112: // 'p' : met ou sort de pause
pause = true; // changement d'état, en mode 'pause'
if (pause){
cout << "Mode pause. Appuyez 'p' pour reprendre !" << endl;
while (pause){ // boucle à l'infini
switch (waitKey()){ // attend sans rien faire
case 112:
pause = false; // changement d'état, sort de 'pause'
cout << "Finie la pause. Reprenons !" << endl;
break;
}
}
}
default:
break;
}
}
capture1.release();
capture2.release();
} // fin while(1)
//system("pause");
return 0;
} |
Rendu après Stereocalibrate
Répondre avec citation
Partager