Visual Servoing Platform  version 3.4.0
vpXmlParserCamera.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * XML parser to load and save camera intrinsic parameters.
33  *
34  *****************************************************************************/
35 
42 #include <visp3/core/vpXmlParserCamera.h>
43 
44 #include <pugixml.hpp>
45 
46 #include <visp3/core/vpDebug.h>
47 /* --------------------------------------------------------------------------
48  */
49 /* --- LABEL XML ------------------------------------------------------------
50  */
51 /* --------------------------------------------------------------------------
52  */
53 
54 #define LABEL_XML_ROOT "root"
55 #define LABEL_XML_CAMERA "camera"
56 #define LABEL_XML_CAMERA_NAME "name"
57 #define LABEL_XML_WIDTH "image_width"
58 #define LABEL_XML_HEIGHT "image_height"
59 #define LABEL_XML_SUBSAMPLING_WIDTH "subsampling_width"
60 #define LABEL_XML_SUBSAMPLING_HEIGHT "subsampling_height"
61 #define LABEL_XML_FULL_WIDTH "full_width"
62 #define LABEL_XML_FULL_HEIGHT "full_height"
63 #define LABEL_XML_MODEL "model"
64 #define LABEL_XML_MODEL_TYPE "type"
65 #define LABEL_XML_U0 "u0"
66 #define LABEL_XML_V0 "v0"
67 #define LABEL_XML_PX "px"
68 #define LABEL_XML_PY "py"
69 #define LABEL_XML_KUD "kud"
70 #define LABEL_XML_KDU "kdu"
71 #define LABEL_XML_K1 "k1"
72 #define LABEL_XML_K2 "k2"
73 #define LABEL_XML_K3 "k3"
74 #define LABEL_XML_K4 "k4"
75 #define LABEL_XML_K5 "k5"
76 
77 #define LABEL_XML_MODEL_WITHOUT_DISTORTION "perspectiveProjWithoutDistortion"
78 #define LABEL_XML_MODEL_WITH_DISTORTION "perspectiveProjWithDistortion"
79 #define LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION "ProjWithKannalaBrandtDistortion"
80 
81 #define LABEL_XML_ADDITIONAL_INFO "additional_information"
82 
83 #ifndef DOXYGEN_SHOULD_SKIP_THIS
84 class vpXmlParserCamera::Impl
85 {
86 private:
87  /* --- XML Code------------------------------------------------------------
88  */
89  enum vpXmlCodeType {
90  CODE_XML_BAD = -1,
91  CODE_XML_OTHER,
92  CODE_XML_CAMERA,
93  CODE_XML_CAMERA_NAME,
94  CODE_XML_HEIGHT,
95  CODE_XML_WIDTH,
96  CODE_XML_SUBSAMPLING_WIDTH,
97  CODE_XML_SUBSAMPLING_HEIGHT,
98  CODE_XML_FULL_HEIGHT,
99  CODE_XML_FULL_WIDTH,
100  CODE_XML_MODEL,
101  CODE_XML_MODEL_TYPE,
102  CODE_XML_U0,
103  CODE_XML_V0,
104  CODE_XML_PX,
105  CODE_XML_PY,
106  CODE_XML_KUD,
107  CODE_XML_KDU,
108  CODE_XML_K1,
109  CODE_XML_K2,
110  CODE_XML_K3,
111  CODE_XML_K4,
112  CODE_XML_K5,
113  CODE_XML_ADDITIONAL_INFO
114  };
115 
116 public:
117  Impl() : camera(), camera_name(), image_width(0), image_height(0), subsampling_width(0),
118  subsampling_height(0), full_width(0), full_height(0)
119  {
120  }
121 
122  int parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
124  unsigned int im_width, unsigned int im_height)
125  {
126  pugi::xml_document doc;
127  if (!doc.load_file(filename.c_str())) {
128  return SEQUENCE_ERROR;
129  }
130 
131  pugi::xml_node node = doc.document_element();
132  if (!node) {
133  return SEQUENCE_ERROR;
134  }
135 
136  int ret = read(node, cam_name, projModel, im_width, im_height);
137 
138  cam = camera;
139 
140  return ret;
141  }
142 
159  int read(const pugi::xml_node &node_, const std::string &cam_name,
161  unsigned int im_width, unsigned int im_height,
162  unsigned int subsampl_width=0, unsigned int subsampl_height=0)
163  {
164  vpXmlCodeType prop;
165 
167  unsigned int nbCamera = 0;
168 
169  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
170  if (node.type() != pugi::node_element)
171  continue;
172 
173  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
174  prop = CODE_XML_OTHER;
175  back = SEQUENCE_ERROR;
176  }
177  if (prop == CODE_XML_CAMERA) {
178  if (SEQUENCE_OK ==
179  read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height))
180  nbCamera++;
181  } else
182  back = SEQUENCE_ERROR;
183  }
184 
185  if (nbCamera == 0) {
186  back = SEQUENCE_ERROR;
187  vpCERROR << "No camera parameters is available" << std::endl << "with your specifications" << std::endl;
188  } else if (nbCamera > 1) {
189  back = SEQUENCE_ERROR;
190  vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl
191  << "with your specifications : " << std::endl
192  << "precise your choice..." << std::endl;
193  }
194 
195  return back;
196  }
197 
214  int read_camera(const pugi::xml_node &node_, const std::string &cam_name,
216  unsigned int im_width, unsigned int im_height,
217  unsigned int subsampl_width, unsigned int subsampl_height)
218  {
219  vpXmlCodeType prop;
220  /* read value in the XML file. */
221  std::string camera_name_tmp = "";
222  unsigned int image_height_tmp = 0;
223  unsigned int image_width_tmp = 0;
224  unsigned int subsampling_width_tmp = 0;
225  unsigned int subsampling_height_tmp = 0;
226  vpCameraParameters cam_tmp;
227  vpCameraParameters cam_tmp_model;
228  bool projModelFound = false;
230 
231  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
232  if (node.type() != pugi::node_element)
233  continue;
234 
235  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
236  prop = CODE_XML_OTHER;
237  back = SEQUENCE_ERROR;
238  }
239 
240  switch (prop) {
241  case CODE_XML_CAMERA_NAME: {
242  camera_name_tmp = node.text().as_string();
243  std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl;
244  break;
245  }
246  case CODE_XML_WIDTH:
247  image_width_tmp = node.text().as_uint();
248  break;
249 
250  case CODE_XML_HEIGHT:
251  image_height_tmp = node.text().as_uint();
252  break;
253  case CODE_XML_SUBSAMPLING_WIDTH:
254  subsampling_width_tmp = node.text().as_uint();
255  break;
256  case CODE_XML_SUBSAMPLING_HEIGHT:
257  subsampling_height_tmp = node.text().as_uint();
258  break;
259 
260  case CODE_XML_MODEL:
261  back = read_camera_model(node, cam_tmp_model);
262  if (cam_tmp_model.get_projModel() == projModel) {
263  cam_tmp = cam_tmp_model;
264  projModelFound = true;
265  }
266  break;
267 
268  case CODE_XML_ADDITIONAL_INFO:
269  break;
270 
271  case CODE_XML_BAD:
272  case CODE_XML_OTHER:
273  case CODE_XML_CAMERA:
274  case CODE_XML_FULL_HEIGHT:
275  case CODE_XML_FULL_WIDTH:
276  case CODE_XML_MODEL_TYPE:
277  case CODE_XML_U0:
278  case CODE_XML_V0:
279  case CODE_XML_PX:
280  case CODE_XML_PY:
281  case CODE_XML_KUD:
282  case CODE_XML_KDU:
283  case CODE_XML_K1:
284  case CODE_XML_K2:
285  case CODE_XML_K3:
286  case CODE_XML_K4:
287  case CODE_XML_K5:
288  default:
289  back = SEQUENCE_ERROR;
290  break;
291  }
292  }
293  // Create a specific test for subsampling_width and subsampling_height to
294  // ensure that division by zero is not possible in the next test
295  bool test_subsampling_width = true;
296  bool test_subsampling_height = true;
297 
298  if (subsampling_width) {
299  test_subsampling_width = (abs((int)subsampl_width - (int)subsampling_width_tmp) <
300  (allowedPixelDiffOnImageSize * (int)(subsampling_width_tmp / subsampling_width)));
301  }
302  if (subsampling_height) {
303  test_subsampling_height = (abs((int)subsampl_height - (int)subsampling_height_tmp) <
304  (allowedPixelDiffOnImageSize * (int)(subsampling_height_tmp / subsampling_height)));
305  }
306  if (!((projModelFound == true) && (cam_name == camera_name_tmp) &&
307  (abs((int)im_width - (int)image_width_tmp) < allowedPixelDiffOnImageSize || im_width == 0) &&
308  (abs((int)im_height - (int)image_height_tmp) < allowedPixelDiffOnImageSize || im_height == 0) &&
309  (test_subsampling_width) && (test_subsampling_height))) {
310  back = SEQUENCE_ERROR;
311  } else {
312  camera = cam_tmp;
313  camera_name = camera_name_tmp;
314  image_width = image_width_tmp;
315  image_height = image_height_tmp;
316  subsampling_width = subsampling_width_tmp;
317  subsampling_height = subsampling_height_tmp;
318  full_width = subsampling_width_tmp * image_width_tmp;
319  full_height = subsampling_height_tmp * image_height_tmp;
320  }
321  return back;
322  }
323 
330  vpXmlCodeSequenceType read_camera_model(const pugi::xml_node &node_,
331  vpCameraParameters &cam_tmp)
332  {
333  // counter of the number of read parameters
334  int nb = 0;
335  vpXmlCodeType prop;
336  /* read value in the XML file. */
337 
338  std::string model_type = "";
339  double u0 = cam_tmp.get_u0();
340  double v0 = cam_tmp.get_v0();
341  double px = cam_tmp.get_px();
342  double py = cam_tmp.get_py();
343  double kud = cam_tmp.get_kud();
344  double kdu = cam_tmp.get_kdu();
345  std::vector<double> distortion_coeffs;
347  int validation = 0;
348 
349  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
350  // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
351  if (node.type() != pugi::node_element)
352  continue;
353 
354  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
355  prop = CODE_XML_OTHER;
356  back = SEQUENCE_ERROR;
357  }
358 
359  switch (prop) {
360  case CODE_XML_MODEL_TYPE: {
361  model_type = node.text().as_string();
362  nb++;
363  validation = validation | 0x01;
364  } break;
365  case CODE_XML_U0:
366  u0 = node.text().as_double();
367  nb++;
368  validation = validation | 0x02;
369  break;
370  case CODE_XML_V0:
371  v0 = node.text().as_double();
372  nb++;
373  validation = validation | 0x04;
374  break;
375  case CODE_XML_PX:
376  px = node.text().as_double();
377  nb++;
378  validation = validation | 0x08;
379  break;
380  case CODE_XML_PY:
381  py = node.text().as_double();
382  nb++;
383  validation = validation | 0x10;
384  break;
385  case CODE_XML_KUD:
386  kud = node.text().as_double();
387  nb++;
388  validation = validation | 0x20;
389  break;
390  case CODE_XML_KDU:
391  kdu = node.text().as_double();
392  nb++;
393  validation = validation | 0x40;
394  break;
395  case CODE_XML_K1:
396  distortion_coeffs.push_back(node.text().as_double());
397  nb++;
398  validation = validation | 0x20;
399  break;
400  case CODE_XML_K2:
401  distortion_coeffs.push_back(node.text().as_double());
402  nb++;
403  validation = validation | 0x40;
404  break;
405  case CODE_XML_K3:
406  distortion_coeffs.push_back(node.text().as_double());
407  nb++;
408  validation = validation | 0x80;
409  break;
410  case CODE_XML_K4:
411  distortion_coeffs.push_back(node.text().as_double());
412  nb++;
413  validation = validation | 0x100;
414  break;
415  case CODE_XML_K5:
416  distortion_coeffs.push_back(node.text().as_double());
417  nb++;
418  validation = validation | 0x200;
419  break;
420  case CODE_XML_BAD:
421  case CODE_XML_OTHER:
422  case CODE_XML_CAMERA:
423  case CODE_XML_CAMERA_NAME:
424  case CODE_XML_HEIGHT:
425  case CODE_XML_WIDTH:
426  case CODE_XML_SUBSAMPLING_WIDTH:
427  case CODE_XML_SUBSAMPLING_HEIGHT:
428  case CODE_XML_FULL_HEIGHT:
429  case CODE_XML_FULL_WIDTH:
430  case CODE_XML_MODEL:
431  case CODE_XML_ADDITIONAL_INFO:
432  default:
433  back = SEQUENCE_ERROR;
434  break;
435  }
436  }
437 
438  if (model_type.empty()) {
439  vpERROR_TRACE("projection model type doesn't match with any known model !");
440  return SEQUENCE_ERROR;
441  }
442 
443  if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITHOUT_DISTORTION)) {
444  if (nb != 5 || validation != 0x001F) {
445  vpCERROR << "ERROR in 'model' field:\n";
446  vpCERROR << "it must contain 5 parameters\n";
447 
448  return SEQUENCE_ERROR;
449  }
450  cam_tmp.initPersProjWithoutDistortion(px, py, u0, v0);
451  } else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_DISTORTION)) {
452  if (nb != 7 || validation != 0x7F) {
453  vpCERROR << "ERROR in 'model' field:\n";
454  vpCERROR << "it must contain 7 parameters\n";
455 
456  return SEQUENCE_ERROR;
457  }
458  cam_tmp.initPersProjWithDistortion(px, py, u0, v0, kud, kdu);
459  } else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION)) {
460  if (nb != 10 || validation != 0x3FF) { // at least one coefficient is missing. We should know which one
461  vpCERROR << "ERROR in 'model' field:\n";
462  vpCERROR << "it must contain 10 parameters\n";
463 
464  std::vector<double> fixed_distortion_coeffs;
465 
466  // In case disortion coefficients are missing, we should complete them with 0 values
467  // Since 0x3FF is 0011|1111|1111 and we are interrested in the most significant 1s shown below
468  // -- ---
469  // If we divide by 32 (>> 2^5 : 5 remaining least significant bits), we will have to check 5 bits only
470  int check = validation / 32;
471  int j = 0;
472 
473  for(int i = 0; i < 5; i++)
474  {
475  int bit = check % 2; // if bit == 1 => the corresponding distortion coefficient is present.
476  if(!bit)
477  fixed_distortion_coeffs.push_back(0.);
478  else
479  fixed_distortion_coeffs.push_back(distortion_coeffs[j++]);
480  check /= 2;
481  }
482 
483  cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, fixed_distortion_coeffs);
484  return SEQUENCE_ERROR;
485  }
486  cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, distortion_coeffs);
487  } else {
488  vpERROR_TRACE("projection model type doesn't match with any known model !");
489 
490  return SEQUENCE_ERROR;
491  }
492  return back;
493  }
494 
495  int save(const vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
496  unsigned int im_width, unsigned int im_height,
497  const std::string &additionalInfo)
498  {
499  pugi::xml_document doc;
500  pugi::xml_node node;
501 
502  if (!doc.load_file(filename.c_str(), pugi::parse_default | pugi::parse_comments)) {
503  node = doc.append_child(pugi::node_declaration);
504  node.append_attribute("version") = "1.0";
505  node = doc.append_child(LABEL_XML_ROOT);
506  pugi::xml_node nodeComment = node.append_child(pugi::node_comment);
507  nodeComment.set_value("This file stores intrinsic camera parameters used\n"
508  " in the vpCameraParameters Class of ViSP available\n"
509  " at https://visp.inria.fr/download/ .\n"
510  " It can be read with the parse method of\n"
511  " the vpXmlParserCamera class.");
512  }
513 
514  node = doc.document_element();
515  if (!node) {
516  return SEQUENCE_ERROR;
517  }
518 
519  camera = cam;
520 
521  int nbCamera = count(node, cam_name, cam.get_projModel(), im_width, im_height);
522  if (nbCamera) {
523  return SEQUENCE_ERROR;
524  }
525 
526  pugi::xml_node nodeCamera = find_camera(node, cam_name, im_width, im_height);
527  if (!nodeCamera) {
528  write(node, cam_name, im_width, im_height);
529  } else {
530  write_camera(nodeCamera);
531  }
532 
533  if (!additionalInfo.empty()) {
534  // Get camera node pointer
535  nodeCamera = find_camera(node, cam_name, im_width, im_height);
536 
537  // Additional information provided by the user
538  pugi::xml_node nodeAdditionalInfo = find_additional_info(nodeCamera);
539 
540  if (!nodeAdditionalInfo) {
541  // Create the additional information node
542  pugi::xml_node node_comment = nodeCamera.append_child(pugi::node_comment);
543  node_comment.set_value("Additional information");
544 
545  nodeAdditionalInfo = nodeCamera.append_child(LABEL_XML_ADDITIONAL_INFO);
546  }
547 
548  if (nodeAdditionalInfo) {
549  // Add the information in this specific node
550  pugi::xml_document tmpDoc;
551  if (tmpDoc.load_string(additionalInfo.c_str())) {
552  for (node = tmpDoc.first_child(); node; node = node.next_sibling()) {
553  nodeAdditionalInfo.append_copy(node);
554  }
555  }
556  }
557  }
558 
559  doc.save_file(filename.c_str(), PUGIXML_TEXT(" "));
560 
561  return SEQUENCE_OK;
562  }
563 
581  int count(const pugi::xml_node &node_, const std::string &cam_name,
583  unsigned int im_width, unsigned int im_height,
584  unsigned int subsampl_width=0, unsigned int subsampl_height=0)
585  {
586  vpXmlCodeType prop;
587  int nbCamera = 0;
588 
589  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
590  if (node.type() != pugi::node_element)
591  continue;
592 
593  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
594  prop = CODE_XML_OTHER;
595  }
596 
597  if (prop == CODE_XML_CAMERA) {
598  if (SEQUENCE_OK ==
599  read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height))
600  nbCamera++;
601  }
602  }
603 
604  return nbCamera;
605  }
606 
623  pugi::xml_node find_camera(const pugi::xml_node &node_, const std::string &cam_name,
624  unsigned int im_width, unsigned int im_height,
625  unsigned int subsampl_width=0, unsigned int subsampl_height=0)
626  {
627  vpXmlCodeType prop;
628 
629  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
630  if (node.type() != pugi::node_element)
631  continue;
632 
633  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
634  prop = CODE_XML_OTHER;
635  }
636  if (prop == CODE_XML_CAMERA) {
637  if (SEQUENCE_OK == read_camera_header(node, cam_name, im_width, im_height, subsampl_width, subsampl_height)) {
638  return node;
639  }
640  }
641  }
642  return pugi::xml_node();
643  }
644 
660  int read_camera_header(const pugi::xml_node &node_, const std::string &cam_name,
661  unsigned int im_width, unsigned int im_height,
662  unsigned int subsampl_width=0, unsigned int subsampl_height=0)
663  {
664  vpXmlCodeType prop;
665  /* read value in the XML file. */
666  std::string camera_name_tmp = "";
667  unsigned int image_height_tmp = 0;
668  unsigned int image_width_tmp = 0;
669  unsigned int subsampling_width_tmp = 0;
670  unsigned int subsampling_height_tmp = 0;
672 
673  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
674  if (node.type() != pugi::node_element)
675  continue;
676  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
677  prop = CODE_XML_OTHER;
678  back = SEQUENCE_ERROR;
679  }
680 
681  switch (prop) {
682  case CODE_XML_CAMERA_NAME:
683  camera_name_tmp = node.text().as_string();
684  break;
685 
686  case CODE_XML_WIDTH:
687  image_width_tmp = node.text().as_uint();
688  break;
689 
690  case CODE_XML_HEIGHT:
691  image_height_tmp = node.text().as_uint();
692  break;
693 
694  case CODE_XML_SUBSAMPLING_WIDTH:
695  subsampling_width_tmp = node.text().as_uint();
696  break;
697 
698  case CODE_XML_SUBSAMPLING_HEIGHT:
699  subsampling_height_tmp = node.text().as_uint();
700  break;
701 
702  case CODE_XML_MODEL:
703  break;
704 
705  case CODE_XML_ADDITIONAL_INFO:
706  break;
707 
708  case CODE_XML_BAD:
709  case CODE_XML_OTHER:
710  case CODE_XML_CAMERA:
711  case CODE_XML_FULL_HEIGHT:
712  case CODE_XML_FULL_WIDTH:
713  case CODE_XML_MODEL_TYPE:
714  case CODE_XML_U0:
715  case CODE_XML_V0:
716  case CODE_XML_PX:
717  case CODE_XML_PY:
718  case CODE_XML_KUD:
719  case CODE_XML_KDU:
720  default:
721  back = SEQUENCE_ERROR;
722  break;
723  }
724  }
725  if (!((cam_name == camera_name_tmp) && (im_width == image_width_tmp || im_width == 0) &&
726  (im_height == image_height_tmp || im_height == 0) &&
727  (subsampl_width == subsampling_width_tmp || subsampl_width == 0) &&
728  (subsampl_height == subsampling_height_tmp || subsampl_height == 0))) {
729  back = SEQUENCE_ERROR;
730  }
731  return back;
732  }
733 
749  int write(pugi::xml_node &node, const std::string &cam_name, unsigned int im_width,
750  unsigned int im_height, unsigned int subsampl_width=0,
751  unsigned int subsampl_height=0)
752  {
753  int back = SEQUENCE_OK;
754 
755  // <camera>
756  pugi::xml_node node_camera = node.append_child(LABEL_XML_CAMERA);
757 
758  pugi::xml_node node_tmp;
759  {
760  //<name>
761  if (!cam_name.empty()) {
762  node_tmp = node_camera.append_child(pugi::node_comment);
763  node_tmp.set_value("Name of the camera");
764  node_tmp = node_camera.append_child(LABEL_XML_CAMERA_NAME);
765  node_tmp.append_child(pugi::node_pcdata).set_value(cam_name.c_str());
766  }
767 
768  if (im_width != 0 || im_height != 0) {
769  node_tmp = node_camera.append_child(pugi::node_comment);
770  node_tmp.set_value("Size of the image on which camera "
771  "calibration was performed");
772 
773  //<image_width>
774  node_tmp = node_camera.append_child(LABEL_XML_WIDTH);
775  node_tmp.append_child(pugi::node_pcdata).text() = im_width;
776 
777  //<image_height>
778  node_tmp = node_camera.append_child(LABEL_XML_HEIGHT);
779  node_tmp.append_child(pugi::node_pcdata).text() = im_height;
780  if (subsampling_width != 0 || subsampling_height != 0) {
781  node_tmp = node_camera.append_child(pugi::node_comment);
782  node_tmp.set_value("Subsampling used to obtain the "
783  "current size of the image.");
784 
785  //<subsampling_width>
786  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_WIDTH);
787  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_width;
788  //<subsampling_height>
789  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_HEIGHT);
790  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_height;
791  node_tmp = node_camera.append_child(pugi::node_comment);
792  node_tmp.set_value("The full size is the sensor size actually used to "
793  "grab the image. full_width = subsampling_width * "
794  "image_width");
795 
796  //<full_width>
797  node_tmp = node_camera.append_child(LABEL_XML_FULL_WIDTH);
798  node_tmp.append_child(pugi::node_pcdata).text() = im_width * subsampl_width;
799  //<full_height>
800  node_tmp = node_camera.append_child(LABEL_XML_FULL_HEIGHT);
801  node_tmp.append_child(pugi::node_pcdata).text() = im_height * subsampl_height;
802  }
803  }
804 
805  node_tmp = node_camera.append_child(pugi::node_comment);
806  node_tmp.set_value("Intrinsic camera parameters "
807  "computed for each projection model");
808 
809  back = write_camera(node_camera);
810  }
811  return back;
812  }
813 
819  int write_camera(pugi::xml_node &node_camera)
820  {
821  pugi::xml_node node_model;
822  pugi::xml_node node_tmp;
823 
824  int back = SEQUENCE_OK;
825  switch (camera.get_projModel()) {
827  //<model>
828  node_model = node_camera.append_child(LABEL_XML_MODEL);
829  {
830  node_tmp = node_model.append_child(pugi::node_comment);
831  node_tmp.set_value("Projection model type");
832 
833  //<type>without_distortion</type>
834  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
835  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITHOUT_DISTORTION);
836 
837  node_tmp = node_model.append_child(pugi::node_comment);
838  node_tmp.set_value("Pixel ratio");
839  //<px>
840  node_tmp = node_model.append_child(LABEL_XML_PX);
841  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
842  //<py>
843  node_tmp = node_model.append_child(LABEL_XML_PY);
844  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
845 
846  node_tmp = node_model.append_child(pugi::node_comment);
847  node_tmp.set_value("Principal point");
848 
849  //<u0>
850  node_tmp = node_model.append_child(LABEL_XML_U0);
851  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
852  //<v0>
853  node_tmp = node_model.append_child(LABEL_XML_V0);
854  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
855  }
856  break;
857 
859  //<model>
860  node_model = node_camera.append_child(LABEL_XML_MODEL);
861  {
862  node_tmp = node_model.append_child(pugi::node_comment);
863  node_tmp.set_value("Projection model type");
864  //<type>with_distortion</type>
865  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
866  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_DISTORTION);
867 
868  node_tmp = node_model.append_child(pugi::node_comment);
869  node_tmp.set_value("Pixel ratio");
870  //<px>
871  node_tmp = node_model.append_child(LABEL_XML_PX);
872  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
873  //<py>
874  node_tmp = node_model.append_child(LABEL_XML_PY);
875  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
876 
877  node_tmp = node_model.append_child(pugi::node_comment);
878  node_tmp.set_value("Principal point");
879  //<u0>
880  node_tmp = node_model.append_child(LABEL_XML_U0);
881  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
882  //<v0>
883  node_tmp = node_model.append_child(LABEL_XML_V0);
884  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
885 
886  //<kud>
887  node_tmp = node_model.append_child(pugi::node_comment);
888  node_tmp.set_value("Undistorted to distorted distortion parameter");
889  node_tmp = node_model.append_child(LABEL_XML_KUD);
890  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kud();
891 
892  //<kud>
893  node_tmp = node_model.append_child(pugi::node_comment);
894  node_tmp.set_value("Distorted to undistorted distortion parameter");
895  node_tmp = node_model.append_child(LABEL_XML_KDU);
896  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kdu();
897  }
898  break;
899 
901  //<model>
902  node_model = node_camera.append_child(LABEL_XML_MODEL);
903  {
904  node_tmp = node_model.append_child(pugi::node_comment);
905  node_tmp.set_value("Projection model type");
906  //<type>with_KannalaBrandt_distortion</type>
907  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
908  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION);
909 
910  node_tmp = node_model.append_child(pugi::node_comment);
911  node_tmp.set_value("Pixel ratio");
912  //<px>
913  node_tmp = node_model.append_child(LABEL_XML_PX);
914  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
915  //<py>
916  node_tmp = node_model.append_child(LABEL_XML_PY);
917  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
918 
919  node_tmp = node_model.append_child(pugi::node_comment);
920  node_tmp.set_value("Principal point");
921  //<u0>
922  node_tmp = node_model.append_child(LABEL_XML_U0);
923  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
924  //<v0>
925  node_tmp = node_model.append_child(LABEL_XML_V0);
926  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
927 
928  //<k1>, <k2>, <k3>, <k4>, <k5>
929  std::vector<double> distortion_coefs = camera.getKannalaBrandtDistortionCoefficients();
930 
931  if(distortion_coefs.size() != 5)
932  std::cout << "Make sure to have 5 distortion coefficients for Kannala-Brandt distortions." << std::endl;
933 
934  node_tmp = node_model.append_child(pugi::node_comment);
935  node_tmp.set_value("Distortion coefficients");
936  node_tmp = node_model.append_child(LABEL_XML_K1);
937  distortion_coefs.size() == 0 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[0];
938  node_tmp = node_model.append_child(LABEL_XML_K2);
939  distortion_coefs.size() <= 1 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[1];
940  node_tmp = node_model.append_child(LABEL_XML_K3);
941  distortion_coefs.size() <= 2 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[2];
942  node_tmp = node_model.append_child(LABEL_XML_K4);
943  distortion_coefs.size() <= 3 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[3];
944  node_tmp = node_model.append_child(LABEL_XML_K5);
945  distortion_coefs.size() <= 4 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[4];
946  }
947  break;
948  }
949  return back;
950  }
951 
958  pugi::xml_node find_additional_info(const pugi::xml_node &node_)
959  {
960  vpXmlCodeType prop;
961 
962  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
963  if (node.type() != pugi::node_element) {
964  continue;
965  }
966 
967  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
968  prop = CODE_XML_OTHER;
969  }
970 
971  if (prop == CODE_XML_ADDITIONAL_INFO) {
972  // We found the node
973  return node;
974  }
975  }
976 
977  return pugi::xml_node();
978  }
979 
980 
987  vpXmlCodeSequenceType str2xmlcode(const char *str, vpXmlCodeType &res)
988  {
989  vpXmlCodeType val_int = CODE_XML_BAD;
991 
992  if (!strcmp(str, LABEL_XML_CAMERA)) {
993  val_int = CODE_XML_CAMERA;
994  } else if (!strcmp(str, LABEL_XML_CAMERA_NAME)) {
995  val_int = CODE_XML_CAMERA_NAME;
996  } else if (!strcmp(str, LABEL_XML_MODEL)) {
997  val_int = CODE_XML_MODEL;
998  } else if (!strcmp(str, LABEL_XML_MODEL_TYPE)) {
999  val_int = CODE_XML_MODEL_TYPE;
1000  } else if (!strcmp(str, LABEL_XML_WIDTH)) {
1001  val_int = CODE_XML_WIDTH;
1002  } else if (!strcmp(str, LABEL_XML_HEIGHT)) {
1003  val_int = CODE_XML_HEIGHT;
1004  } else if (!strcmp(str, LABEL_XML_SUBSAMPLING_WIDTH)) {
1005  val_int = CODE_XML_SUBSAMPLING_WIDTH;
1006  } else if (!strcmp(str, LABEL_XML_SUBSAMPLING_HEIGHT)) {
1007  val_int = CODE_XML_SUBSAMPLING_HEIGHT;
1008  } else if (!strcmp(str, LABEL_XML_FULL_WIDTH)) {
1009  val_int = CODE_XML_FULL_WIDTH;
1010  } else if (!strcmp(str, LABEL_XML_FULL_HEIGHT)) {
1011  val_int = CODE_XML_FULL_HEIGHT;
1012  } else if (!strcmp(str, LABEL_XML_U0)) {
1013  val_int = CODE_XML_U0;
1014  } else if (!strcmp(str, LABEL_XML_V0)) {
1015  val_int = CODE_XML_V0;
1016  } else if (!strcmp(str, LABEL_XML_PX)) {
1017  val_int = CODE_XML_PX;
1018  } else if (!strcmp(str, LABEL_XML_PY)) {
1019  val_int = CODE_XML_PY;
1020  } else if (!strcmp(str, LABEL_XML_KUD)) {
1021  val_int = CODE_XML_KUD;
1022  } else if (!strcmp(str, LABEL_XML_KDU)) {
1023  val_int = CODE_XML_KDU;
1024  } else if (!strcmp(str, LABEL_XML_K1)) {
1025  val_int = CODE_XML_K1;
1026  } else if (!strcmp(str, LABEL_XML_K2)) {
1027  val_int = CODE_XML_K2;
1028  } else if (!strcmp(str, LABEL_XML_K3)) {
1029  val_int = CODE_XML_K3;
1030  } else if (!strcmp(str, LABEL_XML_K4)) {
1031  val_int = CODE_XML_K4;
1032  } else if (!strcmp(str, LABEL_XML_K5)) {
1033  val_int = CODE_XML_K5;
1034  } else if (!strcmp(str, LABEL_XML_ADDITIONAL_INFO)) {
1035  val_int = CODE_XML_ADDITIONAL_INFO;
1036  } else {
1037  val_int = CODE_XML_OTHER;
1038  }
1039  res = val_int;
1040 
1041  return back;
1042  }
1043 
1044  std::string getCameraName() const { return camera_name; }
1045  vpCameraParameters getCameraParameters() const { return camera; }
1046  unsigned int getHeight() const { return image_height; }
1047  unsigned int getSubsampling_width() const { return subsampling_width; }
1048  unsigned int getSubsampling_height() const { return subsampling_height; }
1049  unsigned int getWidth() const { return image_width; }
1050 
1051  void setCameraName(const std::string &name) { camera_name = name; }
1052  void setHeight(unsigned int height) { image_height = height; }
1053  void setSubsampling_width(unsigned int subsampling) { subsampling_width = subsampling; }
1054  void setSubsampling_height(unsigned int subsampling) { subsampling_height = subsampling; }
1055  void setWidth(unsigned int width) { image_width = width; }
1056 
1057 private:
1058  vpCameraParameters camera;
1059  std::string camera_name;
1060  unsigned int image_width;
1061  unsigned int image_height;
1062  unsigned int subsampling_width;
1063  unsigned int subsampling_height;
1064  unsigned int full_width;
1065  unsigned int full_height;
1066 
1069  static const int allowedPixelDiffOnImageSize = 15;
1070 };
1071 #endif // DOXYGEN_SHOULD_SKIP_THIS
1072 
1074 {
1075 }
1076 
1078 {
1079  delete m_impl;
1080 }
1081 
1096 int vpXmlParserCamera::parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1098  unsigned int im_width, unsigned int im_height)
1099 {
1100  return m_impl->parse(cam, filename, cam_name, projModel, im_width, im_height);
1101 }
1102 
1146 int vpXmlParserCamera::save(const vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1147  unsigned int im_width, unsigned int im_height,
1148  const std::string &additionalInfo)
1149 {
1150  return m_impl->save(cam, filename, cam_name, im_width, im_height, additionalInfo);
1151 }
1152 
1154 {
1155  return m_impl->getCameraName();
1156 }
1157 
1159 {
1160  return m_impl->getCameraParameters();
1161 }
1162 
1163 unsigned int vpXmlParserCamera::getHeight() const
1164 {
1165  return m_impl->getHeight();
1166 }
1167 
1169 {
1170  return m_impl->getSubsampling_width();
1171 }
1172 
1174 {
1175  return m_impl->getSubsampling_height();
1176 }
1177 
1178 unsigned int vpXmlParserCamera::getWidth() const
1179 {
1180  return m_impl->getWidth();
1181 }
1182 
1183 void vpXmlParserCamera::setCameraName(const std::string &name)
1184 {
1185  m_impl->setCameraName(name);
1186 }
1187 
1188 void vpXmlParserCamera::setHeight(unsigned int height)
1189 {
1190  m_impl->setHeight(height);
1191 }
1192 
1193 void vpXmlParserCamera::setSubsampling_width(unsigned int subsampling)
1194 {
1195  m_impl->setSubsampling_width(subsampling);
1196 }
1197 
1198 void vpXmlParserCamera::setSubsampling_height(unsigned int subsampling)
1199 {
1200  m_impl->setSubsampling_height(subsampling);
1201 }
1202 
1203 void vpXmlParserCamera::setWidth(unsigned int width)
1204 {
1205  m_impl->setWidth(width);
1206 }
double get_u0() const
unsigned int getHeight() const
#define vpCERROR
Definition: vpDebug.h:365
vpCameraParameters getCameraParameters() const
#define vpERROR_TRACE
Definition: vpDebug.h:393
void setHeight(unsigned int height)
void setWidth(unsigned int width)
void setCameraName(const std::string &name)
double get_py() const
void initProjWithKannalaBrandtDistortion(double px, double py, double u0, double v0, const std::vector< double > &distortion_coefficients)
void setSubsampling_height(unsigned int subsampling)
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
double get_v0() const
unsigned int getSubsampling_height() const
Generic class defining intrinsic camera parameters.
unsigned int getSubsampling_width() const
double get_px() const
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0)
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
double get_kud() const
vpCameraParametersProjType get_projModel() const
double get_kdu() const
unsigned int getWidth() const
int save(const vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, unsigned int image_width=0, unsigned int image_height=0, const std::string &additionalInfo="")
std::string getCameraName() const
void setSubsampling_width(unsigned int subsampling)