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
|
int main(int argc, char* argv){
remove("rotation_out.xml");
remove( "translation_out.xml");
remove( "essential_out.xml");
remove("fundamental_out.xml");
remove( "rotation_rectify_g.xml");
remove( "rotation_rectify_d.xml");
remove( "projection_rectify_g.xml");
remove( "projection_rectify_d.xml");
remove("identity.xml");
remove("new_matrix_g.xml");
remove("fundamental_ext.xml");
remove("homography_matrix_g.xml");
remove("homography_matrix_d.xml");
remove("new_point_g.xml");
remove("new_point_d.xml");
CvCapture* capture_g= cvCreateFileCapture("video_g.avi");
CvSize size_g = parametre_camera(capture_g,"calibration_g","undistort_g","intrinsic_g.xml","distortion_g.xml","rotation_g.xml","translation_g.xml","point_image_g.xml","point_objet_g.xml","count_obj_g.xml");
CvCapture* capture_d= cvCreateFileCapture("video_d.avi");
CvSize size_d = parametre_camera(capture_d,"calibration_d","undistort_d","intrinsic_d.xml","distortion_d.xml","rotation_d.xml","translation_d.xml","point_image_d.xml","point_objet_d.xml","count_obj_d.xml");
CvSize size_ut = cvSize(640,480);
CvMat* rotation_output = cvCreateMat( 3, 3, CV_64F);
CvMat* rotation_vec =cvCreateMat(3,1,CV_32FC1);
CvMat* essential_output = cvCreateMat( 3, 3, CV_64F );
CvMat* fundamental_output = cvCreateMat( 3, 3, CV_64F );
CvMat* fundamental_ext =cvCreateMat(3,3,CV_64F);
CvMat* rotation_rectify_g = cvCreateMat( 3, 3, CV_64F );
CvMat* rotation_rectify_d = cvCreateMat( 3, 3, CV_64F );
CvMat* projection_rectify_g = cvCreateMat( 3, 4, CV_64F );
CvMat* projection_rectify_d = cvCreateMat( 3, 4, CV_64F );
CvMat* disparity_rectify = cvCreateMat( 4, 4, CV_64F );
CvMat* translation_output = cvCreateMat( 3, 1, CV_64F );
CvMat* homography_matrix_g = cvCreateMat(3,3,CV_32FC1);
CvMat* homography_matrix_d = cvCreateMat(3,3,CV_64F);
CvMat* invert_g=cvCreateMat(3,3,CV_32FC1);
CvMat* multiA = cvCreateMat(3,3,CV_32FC1);
CvMat* R_final=cvCreateMat(3,3,CV_32FC1);
CvMat* rotation_matrix_g = (CvMat*)cvLoad( "rotation_g.xml" );
CvMat* rotation_matrix_d = (CvMat*)cvLoad( "rotation_d.xml" );
CvMat* intrinsic_matrix_g = (CvMat*)cvLoad("intrinsic_g.xml");
CvMat* intrinsic_matrix_d = (CvMat*)cvLoad("intrinsic_d.xml");
CvMat* distortion_matrix_g = (CvMat*)cvLoad( "distortion_g.xml" );
CvMat* distortion_matrix_d = (CvMat*)cvLoad( "distortion_d.xml" );
CvMat* translation_matrix_g = (CvMat*)cvLoad( "translation_g.xml" );
CvMat* translation_matrix_d = (CvMat*)cvLoad( "translation_d.xml" );
CvMat* point_image_g = (CvMat*)cvLoad( "point_image_g.xml" );
CvMat* point_image_d = (CvMat*)cvLoad( "point_image_d.xml" );
CvMat point_image_new_g;
CvMat point_image_new_d;
double tabgauche[480];
double tabdroite[480];
int i,j;
int k=0;
double t,y;
for(i=0 ; i<240;i++){
for(j=0;j<2;j++){
t=cvmGet(point_image_g,i,j);
tabgauche[k]=t;
y=cvmGet(point_image_d,i,j);
tabdroite[k]=y;
k++;
}
}
cvInitMatHeader(&point_image_new_g, 480, 1, CV_64F, tabgauche);
cvInitMatHeader(&point_image_new_d, 480, 1, CV_64F, tabdroite);
cvSave("new_point_g.xml",&point_image_new_g);
cvSave("new_point_d.xml",&point_image_new_d);
CvMat* point_objet = (CvMat*)cvLoad( "point_objet_g.xml" );
CvMat* count_objet_g = (CvMat*)cvLoad( "count_obj_g.xml" );
CvMat* count_objet_d = (CvMat*)cvLoad( "count_obj_d.xml" );
cvStereoCalibrate(point_objet,point_image_g,point_image_d,count_objet_g,intrinsic_matrix_g,distortion_matrix_g,intrinsic_matrix_d,distortion_matrix_d,size_ut,rotation_output,translation_output,essential_output,fundamental_output,cvTermCriteria(1+2,30,0.000001),CV_CALIB_USE_INTRINSIC_GUESS);
cvSave( "rotation_out.xml", rotation_output );
cvSave( "translation_out.xml",translation_output);
cvSave( "essential_out.xml", essential_output );
cvSave( "fundamental_out.xml",fundamental_output );
cvStereoRectify(intrinsic_matrix_g,intrinsic_matrix_d,distortion_matrix_g,distortion_matrix_d,size_ut,rotation_output,translation_output,rotation_rectify_g,rotation_rectify_d,projection_rectify_g,projection_rectify_d,disparity_rectify,CV_CALIB_ZERO_DISPARITY);
cvSave( "rotation_rectify_g.xml", rotation_rectify_g );
cvSave( "rotation_rectify_d.xml",rotation_rectify_d);
cvSave( "projection_rectify_g.xml",projection_rectify_g );
cvSave( "projection_rectify_d.xml",projection_rectify_d);
double tableau[]={1,0,0,0,1,0,0,0,1,0,0,0};
int u;
for(u=0;u<12;u++){
printf("%f \n",tableau[u]);
}
CvMat* identity=cvCreateMat(4,3,CV_64F);
cvInitMatHeader(identity,4,3,CV_64F,tableau);
CvMat* new_matrix_g = cvCreateMat(3,3,CV_64F);
cvMatMul(projection_rectify_g,identity,new_matrix_g);
CvMat* mapX =cvCreateMat(size_ut.height,size_ut.width,CV_16SC2);
CvMat* mapY =cvCreateMat(size_ut.height,size_ut.width, CV_16UC1);
cvSave("identity.xml",identity);
cvSave("new_matrix_g.xml",new_matrix_g);
cvInitUndistortRectifyMap(intrinsic_matrix_g,distortion_matrix_g,rotation_rectify_g,new_matrix_g,mapX,mapY);
CvCapture* capture_new_g= cvCreateFileCapture("video_g.avi");
cvNamedWindow( "rectification_new",1 );
assert( capture_new_g );
if(!capture_new_g){
return -1;
}
IplImage *bgr_frame = cvQueryFrame( capture_new_g );
IplImage *image_remap= cvCreateImage( cvGetSize(bgr_frame),8, 3 );
while((bgr_frame = cvQueryFrame(capture_new_g))!=NULL){
cvRemap(bgr_frame,image_remap,mapX,mapY);
cvShowImage("rectification_new",image_remap);
cvWaitKey(10);
}
} |
