Visual Servoing Platform  version 3.3.1 under development (2020-09-18)
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 #ifdef VISP_HAVE_PUGIXML
45 
46 #include <pugixml.hpp>
47 
48 #include <visp3/core/vpDebug.h>
49 /* --------------------------------------------------------------------------
50  */
51 /* --- LABEL XML ------------------------------------------------------------
52  */
53 /* --------------------------------------------------------------------------
54  */
55 
56 #define LABEL_XML_ROOT "root"
57 #define LABEL_XML_CAMERA "camera"
58 #define LABEL_XML_CAMERA_NAME "name"
59 #define LABEL_XML_WIDTH "image_width"
60 #define LABEL_XML_HEIGHT "image_height"
61 #define LABEL_XML_SUBSAMPLING_WIDTH "subsampling_width"
62 #define LABEL_XML_SUBSAMPLING_HEIGHT "subsampling_height"
63 #define LABEL_XML_FULL_WIDTH "full_width"
64 #define LABEL_XML_FULL_HEIGHT "full_height"
65 #define LABEL_XML_MODEL "model"
66 #define LABEL_XML_MODEL_TYPE "type"
67 #define LABEL_XML_U0 "u0"
68 #define LABEL_XML_V0 "v0"
69 #define LABEL_XML_PX "px"
70 #define LABEL_XML_PY "py"
71 #define LABEL_XML_KUD "kud"
72 #define LABEL_XML_KDU "kdu"
73 #define LABEL_XML_K1 "k1"
74 #define LABEL_XML_K2 "k2"
75 #define LABEL_XML_K3 "k3"
76 #define LABEL_XML_K4 "k4"
77 #define LABEL_XML_K5 "k5"
78 
79 #define LABEL_XML_MODEL_WITHOUT_DISTORTION "perspectiveProjWithoutDistortion"
80 #define LABEL_XML_MODEL_WITH_DISTORTION "perspectiveProjWithDistortion"
81 #define LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION "ProjWithKannalaBrandtDistortion"
82 
83 #define LABEL_XML_ADDITIONAL_INFO "additional_information"
84 
85 #ifndef DOXYGEN_SHOULD_SKIP_THIS
86 class vpXmlParserCamera::Impl
87 {
88 private:
89  /* --- XML Code------------------------------------------------------------
90  */
91  enum vpXmlCodeType {
92  CODE_XML_BAD = -1,
93  CODE_XML_OTHER,
94  CODE_XML_CAMERA,
95  CODE_XML_CAMERA_NAME,
96  CODE_XML_HEIGHT,
97  CODE_XML_WIDTH,
98  CODE_XML_SUBSAMPLING_WIDTH,
99  CODE_XML_SUBSAMPLING_HEIGHT,
100  CODE_XML_FULL_HEIGHT,
101  CODE_XML_FULL_WIDTH,
102  CODE_XML_MODEL,
103  CODE_XML_MODEL_TYPE,
104  CODE_XML_U0,
105  CODE_XML_V0,
106  CODE_XML_PX,
107  CODE_XML_PY,
108  CODE_XML_KUD,
109  CODE_XML_KDU,
110  CODE_XML_K1,
111  CODE_XML_K2,
112  CODE_XML_K3,
113  CODE_XML_K4,
114  CODE_XML_K5,
115  CODE_XML_ADDITIONAL_INFO
116  };
117 
118 public:
119  Impl() : camera(), camera_name(), image_width(0), image_height(0), subsampling_width(0),
120  subsampling_height(0), full_width(0), full_height(0)
121  {
122  }
123 
124  int parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
126  unsigned int im_width, unsigned int im_height)
127  {
128  pugi::xml_document doc;
129  if (!doc.load_file(filename.c_str())) {
130  return SEQUENCE_ERROR;
131  }
132 
133  pugi::xml_node node = doc.document_element();
134  if (!node) {
135  return SEQUENCE_ERROR;
136  }
137 
138  int ret = read(node, cam_name, projModel, im_width, im_height);
139 
140  cam = camera;
141 
142  return ret;
143  }
144 
160  int read(const pugi::xml_node &node_, const std::string &cam_name,
162  unsigned int im_width, unsigned int im_height,
163  unsigned int subsampl_width=0, unsigned int subsampl_height=0)
164  {
165  vpXmlCodeType prop;
166 
168  unsigned int nbCamera = 0;
169 
170  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
171  if (node.type() != pugi::node_element)
172  continue;
173 
174  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
175  prop = CODE_XML_OTHER;
176  back = SEQUENCE_ERROR;
177  }
178  if (prop == CODE_XML_CAMERA) {
179  if (SEQUENCE_OK ==
180  read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height))
181  nbCamera++;
182  } else
183  back = SEQUENCE_ERROR;
184  }
185 
186  if (nbCamera == 0) {
187  back = SEQUENCE_ERROR;
188  vpCERROR << "No camera parameters is available" << std::endl << "with your specifications" << std::endl;
189  } else if (nbCamera > 1) {
190  back = SEQUENCE_ERROR;
191  vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl
192  << "with your specifications : " << std::endl
193  << "precise your choice..." << std::endl;
194  }
195 
196  return back;
197  }
198 
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 
580  int count(const pugi::xml_node &node_, const std::string &cam_name,
582  unsigned int im_width, unsigned int im_height,
583  unsigned int subsampl_width=0, unsigned int subsampl_height=0)
584  {
585  vpXmlCodeType prop;
586  int nbCamera = 0;
587 
588  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
589  if (node.type() != pugi::node_element)
590  continue;
591 
592  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
593  prop = CODE_XML_OTHER;
594  }
595 
596  if (prop == CODE_XML_CAMERA) {
597  if (SEQUENCE_OK ==
598  read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height))
599  nbCamera++;
600  }
601  }
602 
603  return nbCamera;
604  }
605 
622  pugi::xml_node find_camera(const pugi::xml_node &node_, const std::string &cam_name,
623  unsigned int im_width, unsigned int im_height,
624  unsigned int subsampl_width=0, unsigned int subsampl_height=0)
625  {
626  vpXmlCodeType prop;
627 
628  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
629  if (node.type() != pugi::node_element)
630  continue;
631 
632  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
633  prop = CODE_XML_OTHER;
634  }
635  if (prop == CODE_XML_CAMERA) {
636  if (SEQUENCE_OK == read_camera_header(node, cam_name, im_width, im_height, subsampl_width, subsampl_height)) {
637  return node;
638  }
639  }
640  }
641  return pugi::xml_node();
642  }
643 
659  int read_camera_header(const pugi::xml_node &node_, const std::string &cam_name,
660  unsigned int im_width, unsigned int im_height,
661  unsigned int subsampl_width=0, unsigned int subsampl_height=0)
662  {
663  vpXmlCodeType prop;
664  /* read value in the XML file. */
665  std::string camera_name_tmp = "";
666  unsigned int image_height_tmp = 0;
667  unsigned int image_width_tmp = 0;
668  unsigned int subsampling_width_tmp = 0;
669  unsigned int subsampling_height_tmp = 0;
671 
672  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
673  if (node.type() != pugi::node_element)
674  continue;
675  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
676  prop = CODE_XML_OTHER;
677  back = SEQUENCE_ERROR;
678  }
679 
680  switch (prop) {
681  case CODE_XML_CAMERA_NAME:
682  camera_name_tmp = node.text().as_string();
683  break;
684 
685  case CODE_XML_WIDTH:
686  image_width_tmp = node.text().as_uint();
687  break;
688 
689  case CODE_XML_HEIGHT:
690  image_height_tmp = node.text().as_uint();
691  break;
692 
693  case CODE_XML_SUBSAMPLING_WIDTH:
694  subsampling_width_tmp = node.text().as_uint();
695  break;
696 
697  case CODE_XML_SUBSAMPLING_HEIGHT:
698  subsampling_height_tmp = node.text().as_uint();
699  break;
700 
701  case CODE_XML_MODEL:
702  break;
703 
704  case CODE_XML_ADDITIONAL_INFO:
705  break;
706 
707  case CODE_XML_BAD:
708  case CODE_XML_OTHER:
709  case CODE_XML_CAMERA:
710  case CODE_XML_FULL_HEIGHT:
711  case CODE_XML_FULL_WIDTH:
712  case CODE_XML_MODEL_TYPE:
713  case CODE_XML_U0:
714  case CODE_XML_V0:
715  case CODE_XML_PX:
716  case CODE_XML_PY:
717  case CODE_XML_KUD:
718  case CODE_XML_KDU:
719  default:
720  back = SEQUENCE_ERROR;
721  break;
722  }
723  }
724  if (!((cam_name == camera_name_tmp) && (im_width == image_width_tmp || im_width == 0) &&
725  (im_height == image_height_tmp || im_height == 0) &&
726  (subsampl_width == subsampling_width_tmp || subsampl_width == 0) &&
727  (subsampl_height == subsampling_height_tmp || subsampl_height == 0))) {
728  back = SEQUENCE_ERROR;
729  }
730  return back;
731  }
732 
748  int write(pugi::xml_node &node, const std::string &cam_name, unsigned int im_width,
749  unsigned int im_height, unsigned int subsampl_width=0,
750  unsigned int subsampl_height=0)
751  {
752  int back = SEQUENCE_OK;
753 
754  // <camera>
755  pugi::xml_node node_camera = node.append_child(LABEL_XML_CAMERA);
756 
757  pugi::xml_node node_tmp;
758  {
759  //<name>
760  if (!cam_name.empty()) {
761  node_tmp = node_camera.append_child(pugi::node_comment);
762  node_tmp.set_value("Name of the camera");
763  node_tmp = node_camera.append_child(LABEL_XML_CAMERA_NAME);
764  node_tmp.append_child(pugi::node_pcdata).set_value(cam_name.c_str());
765  }
766 
767  if (im_width != 0 || im_height != 0) {
768  node_tmp = node_camera.append_child(pugi::node_comment);
769  node_tmp.set_value("Size of the image on which camera "
770  "calibration was performed");
771 
772  //<image_width>
773  node_tmp = node_camera.append_child(LABEL_XML_WIDTH);
774  node_tmp.append_child(pugi::node_pcdata).text() = im_width;
775 
776  //<image_height>
777  node_tmp = node_camera.append_child(LABEL_XML_HEIGHT);
778  node_tmp.append_child(pugi::node_pcdata).text() = im_height;
779  if (subsampling_width != 0 || subsampling_height != 0) {
780  node_tmp = node_camera.append_child(pugi::node_comment);
781  node_tmp.set_value("Subsampling used to obtain the "
782  "current size of the image.");
783 
784  //<subsampling_width>
785  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_WIDTH);
786  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_width;
787  //<subsampling_height>
788  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_HEIGHT);
789  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_height;
790  node_tmp = node_camera.append_child(pugi::node_comment);
791  node_tmp.set_value("The full size is the sensor size actually used to "
792  "grab the image. full_width = subsampling_width * "
793  "image_width");
794 
795  //<full_width>
796  node_tmp = node_camera.append_child(LABEL_XML_FULL_WIDTH);
797  node_tmp.append_child(pugi::node_pcdata).text() = im_width * subsampl_width;
798  //<full_height>
799  node_tmp = node_camera.append_child(LABEL_XML_FULL_HEIGHT);
800  node_tmp.append_child(pugi::node_pcdata).text() = im_height * subsampl_height;
801  }
802  }
803 
804  node_tmp = node_camera.append_child(pugi::node_comment);
805  node_tmp.set_value("Intrinsic camera parameters "
806  "computed for each projection model");
807 
808  back = write_camera(node_camera);
809  }
810  return back;
811  }
812 
818  int write_camera(pugi::xml_node &node_camera)
819  {
820  pugi::xml_node node_model;
821  pugi::xml_node node_tmp;
822 
823  int back = SEQUENCE_OK;
824  switch (camera.get_projModel()) {
826  //<model>
827  node_model = node_camera.append_child(LABEL_XML_MODEL);
828  {
829  node_tmp = node_model.append_child(pugi::node_comment);
830  node_tmp.set_value("Projection model type");
831 
832  //<type>without_distortion</type>
833  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
834  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITHOUT_DISTORTION);
835 
836  node_tmp = node_model.append_child(pugi::node_comment);
837  node_tmp.set_value("Pixel ratio");
838  //<px>
839  node_tmp = node_model.append_child(LABEL_XML_PX);
840  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
841  //<py>
842  node_tmp = node_model.append_child(LABEL_XML_PY);
843  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
844 
845  node_tmp = node_model.append_child(pugi::node_comment);
846  node_tmp.set_value("Principal point");
847 
848  //<u0>
849  node_tmp = node_model.append_child(LABEL_XML_U0);
850  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
851  //<v0>
852  node_tmp = node_model.append_child(LABEL_XML_V0);
853  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
854  }
855  break;
856 
858  //<model>
859  node_model = node_camera.append_child(LABEL_XML_MODEL);
860  {
861  node_tmp = node_model.append_child(pugi::node_comment);
862  node_tmp.set_value("Projection model type");
863  //<type>with_distortion</type>
864  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
865  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_DISTORTION);
866 
867  node_tmp = node_model.append_child(pugi::node_comment);
868  node_tmp.set_value("Pixel ratio");
869  //<px>
870  node_tmp = node_model.append_child(LABEL_XML_PX);
871  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
872  //<py>
873  node_tmp = node_model.append_child(LABEL_XML_PY);
874  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
875 
876  node_tmp = node_model.append_child(pugi::node_comment);
877  node_tmp.set_value("Principal point");
878  //<u0>
879  node_tmp = node_model.append_child(LABEL_XML_U0);
880  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
881  //<v0>
882  node_tmp = node_model.append_child(LABEL_XML_V0);
883  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
884 
885  //<kud>
886  node_tmp = node_model.append_child(pugi::node_comment);
887  node_tmp.set_value("Undistorted to distorted distortion parameter");
888  node_tmp = node_model.append_child(LABEL_XML_KUD);
889  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kud();
890 
891  //<kud>
892  node_tmp = node_model.append_child(pugi::node_comment);
893  node_tmp.set_value("Distorted to undistorted distortion parameter");
894  node_tmp = node_model.append_child(LABEL_XML_KDU);
895  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kdu();
896  }
897  break;
898 
900  //<model>
901  node_model = node_camera.append_child(LABEL_XML_MODEL);
902  {
903  node_tmp = node_model.append_child(pugi::node_comment);
904  node_tmp.set_value("Projection model type");
905  //<type>with_KannalaBrandt_distortion</type>
906  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
907  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION);
908 
909  node_tmp = node_model.append_child(pugi::node_comment);
910  node_tmp.set_value("Pixel ratio");
911  //<px>
912  node_tmp = node_model.append_child(LABEL_XML_PX);
913  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
914  //<py>
915  node_tmp = node_model.append_child(LABEL_XML_PY);
916  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
917 
918  node_tmp = node_model.append_child(pugi::node_comment);
919  node_tmp.set_value("Principal point");
920  //<u0>
921  node_tmp = node_model.append_child(LABEL_XML_U0);
922  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
923  //<v0>
924  node_tmp = node_model.append_child(LABEL_XML_V0);
925  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
926 
927  //<k1>, <k2>, <k3>, <k4>, <k5>
928  std::vector<double> distortion_coefs = camera.getKannalaBrandtDistortionCoefficients();
929 
930  if(distortion_coefs.size() != 5)
931  std::cout << "Make sure to have 5 distortion coefficients for Kannala-Brandt distortions." << std::endl;
932 
933  node_tmp = node_model.append_child(pugi::node_comment);
934  node_tmp.set_value("Distortion coefficients");
935  node_tmp = node_model.append_child(LABEL_XML_K1);
936  distortion_coefs.size() == 0 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[0];
937  node_tmp = node_model.append_child(LABEL_XML_K2);
938  distortion_coefs.size() <= 1 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[1];
939  node_tmp = node_model.append_child(LABEL_XML_K3);
940  distortion_coefs.size() <= 2 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[2];
941  node_tmp = node_model.append_child(LABEL_XML_K4);
942  distortion_coefs.size() <= 3 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[3];
943  node_tmp = node_model.append_child(LABEL_XML_K5);
944  distortion_coefs.size() <= 4 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[4];
945  }
946  break;
947  }
948  return back;
949  }
950 
957  pugi::xml_node find_additional_info(const pugi::xml_node &node_)
958  {
959  vpXmlCodeType prop;
960 
961  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
962  if (node.type() != pugi::node_element) {
963  continue;
964  }
965 
966  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
967  prop = CODE_XML_OTHER;
968  }
969 
970  if (prop == CODE_XML_ADDITIONAL_INFO) {
971  // We found the node
972  return node;
973  }
974  }
975 
976  return pugi::xml_node();
977  }
978 
979 
986  vpXmlCodeSequenceType str2xmlcode(const char *str, vpXmlCodeType &res)
987  {
988  vpXmlCodeType val_int = CODE_XML_BAD;
990 
991  if (!strcmp(str, LABEL_XML_CAMERA)) {
992  val_int = CODE_XML_CAMERA;
993  } else if (!strcmp(str, LABEL_XML_CAMERA_NAME)) {
994  val_int = CODE_XML_CAMERA_NAME;
995  } else if (!strcmp(str, LABEL_XML_MODEL)) {
996  val_int = CODE_XML_MODEL;
997  } else if (!strcmp(str, LABEL_XML_MODEL_TYPE)) {
998  val_int = CODE_XML_MODEL_TYPE;
999  } else if (!strcmp(str, LABEL_XML_WIDTH)) {
1000  val_int = CODE_XML_WIDTH;
1001  } else if (!strcmp(str, LABEL_XML_HEIGHT)) {
1002  val_int = CODE_XML_HEIGHT;
1003  } else if (!strcmp(str, LABEL_XML_SUBSAMPLING_WIDTH)) {
1004  val_int = CODE_XML_SUBSAMPLING_WIDTH;
1005  } else if (!strcmp(str, LABEL_XML_SUBSAMPLING_HEIGHT)) {
1006  val_int = CODE_XML_SUBSAMPLING_HEIGHT;
1007  } else if (!strcmp(str, LABEL_XML_FULL_WIDTH)) {
1008  val_int = CODE_XML_FULL_WIDTH;
1009  } else if (!strcmp(str, LABEL_XML_FULL_HEIGHT)) {
1010  val_int = CODE_XML_FULL_HEIGHT;
1011  } else if (!strcmp(str, LABEL_XML_U0)) {
1012  val_int = CODE_XML_U0;
1013  } else if (!strcmp(str, LABEL_XML_V0)) {
1014  val_int = CODE_XML_V0;
1015  } else if (!strcmp(str, LABEL_XML_PX)) {
1016  val_int = CODE_XML_PX;
1017  } else if (!strcmp(str, LABEL_XML_PY)) {
1018  val_int = CODE_XML_PY;
1019  } else if (!strcmp(str, LABEL_XML_KUD)) {
1020  val_int = CODE_XML_KUD;
1021  } else if (!strcmp(str, LABEL_XML_KDU)) {
1022  val_int = CODE_XML_KDU;
1023  } else if (!strcmp(str, LABEL_XML_K1)) {
1024  val_int = CODE_XML_K1;
1025  } else if (!strcmp(str, LABEL_XML_K2)) {
1026  val_int = CODE_XML_K2;
1027  } else if (!strcmp(str, LABEL_XML_K3)) {
1028  val_int = CODE_XML_K3;
1029  } else if (!strcmp(str, LABEL_XML_K4)) {
1030  val_int = CODE_XML_K4;
1031  } else if (!strcmp(str, LABEL_XML_K5)) {
1032  val_int = CODE_XML_K5;
1033  } else if (!strcmp(str, LABEL_XML_ADDITIONAL_INFO)) {
1034  val_int = CODE_XML_ADDITIONAL_INFO;
1035  } else {
1036  val_int = CODE_XML_OTHER;
1037  }
1038  res = val_int;
1039 
1040  return back;
1041  }
1042 
1043  std::string getCameraName() const { return camera_name; }
1044  vpCameraParameters getCameraParameters() const { return camera; }
1045  unsigned int getHeight() const { return image_height; }
1046  unsigned int getSubsampling_width() const { return subsampling_width; }
1047  unsigned int getSubsampling_height() const { return subsampling_height; }
1048  unsigned int getWidth() const { return image_width; }
1049 
1050  void setCameraName(const std::string &name) { camera_name = name; }
1051  void setHeight(unsigned int height) { image_height = height; }
1052  void setSubsampling_width(unsigned int subsampling) { subsampling_width = subsampling; }
1053  void setSubsampling_height(unsigned int subsampling) { subsampling_height = subsampling; }
1054  void setWidth(unsigned int width) { image_width = width; }
1055 
1056 private:
1057  vpCameraParameters camera;
1058  std::string camera_name;
1059  unsigned int image_width;
1060  unsigned int image_height;
1061  unsigned int subsampling_width;
1062  unsigned int subsampling_height;
1063  unsigned int full_width;
1064  unsigned int full_height;
1065 
1068  static const int allowedPixelDiffOnImageSize = 15;
1069 };
1070 #endif // DOXYGEN_SHOULD_SKIP_THIS
1071 
1073 {
1074 }
1075 
1077 {
1078  delete m_impl;
1079 }
1080 
1095 int vpXmlParserCamera::parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1097  unsigned int im_width, unsigned int im_height)
1098 {
1099  return m_impl->parse(cam, filename, cam_name, projModel, im_width, im_height);
1100 }
1101 
1145 int vpXmlParserCamera::save(const vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1146  unsigned int im_width, unsigned int im_height,
1147  const std::string &additionalInfo)
1148 {
1149  return m_impl->save(cam, filename, cam_name, im_width, im_height, additionalInfo);
1150 }
1151 
1153 {
1154  return m_impl->getCameraName();
1155 }
1156 
1158 {
1159  return m_impl->getCameraParameters();
1160 }
1161 
1162 unsigned int vpXmlParserCamera::getHeight() const
1163 {
1164  return m_impl->getHeight();
1165 }
1166 
1168 {
1169  return m_impl->getSubsampling_width();
1170 }
1171 
1173 {
1174  return m_impl->getSubsampling_height();
1175 }
1176 
1177 unsigned int vpXmlParserCamera::getWidth() const
1178 {
1179  return m_impl->getWidth();
1180 }
1181 
1182 void vpXmlParserCamera::setCameraName(const std::string &name)
1183 {
1184  m_impl->setCameraName(name);
1185 }
1186 
1187 void vpXmlParserCamera::setHeight(unsigned int height)
1188 {
1189  m_impl->setHeight(height);
1190 }
1191 
1192 void vpXmlParserCamera::setSubsampling_width(unsigned int subsampling)
1193 {
1194  m_impl->setSubsampling_width(subsampling);
1195 }
1196 
1197 void vpXmlParserCamera::setSubsampling_height(unsigned int subsampling)
1198 {
1199  m_impl->setSubsampling_height(subsampling);
1200 }
1201 
1202 void vpXmlParserCamera::setWidth(unsigned int width)
1203 {
1204  m_impl->setWidth(width);
1205 }
1206 
1207 #elif !defined(VISP_BUILD_SHARED_LIBS)
1208 // Work arround to avoid warning: libvisp_core.a(vpXmlParserCamera.cpp.o) has
1209 // no symbols
1210 void dummy_vpXmlParserCamera(){};
1211 #endif // VISP_HAVE_PUGIXML
unsigned int getWidth() const
unsigned int getSubsampling_height() const
double get_kdu() const
vpCameraParametersProjType get_projModel() const
#define vpCERROR
Definition: vpDebug.h:365
#define vpERROR_TRACE
Definition: vpDebug.h:393
void setHeight(unsigned int height)
void setWidth(unsigned int width)
void setCameraName(const std::string &name)
void initProjWithKannalaBrandtDistortion(double px, double py, double u0, double v0, const std::vector< double > &distortion_coefficients)
vpCameraParameters getCameraParameters() const
void setSubsampling_height(unsigned int subsampling)
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
Generic class defining intrinsic camera parameters.
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)
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
double get_kud() const
void setSubsampling_width(unsigned int subsampling)
unsigned int getSubsampling_width() const
unsigned int getHeight() const