1 #include <src/Tsai/TsaiCameraCalibration.h>
2
3 #include <vcg/math/matrix44.h>
4 #include <string>
5 #include <sstream>
6
7 const QString TsaiCameraCalibration::XML_TSAICALIB_TYPE = "TSAI";
8 const QString TsaiCameraCalibration::XML_TSAICALIB_OPTIMIZED = "optimized";
9
10 const QString TsaiCameraCalibration::XML_TSAICALIB_SENSOR = "Sensor";
11 const QString TsaiCameraCalibration::XML_TSAICALIB_DPX = "dpx";
12 const QString TsaiCameraCalibration::XML_TSAICALIB_DPY = "dpy";
13 const QString TsaiCameraCalibration::XML_TSAICALIB_SX = "sx";
14 const QString TsaiCameraCalibration::XML_TSAICALIB_CX = "Cx";
15 const QString TsaiCameraCalibration::XML_TSAICALIB_CY = "Cy";
16 const QString TsaiCameraCalibration::XML_TSAICALIB_CC = "CC";
17 const QString TsaiCameraCalibration::XML_TSAICALIB_F = "f";
18 const QString TsaiCameraCalibration::XML_TSAICALIB_KAPPA1 = "kappa1";
19 const QString TsaiCameraCalibration::XML_TSAICALIB_P1 = "p1";
20 const QString TsaiCameraCalibration::XML_TSAICALIB_P2 = "p2";
21
22 const QString TsaiCameraCalibration::XML_TSAICALIB_ROTATION = "Rotation";
23 const QString TsaiCameraCalibration::XML_TSAICALIB_RX = "Rx";
24 const QString TsaiCameraCalibration::XML_TSAICALIB_RY = "Ry";
25 const QString TsaiCameraCalibration::XML_TSAICALIB_RZ = "Rz";
26
27 const QString TsaiCameraCalibration::XML_TSAICALIB_TRANSLATION = "Translation";
28 const QString TsaiCameraCalibration::XML_TSAICALIB_TX = "Tx";
29 const QString TsaiCameraCalibration::XML_TSAICALIB_TY = "Ty";
30 const QString TsaiCameraCalibration::XML_TSAICALIB_TZ = "Tz";
31
32 const QString TsaiCameraCalibration::XML_TSAICALIB_MATRIX = "Matrix";
33 const QString TsaiCameraCalibration::XML_TSAICALIB_R1 = "r1";
34 const QString TsaiCameraCalibration::XML_TSAICALIB_R2 = "r2";
35 const QString TsaiCameraCalibration::XML_TSAICALIB_R3 = "r3";
36 const QString TsaiCameraCalibration::XML_TSAICALIB_R4 = "r4";
37 const QString TsaiCameraCalibration::XML_TSAICALIB_R5 = "r5";
38 const QString TsaiCameraCalibration::XML_TSAICALIB_R6 = "r6";
39 const QString TsaiCameraCalibration::XML_TSAICALIB_R7 = "r7";
40 const QString TsaiCameraCalibration::XML_TSAICALIB_R8 = "r8";
41 const QString TsaiCameraCalibration::XML_TSAICALIB_R9 = "r9";
42
TsaiCameraCalibration()43 TsaiCameraCalibration::TsaiCameraCalibration(){
44 type = TsaiCameraCalibration::XML_TSAICALIB_TYPE;
45 optimized = false;
46 }
~TsaiCameraCalibration()47 TsaiCameraCalibration::~TsaiCameraCalibration(){
48
49 }
50
51
getUVforPoint(double x,double y,double z,double * u,double * v)52 void TsaiCameraCalibration::getUVforPoint(double x, double y, double z, double *u, double *v){
53 world_coord_to_image_coord_cp_cc(&cam_para,&calib_const, x,y,z,u,v);
54 //qDebug("u: %f\tv: %f\n",*u,*v);
55 }
56
loadFromXml(QDomElement * xml_cam)57 void TsaiCameraCalibration::loadFromXml(QDomElement *xml_cam){
58
59 QString soptimized = xml_cam->attribute(TsaiCameraCalibration::XML_TSAICALIB_OPTIMIZED);
60 if(soptimized.toInt()==1){
61 optimized = true;
62 }else{
63 optimized = false;
64 }
65
66
67 QDomElement xml_sensor = xml_cam->firstChildElement(TsaiCameraCalibration::XML_TSAICALIB_SENSOR);
68 QString dpx = xml_sensor.attribute(TsaiCameraCalibration::XML_TSAICALIB_DPX);
69 QString dpy = xml_sensor.attribute(TsaiCameraCalibration::XML_TSAICALIB_DPY);
70 QString sx = xml_sensor.attribute(TsaiCameraCalibration::XML_TSAICALIB_SX);
71 QString Cx = xml_sensor.attribute(TsaiCameraCalibration::XML_TSAICALIB_CX);
72 QString Cy = xml_sensor.attribute(TsaiCameraCalibration::XML_TSAICALIB_CY);
73
74 cam_para.dpx = dpx.toDouble();
75 cam_para.dpy = dpy.toDouble();
76 cam_para.sx = sx.toDouble();
77 cam_para.Cx = Cx.toDouble();
78 cam_para.Cy = Cy.toDouble();
79
80 QDomElement xml_cc = xml_cam->firstChildElement(TsaiCameraCalibration::XML_TSAICALIB_CC);
81 QString f = xml_cc.attribute(TsaiCameraCalibration::XML_TSAICALIB_F);
82 QString kappa1 = xml_cc.attribute(TsaiCameraCalibration::XML_TSAICALIB_KAPPA1);
83 QString p1 = xml_cc.attribute(TsaiCameraCalibration::XML_TSAICALIB_P1);
84 QString p2 = xml_cc.attribute(TsaiCameraCalibration::XML_TSAICALIB_P2);
85
86 calib_const.f = f.toDouble();
87 calib_const.kappa1 = kappa1.toDouble();
88 //calib_const.p1 = p1.toDouble();
89 //calib_const.p2 = p2.toDouble();
90
91 QDomElement xml_rotation = xml_cam->firstChildElement(TsaiCameraCalibration::XML_TSAICALIB_ROTATION);
92 QString rx = xml_rotation.attribute(TsaiCameraCalibration::XML_TSAICALIB_RX);
93 QString ry = xml_rotation.attribute(TsaiCameraCalibration::XML_TSAICALIB_RY);
94 QString rz = xml_rotation.attribute(TsaiCameraCalibration::XML_TSAICALIB_RZ);
95
96 calib_const.Rx = rx.toDouble();
97 calib_const.Ry = ry.toDouble();
98 calib_const.Rz = rz.toDouble();
99
100 QDomElement xml_translation = xml_cam->firstChildElement(TsaiCameraCalibration::XML_TSAICALIB_TRANSLATION);
101 QString tx = xml_translation.attribute(TsaiCameraCalibration::XML_TSAICALIB_TX);
102 QString ty = xml_translation.attribute(TsaiCameraCalibration::XML_TSAICALIB_TY);
103 QString tz = xml_translation.attribute(TsaiCameraCalibration::XML_TSAICALIB_TZ);
104
105 calib_const.Tx = tx.toDouble();
106 calib_const.Ty = ty.toDouble();
107 calib_const.Tz = tz.toDouble();
108
109 QDomElement xml_matrix = xml_cam->firstChildElement(TsaiCameraCalibration::XML_TSAICALIB_MATRIX);
110 QString r1 = xml_matrix.attribute(TsaiCameraCalibration::XML_TSAICALIB_R1);
111 QString r2 = xml_matrix.attribute(TsaiCameraCalibration::XML_TSAICALIB_R2);
112 QString r3 = xml_matrix.attribute(TsaiCameraCalibration::XML_TSAICALIB_R3);
113 QString r4 = xml_matrix.attribute(TsaiCameraCalibration::XML_TSAICALIB_R4);
114 QString r5 = xml_matrix.attribute(TsaiCameraCalibration::XML_TSAICALIB_R5);
115 QString r6 = xml_matrix.attribute(TsaiCameraCalibration::XML_TSAICALIB_R6);
116 QString r7 = xml_matrix.attribute(TsaiCameraCalibration::XML_TSAICALIB_R7);
117 QString r8 = xml_matrix.attribute(TsaiCameraCalibration::XML_TSAICALIB_R8);
118 QString r9 = xml_matrix.attribute(TsaiCameraCalibration::XML_TSAICALIB_R9);
119
120 calib_const.r1 = r1.toDouble();
121 calib_const.r2 = r2.toDouble();
122 calib_const.r3 = r3.toDouble();
123 calib_const.r4 = r4.toDouble();
124 calib_const.r5 = r5.toDouble();
125 calib_const.r6 = r6.toDouble();
126 calib_const.r7 = r7.toDouble();
127 calib_const.r8 = r8.toDouble();
128 calib_const.r9 = r9.toDouble();
129
130 double dmatrix[16];
131 dmatrix[0]= calib_const.r1;
132 dmatrix[1]= calib_const.r2;
133 dmatrix[2]= calib_const.r3;
134 dmatrix[3]= calib_const.Tx;
135
136 dmatrix[4]= calib_const.r4;
137 dmatrix[5]= calib_const.r5;
138 dmatrix[6]= calib_const.r6;
139 dmatrix[7]= calib_const.Ty;
140
141 dmatrix[8]= calib_const.r7;
142 dmatrix[9]= calib_const.r8;
143 dmatrix[10]= calib_const.r9;
144 dmatrix[11]= calib_const.Tz;
145
146 dmatrix[12]= 0;
147 dmatrix[13]= 0;
148 dmatrix[14]= 0;
149 dmatrix[15]= 1;
150
151 vcg::Matrix44<double> *matrix;
152 matrix = new vcg::Matrix44<double>(dmatrix);
153
154 printf("matrix: %f\n",matrix->ElementAt(2,2));
155
156
157 vcg::Matrix44<double> invmatrix = vcg::Inverse(*matrix);
158 double dcam_cam_pos[] = {0.0,0.0,0.0,1.0};
159 vcg::Point4<double> cam_cam_pos = vcg::Point4<double>(dcam_cam_pos);
160
161 vcg::Point4<double> cam_pos = invmatrix*cam_cam_pos;
162
163 printf("camera position: %f\t%f\t%f\t%f\n",cam_pos[0],cam_pos[1],cam_pos[2],cam_pos[3]);
164
165 double dcam_cam_direction[] = {0.0,0.0,1.0,1.0};
166 vcg::Point4<double> cam_cam_direction = vcg::Point4<double>(dcam_cam_direction);
167
168 vcg::Point4<double> cam_direction = invmatrix*cam_cam_direction;
169 cam_direction -= cam_pos;
170 printf("camera direction: %f\t%f\t%f\t%f\n",cam_direction[0],cam_direction[1],cam_direction[2],cam_direction[3]);
171
172 cameraPosition[0]=cam_pos[0];
173 cameraPosition[1]=cam_pos[1];
174 cameraPosition[2]=cam_pos[2];
175
176 cameraDirection[0]=cam_direction[0];
177 cameraDirection[1]=cam_direction[1];
178 cameraDirection[2]=cam_direction[2];
179
180 }
181
saveAsXml(QDomDocument * doc,QDomElement * root)182 void TsaiCameraCalibration::saveAsXml(QDomDocument* doc,QDomElement *root){
183 QDomElement xml_calib = doc->createElement(CameraCalibration::XML_CALIBRATION);
184 xml_calib.setAttribute(CameraCalibration::XML_TYPE,TsaiCameraCalibration::XML_TSAICALIB_TYPE);
185 if(optimized){
186 xml_calib.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_OPTIMIZED,"1");
187 }else{
188 xml_calib.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_OPTIMIZED,"0");
189 }
190 QDomElement xml_sensor = doc->createElement(TsaiCameraCalibration::XML_TSAICALIB_SENSOR);
191 xml_sensor.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_DPX,QString::number(cam_para.dpx));
192 xml_sensor.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_DPY,QString::number(cam_para.dpy));
193 xml_sensor.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_CX,QString::number(cam_para.Cx));
194 xml_sensor.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_CY,QString::number(cam_para.Cy));
195 xml_sensor.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_SX,QString::number(cam_para.sx));
196 xml_calib.appendChild(xml_sensor);
197
198 QDomElement xml_cc = doc->createElement(TsaiCameraCalibration::XML_TSAICALIB_CC);
199 xml_cc.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_F,QString::number(calib_const.f));
200 xml_cc.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_KAPPA1,QString::number(calib_const.kappa1));
201 xml_cc.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_P1,QString::number(calib_const.p1));
202 xml_cc.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_P2,QString::number(calib_const.p2));
203 xml_calib.appendChild(xml_cc);
204
205 QDomElement xml_translation = doc->createElement(TsaiCameraCalibration::XML_TSAICALIB_TRANSLATION);
206 xml_translation.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_TX,QString::number(calib_const.Tx));
207 xml_translation.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_TY,QString::number(calib_const.Ty));
208 xml_translation.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_TZ,QString::number(calib_const.Tz));
209 xml_calib.appendChild(xml_translation);
210
211 QDomElement xml_rotation = doc->createElement(TsaiCameraCalibration::XML_TSAICALIB_ROTATION);
212 xml_rotation.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_RX,QString::number(calib_const.Rx));
213 xml_rotation.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_RY,QString::number(calib_const.Ry));
214 xml_rotation.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_RZ,QString::number(calib_const.Rz));
215 xml_calib.appendChild(xml_rotation);
216
217 QDomElement xml_matrix = doc->createElement(TsaiCameraCalibration::XML_TSAICALIB_MATRIX);
218 xml_matrix.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_R1,QString::number(calib_const.r1));
219 xml_matrix.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_R2,QString::number(calib_const.r2));
220 xml_matrix.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_R3,QString::number(calib_const.r3));
221 xml_matrix.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_R4,QString::number(calib_const.r4));
222 xml_matrix.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_R5,QString::number(calib_const.r5));
223 xml_matrix.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_R6,QString::number(calib_const.r6));
224 xml_matrix.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_R7,QString::number(calib_const.r7));
225 xml_matrix.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_R8,QString::number(calib_const.r8));
226 xml_matrix.setAttribute(TsaiCameraCalibration::XML_TSAICALIB_R9,QString::number(calib_const.r9));
227
228 xml_calib.appendChild(xml_matrix);
229 root->appendChild(xml_calib);
230
231 }
232
calibrate(QList<CameraCalibrationData * > & ccd)233 void TsaiCameraCalibration::calibrate(QList<CameraCalibrationData*> &ccd ){
234 calibrate(ccd, true);
235 }
236
calibrate(QList<CameraCalibrationData * > & ccd,bool optimize)237 void TsaiCameraCalibration::calibrate(QList<CameraCalibrationData*> &ccd, bool optimize){
238
239 optimized = optimize;
240
241 initialize_eos400D_parms();
242
243 cd.point_count = 0;
244 int max_points;
245
246 if (ccd.size()<MAX_POINTS){
247 max_points = ccd.size();
248 }else{
249 max_points= MAX_POINTS;
250 }
251 int k = 0;
252 //printf("malloc\n");
253 int* done = (int*)malloc(ccd.size()*sizeof(int));
254 memset(done,0,ccd.size());
255 printf("try to find points\n");
256 while (k<max_points){
257 int ran = rand() % ccd.size();
258 //printf("max_points: %d\tk: %d\n",max_points,k);
259 if (done[ran]==0){
260 done[ran]=1;
261 CameraCalibrationData* tmp_ccd;
262 tmp_ccd = ccd.at(ran);
263 cd.xw[k] = tmp_ccd->wX;
264 cd.yw[k] = tmp_ccd->wY;
265 cd.zw[k] = tmp_ccd->wZ;
266 cd.Xf[k] = tmp_ccd->iX;
267 cd.Yf[k] = tmp_ccd->iY;
268 cd.point_count++;
269 k++;
270 }
271 }
272
273 //
274 if(optimize){
275 noncoplanar_calibration_with_full_optimization ();
276 }else{
277 noncoplanar_calibration ();
278 }
279 //print_cp_cc_data (stderr, &cp, &cc);
280 print_error_stats (stderr);
281 //dump_cp_cc_data (stdout, &cp, &cc);
282 cam_para.Cx = cp.Cx;
283 cam_para.Cy = cp.Cy;
284 cam_para.Ncx = cp.Ncx;
285 cam_para.Nfx = cp.Nfx;
286 cam_para.dpx = cp.dpx;
287 cam_para.dpy = cp.dpy;
288 cam_para.dx = cp.dx;
289 cam_para.dy = cp.dy;
290 cam_para.sx = cp.sx;
291
292 calib_const.Rx = cc.Rx;
293 calib_const.Ry = cc.Ry;
294 calib_const.Rz = cc.Rz;
295 calib_const.Tx = cc.Tx;
296 calib_const.Ty = cc.Ty;
297 calib_const.Tz = cc.Tz;
298 calib_const.f = cc.f;
299 calib_const.kappa1 = cc.kappa1;
300 calib_const.p1 = cc.p1;
301 calib_const.p2 = cc.p2;
302 calib_const.r1 = cc.r1;
303 calib_const.r2 = cc.r2;
304 calib_const.r3 = cc.r3;
305 calib_const.r4 = cc.r4;
306 calib_const.r5 = cc.r5;
307 calib_const.r6 = cc.r6;
308 calib_const.r7 = cc.r7;
309 calib_const.r8 = cc.r8;
310 calib_const.r9 = cc.r9;
311
312
313
314 double dmatrix[16];
315 dmatrix[0]= cc.r1;
316 dmatrix[1]= cc.r2;
317 dmatrix[2]= cc.r3;
318 dmatrix[3]= cc.Tx;
319
320 dmatrix[4]= cc.r4;
321 dmatrix[5]= cc.r5;
322 dmatrix[6]= cc.r6;
323 dmatrix[7]= cc.Ty;
324
325 dmatrix[8]= cc.r7;
326 dmatrix[9]= cc.r8;
327 dmatrix[10]= cc.r9;
328 dmatrix[11]= cc.Tz;
329
330 dmatrix[12]= 0;
331 dmatrix[13]= 0;
332 dmatrix[14]= 0;
333 dmatrix[15]= 1;
334
335 vcg::Matrix44<double> *matrix;
336 matrix = new vcg::Matrix44<double>(dmatrix);
337
338 printf("matrix: %f\n",matrix->ElementAt(2,2));
339
340
341 vcg::Matrix44<double> invmatrix = vcg::Inverse(*matrix);
342 double dcam_cam_pos[] = {0.0,0.0,0.0,1.0};
343 vcg::Point4<double> cam_cam_pos = vcg::Point4<double>(dcam_cam_pos);
344
345 vcg::Point4<double> cam_pos = invmatrix*cam_cam_pos;
346
347 printf("camera position: %f\t%f\t%f\t%f\n",cam_pos[0],cam_pos[1],cam_pos[2],cam_pos[3]);
348
349 double dcam_cam_direction[] = {0.0,0.0,1.0,1.0};
350 vcg::Point4<double> cam_cam_direction = vcg::Point4<double>(dcam_cam_direction);
351
352 vcg::Point4<double> cam_direction = invmatrix*cam_cam_direction;
353 cam_direction -= cam_pos;
354 printf("camera direction: %f\t%f\t%f\t%f\n",cam_direction[0],cam_direction[1],cam_direction[2],cam_direction[3]);
355
356 cameraPosition[0]=cam_pos[0];
357 cameraPosition[1]=cam_pos[1];
358 cameraPosition[2]=cam_pos[2];
359
360 cameraDirection[0]=cam_direction[0];
361 cameraDirection[1]=cam_direction[1];
362 cameraDirection[2]=cam_direction[2];
363 }
364
calibrateToTsai(MeshModel * mm,bool optimize)365 CameraCalibration* TsaiCameraCalibration::calibrateToTsai(MeshModel *mm, bool optimize){
366 QList<CameraCalibrationData*> ccd_list;
367
368 unsigned int i;
369 for (i=0;i<mm->cm.vert.size();i++){
370 CameraCalibrationData* ccd = new CameraCalibrationData();
371
372 double u,v;
373
374 getUVforPoint(mm->cm.vert[i].P()[0],mm->cm.vert[i].P()[1],mm->cm.vert[i].P()[0],&u,&v);
375
376 ccd->wX = mm->cm.vert[i].P()[0];
377 ccd->wY = mm->cm.vert[i].P()[1];
378 ccd->wZ = mm->cm.vert[i].P()[2];
379 ccd->iX = u;
380 ccd->iY = v;
381 if (((int)u >=0 && (int)u<= resolution[0])&& ((int)v >=0 && (int)v<= resolution[1]))
382 ccd_list.push_back(ccd);
383 }
384
385 TsaiCameraCalibration *tsai = new TsaiCameraCalibration();
386 tsai->calibrate(ccd_list,optimize);
387 return tsai;
388 }
389
390
391