Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
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()
118  : camera(), camera_name(), image_width(0), image_height(0), subsampling_width(0), subsampling_height(0),
119  full_width(0), full_height(0)
120  {
121  }
122 
123  int parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
124  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
125  unsigned int im_height)
126  {
127  pugi::xml_document doc;
128  if (!doc.load_file(filename.c_str())) {
129  return SEQUENCE_ERROR;
130  }
131 
132  pugi::xml_node node = doc.document_element();
133  if (!node) {
134  return SEQUENCE_ERROR;
135  }
136 
137  int ret = read(node, cam_name, projModel, im_width, im_height);
138 
139  cam = camera;
140 
141  return ret;
142  }
143 
160  int read(const pugi::xml_node &node_, const std::string &cam_name,
161  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
162  unsigned int im_height, 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 == read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height))
179  nbCamera++;
180  } else
181  back = SEQUENCE_ERROR;
182  }
183 
184  if (nbCamera == 0) {
185  back = SEQUENCE_ERROR;
186  vpCERROR << "No camera parameters is available" << std::endl << "with your specifications" << std::endl;
187  } else if (nbCamera > 1) {
188  back = SEQUENCE_ERROR;
189  vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl
190  << "with your specifications : " << std::endl
191  << "precise your choice..." << std::endl;
192  }
193 
194  return back;
195  }
196 
213  int read_camera(const pugi::xml_node &node_, const std::string &cam_name,
214  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
215  unsigned int im_height, unsigned int subsampl_width, unsigned int subsampl_height)
216  {
217  vpXmlCodeType prop;
218  /* read value in the XML file. */
219  std::string camera_name_tmp = "";
220  unsigned int image_height_tmp = 0;
221  unsigned int image_width_tmp = 0;
222  unsigned int subsampling_width_tmp = 0;
223  unsigned int subsampling_height_tmp = 0;
224  vpCameraParameters cam_tmp;
225  vpCameraParameters cam_tmp_model;
226  bool projModelFound = false;
228 
229  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
230  if (node.type() != pugi::node_element)
231  continue;
232 
233  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
234  prop = CODE_XML_OTHER;
235  back = SEQUENCE_ERROR;
236  }
237 
238  switch (prop) {
239  case CODE_XML_CAMERA_NAME: {
240  camera_name_tmp = node.text().as_string();
241  std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl;
242  break;
243  }
244  case CODE_XML_WIDTH:
245  image_width_tmp = node.text().as_uint();
246  break;
247 
248  case CODE_XML_HEIGHT:
249  image_height_tmp = node.text().as_uint();
250  break;
251  case CODE_XML_SUBSAMPLING_WIDTH:
252  subsampling_width_tmp = node.text().as_uint();
253  break;
254  case CODE_XML_SUBSAMPLING_HEIGHT:
255  subsampling_height_tmp = node.text().as_uint();
256  break;
257 
258  case CODE_XML_MODEL:
259  back = read_camera_model(node, cam_tmp_model);
260  if (cam_tmp_model.get_projModel() == projModel) {
261  cam_tmp = cam_tmp_model;
262  projModelFound = true;
263  }
264  break;
265 
266  case CODE_XML_ADDITIONAL_INFO:
267  break;
268 
269  case CODE_XML_BAD:
270  case CODE_XML_OTHER:
271  case CODE_XML_CAMERA:
272  case CODE_XML_FULL_HEIGHT:
273  case CODE_XML_FULL_WIDTH:
274  case CODE_XML_MODEL_TYPE:
275  case CODE_XML_U0:
276  case CODE_XML_V0:
277  case CODE_XML_PX:
278  case CODE_XML_PY:
279  case CODE_XML_KUD:
280  case CODE_XML_KDU:
281  case CODE_XML_K1:
282  case CODE_XML_K2:
283  case CODE_XML_K3:
284  case CODE_XML_K4:
285  case CODE_XML_K5:
286  default:
287  back = SEQUENCE_ERROR;
288  break;
289  }
290  }
291  // Create a specific test for subsampling_width and subsampling_height to
292  // ensure that division by zero is not possible in the next test
293  bool test_subsampling_width = true;
294  bool test_subsampling_height = true;
295 
296  if (subsampling_width) {
297  test_subsampling_width = (abs((int)subsampl_width - (int)subsampling_width_tmp) <
298  (allowedPixelDiffOnImageSize * (int)(subsampling_width_tmp / subsampling_width)));
299  }
300  if (subsampling_height) {
301  test_subsampling_height = (abs((int)subsampl_height - (int)subsampling_height_tmp) <
302  (allowedPixelDiffOnImageSize * (int)(subsampling_height_tmp / subsampling_height)));
303  }
304  if (!((projModelFound == true) && (cam_name == camera_name_tmp) &&
305  (abs((int)im_width - (int)image_width_tmp) < allowedPixelDiffOnImageSize || im_width == 0) &&
306  (abs((int)im_height - (int)image_height_tmp) < allowedPixelDiffOnImageSize || im_height == 0) &&
307  (test_subsampling_width) && (test_subsampling_height))) {
308  back = SEQUENCE_ERROR;
309  } else {
310  camera = cam_tmp;
311  camera_name = camera_name_tmp;
312  image_width = image_width_tmp;
313  image_height = image_height_tmp;
314  subsampling_width = subsampling_width_tmp;
315  subsampling_height = subsampling_height_tmp;
316  full_width = subsampling_width_tmp * image_width_tmp;
317  full_height = subsampling_height_tmp * image_height_tmp;
318  }
319  return back;
320  }
321 
328  vpXmlCodeSequenceType read_camera_model(const pugi::xml_node &node_, vpCameraParameters &cam_tmp)
329  {
330  // counter of the number of read parameters
331  int nb = 0;
332  vpXmlCodeType prop;
333  /* read value in the XML file. */
334 
335  std::string model_type = "";
336  double u0 = cam_tmp.get_u0();
337  double v0 = cam_tmp.get_v0();
338  double px = cam_tmp.get_px();
339  double py = cam_tmp.get_py();
340  double kud = cam_tmp.get_kud();
341  double kdu = cam_tmp.get_kdu();
342  std::vector<double> distortion_coeffs;
344  int validation = 0;
345 
346  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
347  // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
348  if (node.type() != pugi::node_element)
349  continue;
350 
351  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
352  prop = CODE_XML_OTHER;
353  back = SEQUENCE_ERROR;
354  }
355 
356  switch (prop) {
357  case CODE_XML_MODEL_TYPE: {
358  model_type = node.text().as_string();
359  nb++;
360  validation = validation | 0x01;
361  } break;
362  case CODE_XML_U0:
363  u0 = node.text().as_double();
364  nb++;
365  validation = validation | 0x02;
366  break;
367  case CODE_XML_V0:
368  v0 = node.text().as_double();
369  nb++;
370  validation = validation | 0x04;
371  break;
372  case CODE_XML_PX:
373  px = node.text().as_double();
374  nb++;
375  validation = validation | 0x08;
376  break;
377  case CODE_XML_PY:
378  py = node.text().as_double();
379  nb++;
380  validation = validation | 0x10;
381  break;
382  case CODE_XML_KUD:
383  kud = node.text().as_double();
384  nb++;
385  validation = validation | 0x20;
386  break;
387  case CODE_XML_KDU:
388  kdu = node.text().as_double();
389  nb++;
390  validation = validation | 0x40;
391  break;
392  case CODE_XML_K1:
393  distortion_coeffs.push_back(node.text().as_double());
394  nb++;
395  validation = validation | 0x20;
396  break;
397  case CODE_XML_K2:
398  distortion_coeffs.push_back(node.text().as_double());
399  nb++;
400  validation = validation | 0x40;
401  break;
402  case CODE_XML_K3:
403  distortion_coeffs.push_back(node.text().as_double());
404  nb++;
405  validation = validation | 0x80;
406  break;
407  case CODE_XML_K4:
408  distortion_coeffs.push_back(node.text().as_double());
409  nb++;
410  validation = validation | 0x100;
411  break;
412  case CODE_XML_K5:
413  distortion_coeffs.push_back(node.text().as_double());
414  nb++;
415  validation = validation | 0x200;
416  break;
417  case CODE_XML_BAD:
418  case CODE_XML_OTHER:
419  case CODE_XML_CAMERA:
420  case CODE_XML_CAMERA_NAME:
421  case CODE_XML_HEIGHT:
422  case CODE_XML_WIDTH:
423  case CODE_XML_SUBSAMPLING_WIDTH:
424  case CODE_XML_SUBSAMPLING_HEIGHT:
425  case CODE_XML_FULL_HEIGHT:
426  case CODE_XML_FULL_WIDTH:
427  case CODE_XML_MODEL:
428  case CODE_XML_ADDITIONAL_INFO:
429  default:
430  back = SEQUENCE_ERROR;
431  break;
432  }
433  }
434 
435  if (model_type.empty()) {
436  vpERROR_TRACE("projection model type doesn't match with any known model !");
437  return SEQUENCE_ERROR;
438  }
439 
440  if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITHOUT_DISTORTION)) {
441  if (nb != 5 || validation != 0x001F) {
442  vpCERROR << "ERROR in 'model' field:\n";
443  vpCERROR << "it must contain 5 parameters\n";
444 
445  return SEQUENCE_ERROR;
446  }
447  cam_tmp.initPersProjWithoutDistortion(px, py, u0, v0);
448  } else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_DISTORTION)) {
449  if (nb != 7 || validation != 0x7F) {
450  vpCERROR << "ERROR in 'model' field:\n";
451  vpCERROR << "it must contain 7 parameters\n";
452 
453  return SEQUENCE_ERROR;
454  }
455  cam_tmp.initPersProjWithDistortion(px, py, u0, v0, kud, kdu);
456  } else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION)) {
457  if (nb != 10 || validation != 0x3FF) { // at least one coefficient is missing. We should know which one
458  vpCERROR << "ERROR in 'model' field:\n";
459  vpCERROR << "it must contain 10 parameters\n";
460 
461  std::vector<double> fixed_distortion_coeffs;
462 
463  // In case disortion coefficients are missing, we should complete them with 0 values
464  // Since 0x3FF is 0011|1111|1111 and we are interrested in the most significant 1s shown below
465  // -- ---
466  // If we divide by 32 (>> 2^5 : 5 remaining least significant bits), we will have to check 5 bits only
467  int check = validation / 32;
468  int j = 0;
469 
470  for (int i = 0; i < 5; i++) {
471  int bit = check % 2; // if bit == 1 => the corresponding distortion coefficient is present.
472  if (!bit)
473  fixed_distortion_coeffs.push_back(0.);
474  else
475  fixed_distortion_coeffs.push_back(distortion_coeffs[j++]);
476  check /= 2;
477  }
478 
479  cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, fixed_distortion_coeffs);
480  return SEQUENCE_ERROR;
481  }
482  cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, distortion_coeffs);
483  } else {
484  vpERROR_TRACE("projection model type doesn't match with any known model !");
485 
486  return SEQUENCE_ERROR;
487  }
488  return back;
489  }
490 
491  int save(const vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
492  unsigned int im_width, unsigned int im_height, const std::string &additionalInfo)
493  {
494  pugi::xml_document doc;
495  pugi::xml_node node;
496 
497  if (!doc.load_file(filename.c_str(), pugi::parse_default | pugi::parse_comments)) {
498  node = doc.append_child(pugi::node_declaration);
499  node.append_attribute("version") = "1.0";
500  node = doc.append_child(LABEL_XML_ROOT);
501  pugi::xml_node nodeComment = node.append_child(pugi::node_comment);
502  nodeComment.set_value("This file stores intrinsic camera parameters used\n"
503  " in the vpCameraParameters Class of ViSP available\n"
504  " at https://visp.inria.fr/download/ .\n"
505  " It can be read with the parse method of\n"
506  " the vpXmlParserCamera class.");
507  }
508 
509  node = doc.document_element();
510  if (!node) {
511  return SEQUENCE_ERROR;
512  }
513 
514  camera = cam;
515 
516  int nbCamera = count(node, cam_name, cam.get_projModel(), im_width, im_height);
517  if (nbCamera) {
518  return SEQUENCE_ERROR;
519  }
520 
521  pugi::xml_node nodeCamera = find_camera(node, cam_name, im_width, im_height);
522  if (!nodeCamera) {
523  write(node, cam_name, im_width, im_height);
524  } else {
525  write_camera(nodeCamera);
526  }
527 
528  if (!additionalInfo.empty()) {
529  // Get camera node pointer
530  nodeCamera = find_camera(node, cam_name, im_width, im_height);
531 
532  // Additional information provided by the user
533  pugi::xml_node nodeAdditionalInfo = find_additional_info(nodeCamera);
534 
535  if (!nodeAdditionalInfo) {
536  // Create the additional information node
537  pugi::xml_node node_comment = nodeCamera.append_child(pugi::node_comment);
538  node_comment.set_value("Additional information");
539 
540  nodeAdditionalInfo = nodeCamera.append_child(LABEL_XML_ADDITIONAL_INFO);
541  }
542 
543  if (nodeAdditionalInfo) {
544  // Add the information in this specific node
545  pugi::xml_document tmpDoc;
546  if (tmpDoc.load_string(additionalInfo.c_str())) {
547  for (node = tmpDoc.first_child(); node; node = node.next_sibling()) {
548  nodeAdditionalInfo.append_copy(node);
549  }
550  }
551  }
552  }
553 
554  doc.save_file(filename.c_str(), PUGIXML_TEXT(" "));
555 
556  return SEQUENCE_OK;
557  }
558 
576  int count(const pugi::xml_node &node_, const std::string &cam_name,
577  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
578  unsigned int im_height, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
579  {
580  vpXmlCodeType prop;
581  int nbCamera = 0;
582 
583  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
584  if (node.type() != pugi::node_element)
585  continue;
586 
587  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
588  prop = CODE_XML_OTHER;
589  }
590 
591  if (prop == CODE_XML_CAMERA) {
592  if (SEQUENCE_OK == read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height))
593  nbCamera++;
594  }
595  }
596 
597  return nbCamera;
598  }
599 
616  pugi::xml_node find_camera(const pugi::xml_node &node_, const std::string &cam_name, unsigned int im_width,
617  unsigned int im_height, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
618  {
619  vpXmlCodeType prop;
620 
621  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
622  if (node.type() != pugi::node_element)
623  continue;
624 
625  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
626  prop = CODE_XML_OTHER;
627  }
628  if (prop == CODE_XML_CAMERA) {
629  if (SEQUENCE_OK == read_camera_header(node, cam_name, im_width, im_height, subsampl_width, subsampl_height)) {
630  return node;
631  }
632  }
633  }
634  return pugi::xml_node();
635  }
636 
652  int read_camera_header(const pugi::xml_node &node_, const std::string &cam_name, unsigned int im_width,
653  unsigned int im_height, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
654  {
655  vpXmlCodeType prop;
656  /* read value in the XML file. */
657  std::string camera_name_tmp = "";
658  unsigned int image_height_tmp = 0;
659  unsigned int image_width_tmp = 0;
660  unsigned int subsampling_width_tmp = 0;
661  unsigned int subsampling_height_tmp = 0;
663 
664  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
665  if (node.type() != pugi::node_element)
666  continue;
667  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
668  prop = CODE_XML_OTHER;
669  back = SEQUENCE_ERROR;
670  }
671 
672  switch (prop) {
673  case CODE_XML_CAMERA_NAME:
674  camera_name_tmp = node.text().as_string();
675  break;
676 
677  case CODE_XML_WIDTH:
678  image_width_tmp = node.text().as_uint();
679  break;
680 
681  case CODE_XML_HEIGHT:
682  image_height_tmp = node.text().as_uint();
683  break;
684 
685  case CODE_XML_SUBSAMPLING_WIDTH:
686  subsampling_width_tmp = node.text().as_uint();
687  break;
688 
689  case CODE_XML_SUBSAMPLING_HEIGHT:
690  subsampling_height_tmp = node.text().as_uint();
691  break;
692 
693  case CODE_XML_MODEL:
694  break;
695 
696  case CODE_XML_ADDITIONAL_INFO:
697  break;
698 
699  case CODE_XML_BAD:
700  case CODE_XML_OTHER:
701  case CODE_XML_CAMERA:
702  case CODE_XML_FULL_HEIGHT:
703  case CODE_XML_FULL_WIDTH:
704  case CODE_XML_MODEL_TYPE:
705  case CODE_XML_U0:
706  case CODE_XML_V0:
707  case CODE_XML_PX:
708  case CODE_XML_PY:
709  case CODE_XML_KUD:
710  case CODE_XML_KDU:
711  default:
712  back = SEQUENCE_ERROR;
713  break;
714  }
715  }
716  if (!((cam_name == camera_name_tmp) && (im_width == image_width_tmp || im_width == 0) &&
717  (im_height == image_height_tmp || im_height == 0) &&
718  (subsampl_width == subsampling_width_tmp || subsampl_width == 0) &&
719  (subsampl_height == subsampling_height_tmp || subsampl_height == 0))) {
720  back = SEQUENCE_ERROR;
721  }
722  return back;
723  }
724 
740  int write(pugi::xml_node &node, const std::string &cam_name, unsigned int im_width, unsigned int im_height,
741  unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
742  {
743  int back = SEQUENCE_OK;
744 
745  // <camera>
746  pugi::xml_node node_camera = node.append_child(LABEL_XML_CAMERA);
747 
748  pugi::xml_node node_tmp;
749  {
750  //<name>
751  if (!cam_name.empty()) {
752  node_tmp = node_camera.append_child(pugi::node_comment);
753  node_tmp.set_value("Name of the camera");
754  node_tmp = node_camera.append_child(LABEL_XML_CAMERA_NAME);
755  node_tmp.append_child(pugi::node_pcdata).set_value(cam_name.c_str());
756  }
757 
758  if (im_width != 0 || im_height != 0) {
759  node_tmp = node_camera.append_child(pugi::node_comment);
760  node_tmp.set_value("Size of the image on which camera "
761  "calibration was performed");
762 
763  //<image_width>
764  node_tmp = node_camera.append_child(LABEL_XML_WIDTH);
765  node_tmp.append_child(pugi::node_pcdata).text() = im_width;
766 
767  //<image_height>
768  node_tmp = node_camera.append_child(LABEL_XML_HEIGHT);
769  node_tmp.append_child(pugi::node_pcdata).text() = im_height;
770  if (subsampling_width != 0 || subsampling_height != 0) {
771  node_tmp = node_camera.append_child(pugi::node_comment);
772  node_tmp.set_value("Subsampling used to obtain the "
773  "current size of the image.");
774 
775  //<subsampling_width>
776  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_WIDTH);
777  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_width;
778  //<subsampling_height>
779  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_HEIGHT);
780  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_height;
781  node_tmp = node_camera.append_child(pugi::node_comment);
782  node_tmp.set_value("The full size is the sensor size actually used to "
783  "grab the image. full_width = subsampling_width * "
784  "image_width");
785 
786  //<full_width>
787  node_tmp = node_camera.append_child(LABEL_XML_FULL_WIDTH);
788  node_tmp.append_child(pugi::node_pcdata).text() = im_width * subsampl_width;
789  //<full_height>
790  node_tmp = node_camera.append_child(LABEL_XML_FULL_HEIGHT);
791  node_tmp.append_child(pugi::node_pcdata).text() = im_height * subsampl_height;
792  }
793  }
794 
795  node_tmp = node_camera.append_child(pugi::node_comment);
796  node_tmp.set_value("Intrinsic camera parameters "
797  "computed for each projection model");
798 
799  back = write_camera(node_camera);
800  }
801  return back;
802  }
803 
809  int write_camera(pugi::xml_node &node_camera)
810  {
811  pugi::xml_node node_model;
812  pugi::xml_node node_tmp;
813 
814  int back = SEQUENCE_OK;
815  switch (camera.get_projModel()) {
817  //<model>
818  node_model = node_camera.append_child(LABEL_XML_MODEL);
819  {
820  node_tmp = node_model.append_child(pugi::node_comment);
821  node_tmp.set_value("Projection model type");
822 
823  //<type>without_distortion</type>
824  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
825  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITHOUT_DISTORTION);
826 
827  node_tmp = node_model.append_child(pugi::node_comment);
828  node_tmp.set_value("Pixel ratio");
829  //<px>
830  node_tmp = node_model.append_child(LABEL_XML_PX);
831  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
832  //<py>
833  node_tmp = node_model.append_child(LABEL_XML_PY);
834  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
835 
836  node_tmp = node_model.append_child(pugi::node_comment);
837  node_tmp.set_value("Principal point");
838 
839  //<u0>
840  node_tmp = node_model.append_child(LABEL_XML_U0);
841  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
842  //<v0>
843  node_tmp = node_model.append_child(LABEL_XML_V0);
844  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
845  }
846  break;
847 
849  //<model>
850  node_model = node_camera.append_child(LABEL_XML_MODEL);
851  {
852  node_tmp = node_model.append_child(pugi::node_comment);
853  node_tmp.set_value("Projection model type");
854  //<type>with_distortion</type>
855  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
856  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_DISTORTION);
857 
858  node_tmp = node_model.append_child(pugi::node_comment);
859  node_tmp.set_value("Pixel ratio");
860  //<px>
861  node_tmp = node_model.append_child(LABEL_XML_PX);
862  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
863  //<py>
864  node_tmp = node_model.append_child(LABEL_XML_PY);
865  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
866 
867  node_tmp = node_model.append_child(pugi::node_comment);
868  node_tmp.set_value("Principal point");
869  //<u0>
870  node_tmp = node_model.append_child(LABEL_XML_U0);
871  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
872  //<v0>
873  node_tmp = node_model.append_child(LABEL_XML_V0);
874  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
875 
876  //<kud>
877  node_tmp = node_model.append_child(pugi::node_comment);
878  node_tmp.set_value("Undistorted to distorted distortion parameter");
879  node_tmp = node_model.append_child(LABEL_XML_KUD);
880  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kud();
881 
882  //<kud>
883  node_tmp = node_model.append_child(pugi::node_comment);
884  node_tmp.set_value("Distorted to undistorted distortion parameter");
885  node_tmp = node_model.append_child(LABEL_XML_KDU);
886  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kdu();
887  }
888  break;
889 
891  //<model>
892  node_model = node_camera.append_child(LABEL_XML_MODEL);
893  {
894  node_tmp = node_model.append_child(pugi::node_comment);
895  node_tmp.set_value("Projection model type");
896  //<type>with_KannalaBrandt_distortion</type>
897  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
898  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION);
899 
900  node_tmp = node_model.append_child(pugi::node_comment);
901  node_tmp.set_value("Pixel ratio");
902  //<px>
903  node_tmp = node_model.append_child(LABEL_XML_PX);
904  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
905  //<py>
906  node_tmp = node_model.append_child(LABEL_XML_PY);
907  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
908 
909  node_tmp = node_model.append_child(pugi::node_comment);
910  node_tmp.set_value("Principal point");
911  //<u0>
912  node_tmp = node_model.append_child(LABEL_XML_U0);
913  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
914  //<v0>
915  node_tmp = node_model.append_child(LABEL_XML_V0);
916  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
917 
918  //<k1>, <k2>, <k3>, <k4>, <k5>
919  std::vector<double> distortion_coefs = camera.getKannalaBrandtDistortionCoefficients();
920 
921  if (distortion_coefs.size() != 5)
922  std::cout << "Make sure to have 5 distortion coefficients for Kannala-Brandt distortions." << std::endl;
923 
924  node_tmp = node_model.append_child(pugi::node_comment);
925  node_tmp.set_value("Distortion coefficients");
926  node_tmp = node_model.append_child(LABEL_XML_K1);
927  distortion_coefs.size() == 0 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
928  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[0];
929  node_tmp = node_model.append_child(LABEL_XML_K2);
930  distortion_coefs.size() <= 1 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
931  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[1];
932  node_tmp = node_model.append_child(LABEL_XML_K3);
933  distortion_coefs.size() <= 2 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
934  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[2];
935  node_tmp = node_model.append_child(LABEL_XML_K4);
936  distortion_coefs.size() <= 3 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
937  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[3];
938  node_tmp = node_model.append_child(LABEL_XML_K5);
939  distortion_coefs.size() <= 4 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
940  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[4];
941  }
942  break;
943  }
944  return back;
945  }
946 
953  pugi::xml_node find_additional_info(const pugi::xml_node &node_)
954  {
955  vpXmlCodeType prop;
956 
957  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
958  if (node.type() != pugi::node_element) {
959  continue;
960  }
961 
962  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
963  prop = CODE_XML_OTHER;
964  }
965 
966  if (prop == CODE_XML_ADDITIONAL_INFO) {
967  // We found the node
968  return node;
969  }
970  }
971 
972  return pugi::xml_node();
973  }
974 
981  vpXmlCodeSequenceType str2xmlcode(const char *str, vpXmlCodeType &res)
982  {
983  vpXmlCodeType val_int = CODE_XML_BAD;
985 
986  if (!strcmp(str, LABEL_XML_CAMERA)) {
987  val_int = CODE_XML_CAMERA;
988  } else if (!strcmp(str, LABEL_XML_CAMERA_NAME)) {
989  val_int = CODE_XML_CAMERA_NAME;
990  } else if (!strcmp(str, LABEL_XML_MODEL)) {
991  val_int = CODE_XML_MODEL;
992  } else if (!strcmp(str, LABEL_XML_MODEL_TYPE)) {
993  val_int = CODE_XML_MODEL_TYPE;
994  } else if (!strcmp(str, LABEL_XML_WIDTH)) {
995  val_int = CODE_XML_WIDTH;
996  } else if (!strcmp(str, LABEL_XML_HEIGHT)) {
997  val_int = CODE_XML_HEIGHT;
998  } else if (!strcmp(str, LABEL_XML_SUBSAMPLING_WIDTH)) {
999  val_int = CODE_XML_SUBSAMPLING_WIDTH;
1000  } else if (!strcmp(str, LABEL_XML_SUBSAMPLING_HEIGHT)) {
1001  val_int = CODE_XML_SUBSAMPLING_HEIGHT;
1002  } else if (!strcmp(str, LABEL_XML_FULL_WIDTH)) {
1003  val_int = CODE_XML_FULL_WIDTH;
1004  } else if (!strcmp(str, LABEL_XML_FULL_HEIGHT)) {
1005  val_int = CODE_XML_FULL_HEIGHT;
1006  } else if (!strcmp(str, LABEL_XML_U0)) {
1007  val_int = CODE_XML_U0;
1008  } else if (!strcmp(str, LABEL_XML_V0)) {
1009  val_int = CODE_XML_V0;
1010  } else if (!strcmp(str, LABEL_XML_PX)) {
1011  val_int = CODE_XML_PX;
1012  } else if (!strcmp(str, LABEL_XML_PY)) {
1013  val_int = CODE_XML_PY;
1014  } else if (!strcmp(str, LABEL_XML_KUD)) {
1015  val_int = CODE_XML_KUD;
1016  } else if (!strcmp(str, LABEL_XML_KDU)) {
1017  val_int = CODE_XML_KDU;
1018  } else if (!strcmp(str, LABEL_XML_K1)) {
1019  val_int = CODE_XML_K1;
1020  } else if (!strcmp(str, LABEL_XML_K2)) {
1021  val_int = CODE_XML_K2;
1022  } else if (!strcmp(str, LABEL_XML_K3)) {
1023  val_int = CODE_XML_K3;
1024  } else if (!strcmp(str, LABEL_XML_K4)) {
1025  val_int = CODE_XML_K4;
1026  } else if (!strcmp(str, LABEL_XML_K5)) {
1027  val_int = CODE_XML_K5;
1028  } else if (!strcmp(str, LABEL_XML_ADDITIONAL_INFO)) {
1029  val_int = CODE_XML_ADDITIONAL_INFO;
1030  } else {
1031  val_int = CODE_XML_OTHER;
1032  }
1033  res = val_int;
1034 
1035  return back;
1036  }
1037 
1038  std::string getCameraName() const { return camera_name; }
1039  vpCameraParameters getCameraParameters() const { return camera; }
1040  unsigned int getHeight() const { return image_height; }
1041  unsigned int getSubsampling_width() const { return subsampling_width; }
1042  unsigned int getSubsampling_height() const { return subsampling_height; }
1043  unsigned int getWidth() const { return image_width; }
1044 
1045  void setCameraName(const std::string &name) { camera_name = name; }
1046  void setHeight(unsigned int height) { image_height = height; }
1047  void setSubsampling_width(unsigned int subsampling) { subsampling_width = subsampling; }
1048  void setSubsampling_height(unsigned int subsampling) { subsampling_height = subsampling; }
1049  void setWidth(unsigned int width) { image_width = width; }
1050 
1051 private:
1052  vpCameraParameters camera;
1053  std::string camera_name;
1054  unsigned int image_width;
1055  unsigned int image_height;
1056  unsigned int subsampling_width;
1057  unsigned int subsampling_height;
1058  unsigned int full_width;
1059  unsigned int full_height;
1060 
1063  static const int allowedPixelDiffOnImageSize = 15;
1064 };
1065 #endif // DOXYGEN_SHOULD_SKIP_THIS
1066 
1067 vpXmlParserCamera::vpXmlParserCamera() : m_impl(new Impl()) {}
1068 
1070 
1085 int vpXmlParserCamera::parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1086  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
1087  unsigned int im_height)
1088 {
1089  return m_impl->parse(cam, filename, cam_name, projModel, im_width, im_height);
1090 }
1091 
1135 int vpXmlParserCamera::save(const vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1136  unsigned int im_width, unsigned int im_height, const std::string &additionalInfo)
1137 {
1138  return m_impl->save(cam, filename, cam_name, im_width, im_height, additionalInfo);
1139 }
1140 
1141 std::string vpXmlParserCamera::getCameraName() const { return m_impl->getCameraName(); }
1142 
1143 vpCameraParameters vpXmlParserCamera::getCameraParameters() const { return m_impl->getCameraParameters(); }
1144 
1145 unsigned int vpXmlParserCamera::getHeight() const { return m_impl->getHeight(); }
1146 
1147 unsigned int vpXmlParserCamera::getSubsampling_width() const { return m_impl->getSubsampling_width(); }
1148 
1149 unsigned int vpXmlParserCamera::getSubsampling_height() const { return m_impl->getSubsampling_height(); }
1150 
1151 unsigned int vpXmlParserCamera::getWidth() const { return m_impl->getWidth(); }
1152 
1153 void vpXmlParserCamera::setCameraName(const std::string &name) { m_impl->setCameraName(name); }
1154 
1155 void vpXmlParserCamera::setHeight(unsigned int height) { m_impl->setHeight(height); }
1156 
1157 void vpXmlParserCamera::setSubsampling_width(unsigned int subsampling) { m_impl->setSubsampling_width(subsampling); }
1158 
1159 void vpXmlParserCamera::setSubsampling_height(unsigned int subsampling) { m_impl->setSubsampling_height(subsampling); }
1160 
1161 void vpXmlParserCamera::setWidth(unsigned int width) { m_impl->setWidth(width); }
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
double get_kdu() const
vpCameraParametersProjType get_projModel() const
void initProjWithKannalaBrandtDistortion(double px, double py, double u0, double v0, const std::vector< double > &distortion_coefficients)
double get_kud() const
void setSubsampling_width(unsigned int subsampling)
void setWidth(unsigned int width)
unsigned int getHeight() const
vpCameraParameters getCameraParameters() const
unsigned int getWidth() const
void setSubsampling_height(unsigned int subsampling)
void setCameraName(const std::string &name)
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="")
void setHeight(unsigned int height)
unsigned int getSubsampling_height() const
unsigned int getSubsampling_width() const
std::string getCameraName() 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)
#define vpCERROR
Definition: vpDebug.h:365
#define vpERROR_TRACE
Definition: vpDebug.h:393