Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpXmlParserCamera.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 #if defined(VISP_HAVE_PUGIXML)
45 #include <pugixml.hpp>
46 
47 #include <visp3/core/vpDebug.h>
48 /* --------------------------------------------------------------------------
49  */
50  /* --- LABEL XML ------------------------------------------------------------
51  */
52  /* --------------------------------------------------------------------------
53  */
54 
55 #define LABEL_XML_ROOT "root"
56 #define LABEL_XML_CAMERA "camera"
57 #define LABEL_XML_CAMERA_NAME "name"
58 #define LABEL_XML_WIDTH "image_width"
59 #define LABEL_XML_HEIGHT "image_height"
60 #define LABEL_XML_SUBSAMPLING_WIDTH "subsampling_width"
61 #define LABEL_XML_SUBSAMPLING_HEIGHT "subsampling_height"
62 #define LABEL_XML_FULL_WIDTH "full_width"
63 #define LABEL_XML_FULL_HEIGHT "full_height"
64 #define LABEL_XML_MODEL "model"
65 #define LABEL_XML_MODEL_TYPE "type"
66 #define LABEL_XML_U0 "u0"
67 #define LABEL_XML_V0 "v0"
68 #define LABEL_XML_PX "px"
69 #define LABEL_XML_PY "py"
70 #define LABEL_XML_KUD "kud"
71 #define LABEL_XML_KDU "kdu"
72 #define LABEL_XML_K1 "k1"
73 #define LABEL_XML_K2 "k2"
74 #define LABEL_XML_K3 "k3"
75 #define LABEL_XML_K4 "k4"
76 #define LABEL_XML_K5 "k5"
77 
78 #define LABEL_XML_MODEL_WITHOUT_DISTORTION "perspectiveProjWithoutDistortion"
79 #define LABEL_XML_MODEL_WITH_DISTORTION "perspectiveProjWithDistortion"
80 #define LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION "ProjWithKannalaBrandtDistortion"
81 
82 #define LABEL_XML_ADDITIONAL_INFO "additional_information"
83 
84 #ifndef DOXYGEN_SHOULD_SKIP_THIS
85 class vpXmlParserCamera::Impl
86 {
87 private:
88  /* --- XML Code------------------------------------------------------------
89  */
90  enum vpXmlCodeType
91  {
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()
120  : camera(), camera_name(), image_width(0), image_height(0), subsampling_width(0), subsampling_height(0),
121  full_width(0), full_height(0)
122  { }
123 
124  int parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
125  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
126  unsigned int im_height, bool verbose)
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, verbose);
139 
140  cam = camera;
141 
142  return ret;
143  }
144 
162  int read(const pugi::xml_node &node_, const std::string &cam_name,
163  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
164  unsigned int im_height, bool verbose, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
165  {
166  vpXmlCodeType prop;
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 == read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height, verbose))
180  nbCamera++;
181  }
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  }
190  else if (nbCamera > 1) {
191  back = SEQUENCE_ERROR;
192  vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl
193  << "with your specifications : " << std::endl
194  << "precise your choice..." << std::endl;
195  }
196 
197  return back;
198  }
199 
217  int read_camera(const pugi::xml_node &node_, const std::string &cam_name,
218  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
219  unsigned int im_height, unsigned int subsampl_width, unsigned int subsampl_height, bool verbose)
220  {
221  vpXmlCodeType prop;
222  /* read value in the XML file. */
223  std::string camera_name_tmp = "";
224  unsigned int image_height_tmp = 0;
225  unsigned int image_width_tmp = 0;
226  unsigned int subsampling_width_tmp = 0;
227  unsigned int subsampling_height_tmp = 0;
228  vpCameraParameters cam_tmp;
229  vpCameraParameters cam_tmp_model;
230  bool same_proj_model = false;
232 
233  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
234  if (node.type() != pugi::node_element)
235  continue;
236 
237  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
238  prop = CODE_XML_OTHER;
239  back = SEQUENCE_ERROR;
240  }
241 
242  switch (prop) {
243  case CODE_XML_CAMERA_NAME: {
244  camera_name_tmp = node.text().as_string();
245  if (verbose) {
246  std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl;
247  }
248  break;
249  }
250  case CODE_XML_WIDTH:
251  image_width_tmp = node.text().as_uint();
252  break;
253 
254  case CODE_XML_HEIGHT:
255  image_height_tmp = node.text().as_uint();
256  break;
257  case CODE_XML_SUBSAMPLING_WIDTH:
258  subsampling_width_tmp = node.text().as_uint();
259  break;
260  case CODE_XML_SUBSAMPLING_HEIGHT:
261  subsampling_height_tmp = node.text().as_uint();
262  break;
263 
264  case CODE_XML_MODEL:
265  back = read_camera_model(node, cam_tmp_model);
266  if (cam_tmp_model.get_projModel() == projModel) {
267  cam_tmp = cam_tmp_model;
268  same_proj_model = true; // Same projection model
269  }
270  break;
271 
272  case CODE_XML_ADDITIONAL_INFO:
273  break;
274 
275  case CODE_XML_BAD:
276  case CODE_XML_OTHER:
277  case CODE_XML_CAMERA:
278  case CODE_XML_FULL_HEIGHT:
279  case CODE_XML_FULL_WIDTH:
280  case CODE_XML_MODEL_TYPE:
281  case CODE_XML_U0:
282  case CODE_XML_V0:
283  case CODE_XML_PX:
284  case CODE_XML_PY:
285  case CODE_XML_KUD:
286  case CODE_XML_KDU:
287  case CODE_XML_K1:
288  case CODE_XML_K2:
289  case CODE_XML_K3:
290  case CODE_XML_K4:
291  case CODE_XML_K5:
292  default:
293  back = SEQUENCE_ERROR;
294 
295  break;
296  }
297  }
298  // Create a specific test for subsampling_width and subsampling_height to
299  // ensure that division by zero is not possible in the next test
300  bool test_subsampling_width = true;
301  bool test_subsampling_height = true;
302 
303  if (subsampling_width) {
304  test_subsampling_width = (abs((int)subsampl_width - (int)subsampling_width_tmp) <
305  (allowedPixelDiffOnImageSize * (int)(subsampling_width_tmp / subsampling_width)));
306  }
307  if (subsampling_height) {
308  test_subsampling_height = (abs((int)subsampl_height - (int)subsampling_height_tmp) <
309  (allowedPixelDiffOnImageSize * (int)(subsampling_height_tmp / subsampling_height)));
310  }
311  // if same name && same projection model && same image size camera already exists, we return SEQUENCE_OK
312  // otherwise it is a new camera that need to be updated and we return SEQUENCE_OK
313  bool same_name = (cam_name.empty() || (cam_name == camera_name_tmp));
314  bool same_img_size = (abs((int)im_width - (int)image_width_tmp) < allowedPixelDiffOnImageSize || im_width == 0) &&
315  (abs((int)im_height - (int)image_height_tmp) < allowedPixelDiffOnImageSize || im_height == 0) &&
316  (test_subsampling_width) && (test_subsampling_height);
317  if (same_name && same_img_size && same_proj_model) {
318  back = SEQUENCE_OK; // Camera exists
319  camera = cam_tmp;
320  camera_name = camera_name_tmp;
321  image_width = image_width_tmp;
322  image_height = image_height_tmp;
323  subsampling_width = subsampling_width_tmp;
324  subsampling_height = subsampling_height_tmp;
325  full_width = subsampling_width_tmp * image_width_tmp;
326  full_height = subsampling_height_tmp * image_height_tmp;
327  }
328  else {
329 
330  back = SEQUENCE_ERROR; // Camera doesn't exist yet in the file
331  }
332 #if 0
333  if (!((projModelFound == true) &&
334  (abs((int)im_width - (int)image_width_tmp) < allowedPixelDiffOnImageSize || im_width == 0) &&
335  (abs((int)im_height - (int)image_height_tmp) < allowedPixelDiffOnImageSize || im_height == 0) &&
336  (test_subsampling_width) && (test_subsampling_height))) {
337  // Same images size, we need to check if the camera have the same name
338  if (!cam_name.empty() && (cam_name != camera_name_tmp)) {
339  back = SEQUENCE_ERROR; // Camera doesn't exist yet in the file
340  }
341  else {
342  back = SEQUENCE_OK; // Camera already found
343  }
344  }
345  else {
346  camera = cam_tmp;
347  camera_name = camera_name_tmp;
348  image_width = image_width_tmp;
349  image_height = image_height_tmp;
350  subsampling_width = subsampling_width_tmp;
351  subsampling_height = subsampling_height_tmp;
352  full_width = subsampling_width_tmp * image_width_tmp;
353  full_height = subsampling_height_tmp * image_height_tmp;
354  back = SEQUENCE_ERROR; // Camera doesn't exist yet in the file
355  }
356 #endif
357  return back;
358  }
359 
366  vpXmlCodeSequenceType read_camera_model(const pugi::xml_node &node_, vpCameraParameters &cam_tmp)
367  {
368  // counter of the number of read parameters
369  int nb = 0;
370  vpXmlCodeType prop;
371  /* read value in the XML file. */
372 
373  std::string model_type = "";
374  double u0 = cam_tmp.get_u0();
375  double v0 = cam_tmp.get_v0();
376  double px = cam_tmp.get_px();
377  double py = cam_tmp.get_py();
378  double kud = cam_tmp.get_kud();
379  double kdu = cam_tmp.get_kdu();
380  std::vector<double> distortion_coeffs;
382  int validation = 0;
383 
384  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
385  // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
386  if (node.type() != pugi::node_element)
387  continue;
388 
389  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
390  prop = CODE_XML_OTHER;
391  back = SEQUENCE_ERROR;
392  }
393 
394  switch (prop) {
395  case CODE_XML_MODEL_TYPE: {
396  model_type = node.text().as_string();
397  nb++;
398  validation = validation | 0x01;
399  } break;
400  case CODE_XML_U0:
401  u0 = node.text().as_double();
402  nb++;
403  validation = validation | 0x02;
404  break;
405  case CODE_XML_V0:
406  v0 = node.text().as_double();
407  nb++;
408  validation = validation | 0x04;
409  break;
410  case CODE_XML_PX:
411  px = node.text().as_double();
412  nb++;
413  validation = validation | 0x08;
414  break;
415  case CODE_XML_PY:
416  py = node.text().as_double();
417  nb++;
418  validation = validation | 0x10;
419  break;
420  case CODE_XML_KUD:
421  kud = node.text().as_double();
422  nb++;
423  validation = validation | 0x20;
424  break;
425  case CODE_XML_KDU:
426  kdu = node.text().as_double();
427  nb++;
428  validation = validation | 0x40;
429  break;
430  case CODE_XML_K1:
431  distortion_coeffs.push_back(node.text().as_double());
432  nb++;
433  validation = validation | 0x20;
434  break;
435  case CODE_XML_K2:
436  distortion_coeffs.push_back(node.text().as_double());
437  nb++;
438  validation = validation | 0x40;
439  break;
440  case CODE_XML_K3:
441  distortion_coeffs.push_back(node.text().as_double());
442  nb++;
443  validation = validation | 0x80;
444  break;
445  case CODE_XML_K4:
446  distortion_coeffs.push_back(node.text().as_double());
447  nb++;
448  validation = validation | 0x100;
449  break;
450  case CODE_XML_K5:
451  distortion_coeffs.push_back(node.text().as_double());
452  nb++;
453  validation = validation | 0x200;
454  break;
455  case CODE_XML_BAD:
456  case CODE_XML_OTHER:
457  case CODE_XML_CAMERA:
458  case CODE_XML_CAMERA_NAME:
459  case CODE_XML_HEIGHT:
460  case CODE_XML_WIDTH:
461  case CODE_XML_SUBSAMPLING_WIDTH:
462  case CODE_XML_SUBSAMPLING_HEIGHT:
463  case CODE_XML_FULL_HEIGHT:
464  case CODE_XML_FULL_WIDTH:
465  case CODE_XML_MODEL:
466  case CODE_XML_ADDITIONAL_INFO:
467  default:
468  back = SEQUENCE_ERROR;
469  break;
470  }
471  }
472 
473  if (model_type.empty()) {
474  vpERROR_TRACE("projection model type doesn't match with any known model !");
475  return SEQUENCE_ERROR;
476  }
477 
478  if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITHOUT_DISTORTION)) {
479  if (nb != 5 || validation != 0x001F) {
480  vpCERROR << "ERROR in 'model' field:\n";
481  vpCERROR << "it must contain 5 parameters\n";
482 
483  return SEQUENCE_ERROR;
484  }
485  cam_tmp.initPersProjWithoutDistortion(px, py, u0, v0);
486  }
487  else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_DISTORTION)) {
488  if (nb != 7 || validation != 0x7F) {
489  vpCERROR << "ERROR in 'model' field:\n";
490  vpCERROR << "it must contain 7 parameters\n";
491 
492  return SEQUENCE_ERROR;
493  }
494  cam_tmp.initPersProjWithDistortion(px, py, u0, v0, kud, kdu);
495  }
496  else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION)) {
497  if (nb != 10 || validation != 0x3FF) { // at least one coefficient is missing. We should know which one
498  vpCERROR << "ERROR in 'model' field:\n";
499  vpCERROR << "it must contain 10 parameters\n";
500 
501  std::vector<double> fixed_distortion_coeffs;
502 
503  // In case disortion coefficients are missing, we should complete them with 0 values
504  // Since 0x3FF is 0011|1111|1111 and we are interrested in the most significant 1s shown below
505  // -- ---
506  // If we divide by 32 (>> 2^5 : 5 remaining least significant bits), we will have to check 5 bits only
507  int check = validation / 32;
508  int j = 0;
509 
510  for (int i = 0; i < 5; i++) {
511  int bit = check % 2; // if bit == 1 => the corresponding distortion coefficient is present.
512  if (!bit)
513  fixed_distortion_coeffs.push_back(0.);
514  else
515  fixed_distortion_coeffs.push_back(distortion_coeffs[j++]);
516  check /= 2;
517  }
518 
519  cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, fixed_distortion_coeffs);
520  return SEQUENCE_ERROR;
521  }
522  cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, distortion_coeffs);
523  }
524  else {
525  vpERROR_TRACE("projection model type doesn't match with any known model !");
526 
527  return SEQUENCE_ERROR;
528  }
529  return back;
530  }
531 
532  int save(const vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
533  unsigned int im_width, unsigned int im_height, const std::string &additionalInfo, bool verbose)
534  {
535  pugi::xml_document doc;
536  pugi::xml_node node;
537 
538  if (!doc.load_file(filename.c_str(), pugi::parse_default | pugi::parse_comments)) {
539  node = doc.append_child(pugi::node_declaration);
540  node.append_attribute("version") = "1.0";
541  node = doc.append_child(LABEL_XML_ROOT);
542  pugi::xml_node nodeComment = node.append_child(pugi::node_comment);
543  nodeComment.set_value("This file stores intrinsic camera parameters used\n"
544  " in the vpCameraParameters Class of ViSP available\n"
545  " at https://visp.inria.fr/download/ .\n"
546  " It can be read with the parse method of\n"
547  " the vpXmlParserCamera class.");
548  }
549 
550  node = doc.document_element();
551  if (!node) {
552  return SEQUENCE_ERROR;
553  }
554 
555  camera = cam;
556 
557  int nbCamera = count(node, cam_name, cam.get_projModel(), im_width, im_height, verbose);
558  if (nbCamera) {
559  return SEQUENCE_ERROR;
560  }
561 
562  pugi::xml_node nodeCamera = find_camera(node, cam_name, im_width, im_height);
563  if (!nodeCamera) {
564  write(node, cam_name, im_width, im_height);
565  }
566  else {
567  write_camera(nodeCamera);
568  }
569 
570  if (!additionalInfo.empty()) {
571  // Get camera node pointer
572  nodeCamera = find_camera(node, cam_name, im_width, im_height);
573 
574  // Additional information provided by the user
575  pugi::xml_node nodeAdditionalInfo = find_additional_info(nodeCamera);
576 
577  if (!nodeAdditionalInfo) {
578  // Create the additional information node
579  pugi::xml_node node_comment = nodeCamera.append_child(pugi::node_comment);
580  node_comment.set_value("Additional information");
581 
582  nodeAdditionalInfo = nodeCamera.append_child(LABEL_XML_ADDITIONAL_INFO);
583  }
584 
585  if (nodeAdditionalInfo) {
586  // Add the information in this specific node
587  pugi::xml_document tmpDoc;
588  if (tmpDoc.load_string(additionalInfo.c_str())) {
589  for (node = tmpDoc.first_child(); node; node = node.next_sibling()) {
590  nodeAdditionalInfo.append_copy(node);
591  }
592  }
593  }
594  }
595 
596  doc.save_file(filename.c_str(), PUGIXML_TEXT(" "));
597 
598  return SEQUENCE_OK;
599  }
600 
619  int count(const pugi::xml_node &node_, const std::string &cam_name,
620  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
621  unsigned int im_height, bool verbose, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
622  {
623  vpXmlCodeType prop;
624  int nbCamera = 0;
625 
626  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
627  if (node.type() != pugi::node_element)
628  continue;
629 
630  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
631  prop = CODE_XML_OTHER;
632  }
633 
634  if (prop == CODE_XML_CAMERA) {
635  if (SEQUENCE_OK == read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height, verbose))
636  nbCamera++;
637  }
638  }
639 
640  return nbCamera;
641  }
642 
659  pugi::xml_node find_camera(const pugi::xml_node &node_, const std::string &cam_name, unsigned int im_width,
660  unsigned int im_height, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
661  {
662  vpXmlCodeType prop;
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 
668  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
669  prop = CODE_XML_OTHER;
670  }
671  if (prop == CODE_XML_CAMERA) {
672  if (SEQUENCE_OK == read_camera_header(node, cam_name, im_width, im_height, subsampl_width, subsampl_height)) {
673  return node;
674  }
675  }
676  }
677  return pugi::xml_node();
678  }
679 
695  int read_camera_header(const pugi::xml_node &node_, const std::string &cam_name, unsigned int im_width,
696  unsigned int im_height, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
697  {
698  vpXmlCodeType prop;
699  /* read value in the XML file. */
700  std::string camera_name_tmp = "";
701  unsigned int image_height_tmp = 0;
702  unsigned int image_width_tmp = 0;
703  unsigned int subsampling_width_tmp = 0;
704  unsigned int subsampling_height_tmp = 0;
706 
707  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
708  if (node.type() != pugi::node_element)
709  continue;
710  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
711  prop = CODE_XML_OTHER;
712  back = SEQUENCE_ERROR;
713  }
714 
715  switch (prop) {
716  case CODE_XML_CAMERA_NAME:
717  camera_name_tmp = node.text().as_string();
718  break;
719 
720  case CODE_XML_WIDTH:
721  image_width_tmp = node.text().as_uint();
722  break;
723 
724  case CODE_XML_HEIGHT:
725  image_height_tmp = node.text().as_uint();
726  break;
727 
728  case CODE_XML_SUBSAMPLING_WIDTH:
729  subsampling_width_tmp = node.text().as_uint();
730  break;
731 
732  case CODE_XML_SUBSAMPLING_HEIGHT:
733  subsampling_height_tmp = node.text().as_uint();
734  break;
735 
736  case CODE_XML_MODEL:
737  break;
738 
739  case CODE_XML_ADDITIONAL_INFO:
740  break;
741 
742  case CODE_XML_BAD:
743  case CODE_XML_OTHER:
744  case CODE_XML_CAMERA:
745  case CODE_XML_FULL_HEIGHT:
746  case CODE_XML_FULL_WIDTH:
747  case CODE_XML_MODEL_TYPE:
748  case CODE_XML_U0:
749  case CODE_XML_V0:
750  case CODE_XML_PX:
751  case CODE_XML_PY:
752  case CODE_XML_KUD:
753  case CODE_XML_KDU:
754  default:
755  back = SEQUENCE_ERROR;
756  break;
757  }
758  }
759  if (!((cam_name == camera_name_tmp) && (im_width == image_width_tmp || im_width == 0) &&
760  (im_height == image_height_tmp || im_height == 0) &&
761  (subsampl_width == subsampling_width_tmp || subsampl_width == 0) &&
762  (subsampl_height == subsampling_height_tmp || subsampl_height == 0))) {
763  back = SEQUENCE_ERROR;
764  }
765  return back;
766  }
767 
783  int write(pugi::xml_node &node, const std::string &cam_name, unsigned int im_width, unsigned int im_height,
784  unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
785  {
786  int back = SEQUENCE_OK;
787 
788  // <camera>
789  pugi::xml_node node_camera = node.append_child(LABEL_XML_CAMERA);
790 
791  pugi::xml_node node_tmp;
792  {
793  //<name>
794  if (!cam_name.empty()) {
795  node_tmp = node_camera.append_child(pugi::node_comment);
796  node_tmp.set_value("Name of the camera");
797  node_tmp = node_camera.append_child(LABEL_XML_CAMERA_NAME);
798  node_tmp.append_child(pugi::node_pcdata).set_value(cam_name.c_str());
799  }
800 
801  if (im_width != 0 || im_height != 0) {
802  node_tmp = node_camera.append_child(pugi::node_comment);
803  node_tmp.set_value("Size of the image on which camera "
804  "calibration was performed");
805 
806  //<image_width>
807  node_tmp = node_camera.append_child(LABEL_XML_WIDTH);
808  node_tmp.append_child(pugi::node_pcdata).text() = im_width;
809 
810  //<image_height>
811  node_tmp = node_camera.append_child(LABEL_XML_HEIGHT);
812  node_tmp.append_child(pugi::node_pcdata).text() = im_height;
813  if (subsampling_width != 0 || subsampling_height != 0) {
814  node_tmp = node_camera.append_child(pugi::node_comment);
815  node_tmp.set_value("Subsampling used to obtain the "
816  "current size of the image.");
817 
818  //<subsampling_width>
819  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_WIDTH);
820  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_width;
821  //<subsampling_height>
822  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_HEIGHT);
823  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_height;
824  node_tmp = node_camera.append_child(pugi::node_comment);
825  node_tmp.set_value("The full size is the sensor size actually used to "
826  "grab the image. full_width = subsampling_width * "
827  "image_width");
828 
829  //<full_width>
830  node_tmp = node_camera.append_child(LABEL_XML_FULL_WIDTH);
831  node_tmp.append_child(pugi::node_pcdata).text() = im_width * subsampl_width;
832  //<full_height>
833  node_tmp = node_camera.append_child(LABEL_XML_FULL_HEIGHT);
834  node_tmp.append_child(pugi::node_pcdata).text() = im_height * subsampl_height;
835  }
836  }
837 
838  node_tmp = node_camera.append_child(pugi::node_comment);
839  node_tmp.set_value("Intrinsic camera parameters "
840  "computed for each projection model");
841 
842  back = write_camera(node_camera);
843  }
844  return back;
845  }
846 
852  int write_camera(pugi::xml_node &node_camera)
853  {
854  pugi::xml_node node_model;
855  pugi::xml_node node_tmp;
856 
857  int back = SEQUENCE_OK;
858 
859  switch (camera.get_projModel()) {
861  //<model>
862  node_model = node_camera.append_child(LABEL_XML_MODEL);
863  {
864  node_tmp = node_model.append_child(pugi::node_comment);
865  node_tmp.set_value("Projection model type");
866 
867  //<type>without_distortion</type>
868  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
869  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITHOUT_DISTORTION);
870 
871  node_tmp = node_model.append_child(pugi::node_comment);
872  node_tmp.set_value("Pixel ratio");
873  //<px>
874  node_tmp = node_model.append_child(LABEL_XML_PX);
875  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
876  //<py>
877  node_tmp = node_model.append_child(LABEL_XML_PY);
878  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
879 
880  node_tmp = node_model.append_child(pugi::node_comment);
881  node_tmp.set_value("Principal point");
882 
883  //<u0>
884  node_tmp = node_model.append_child(LABEL_XML_U0);
885  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
886  //<v0>
887  node_tmp = node_model.append_child(LABEL_XML_V0);
888  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
889  }
890  break;
891 
893  //<model>
894  node_model = node_camera.append_child(LABEL_XML_MODEL);
895  {
896  node_tmp = node_model.append_child(pugi::node_comment);
897  node_tmp.set_value("Projection model type");
898  //<type>with_distortion</type>
899  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
900  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_DISTORTION);
901 
902  node_tmp = node_model.append_child(pugi::node_comment);
903  node_tmp.set_value("Pixel ratio");
904  //<px>
905  node_tmp = node_model.append_child(LABEL_XML_PX);
906  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
907  //<py>
908  node_tmp = node_model.append_child(LABEL_XML_PY);
909  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
910 
911  node_tmp = node_model.append_child(pugi::node_comment);
912  node_tmp.set_value("Principal point");
913  //<u0>
914  node_tmp = node_model.append_child(LABEL_XML_U0);
915  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
916  //<v0>
917  node_tmp = node_model.append_child(LABEL_XML_V0);
918  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
919 
920  //<kud>
921  node_tmp = node_model.append_child(pugi::node_comment);
922  node_tmp.set_value("Undistorted to distorted distortion parameter");
923  node_tmp = node_model.append_child(LABEL_XML_KUD);
924  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kud();
925 
926  //<kud>
927  node_tmp = node_model.append_child(pugi::node_comment);
928  node_tmp.set_value("Distorted to undistorted distortion parameter");
929  node_tmp = node_model.append_child(LABEL_XML_KDU);
930  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kdu();
931  }
932  break;
933 
935  //<model>
936  node_model = node_camera.append_child(LABEL_XML_MODEL);
937  {
938  node_tmp = node_model.append_child(pugi::node_comment);
939  node_tmp.set_value("Projection model type");
940  //<type>with_KannalaBrandt_distortion</type>
941  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
942  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION);
943 
944  node_tmp = node_model.append_child(pugi::node_comment);
945  node_tmp.set_value("Pixel ratio");
946  //<px>
947  node_tmp = node_model.append_child(LABEL_XML_PX);
948  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
949  //<py>
950  node_tmp = node_model.append_child(LABEL_XML_PY);
951  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
952 
953  node_tmp = node_model.append_child(pugi::node_comment);
954  node_tmp.set_value("Principal point");
955  //<u0>
956  node_tmp = node_model.append_child(LABEL_XML_U0);
957  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
958  //<v0>
959  node_tmp = node_model.append_child(LABEL_XML_V0);
960  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
961 
962  //<k1>, <k2>, <k3>, <k4>, <k5>
963  std::vector<double> distortion_coefs = camera.getKannalaBrandtDistortionCoefficients();
964 
965  if (distortion_coefs.size() != 5)
966  std::cout << "Make sure to have 5 distortion coefficients for Kannala-Brandt distortions." << std::endl;
967 
968  node_tmp = node_model.append_child(pugi::node_comment);
969  node_tmp.set_value("Distortion coefficients");
970  node_tmp = node_model.append_child(LABEL_XML_K1);
971  distortion_coefs.size() == 0 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
972  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[0];
973  node_tmp = node_model.append_child(LABEL_XML_K2);
974  distortion_coefs.size() <= 1 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
975  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[1];
976  node_tmp = node_model.append_child(LABEL_XML_K3);
977  distortion_coefs.size() <= 2 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
978  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[2];
979  node_tmp = node_model.append_child(LABEL_XML_K4);
980  distortion_coefs.size() <= 3 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
981  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[3];
982  node_tmp = node_model.append_child(LABEL_XML_K5);
983  distortion_coefs.size() <= 4 ? node_tmp.append_child(pugi::node_pcdata).text() = 0
984  : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[4];
985  }
986  break;
987  }
988  return back;
989  }
990 
997  pugi::xml_node find_additional_info(const pugi::xml_node &node_)
998  {
999  vpXmlCodeType prop;
1000 
1001  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
1002  if (node.type() != pugi::node_element) {
1003  continue;
1004  }
1005 
1006  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
1007  prop = CODE_XML_OTHER;
1008  }
1009 
1010  if (prop == CODE_XML_ADDITIONAL_INFO) {
1011  // We found the node
1012  return node;
1013  }
1014  }
1015 
1016  return pugi::xml_node();
1017  }
1018 
1025  vpXmlCodeSequenceType str2xmlcode(const char *str, vpXmlCodeType &res)
1026  {
1027  vpXmlCodeType val_int = CODE_XML_BAD;
1029 
1030  if (!strcmp(str, LABEL_XML_CAMERA)) {
1031  val_int = CODE_XML_CAMERA;
1032  }
1033  else if (!strcmp(str, LABEL_XML_CAMERA_NAME)) {
1034  val_int = CODE_XML_CAMERA_NAME;
1035  }
1036  else if (!strcmp(str, LABEL_XML_MODEL)) {
1037  val_int = CODE_XML_MODEL;
1038  }
1039  else if (!strcmp(str, LABEL_XML_MODEL_TYPE)) {
1040  val_int = CODE_XML_MODEL_TYPE;
1041  }
1042  else if (!strcmp(str, LABEL_XML_WIDTH)) {
1043  val_int = CODE_XML_WIDTH;
1044  }
1045  else if (!strcmp(str, LABEL_XML_HEIGHT)) {
1046  val_int = CODE_XML_HEIGHT;
1047  }
1048  else if (!strcmp(str, LABEL_XML_SUBSAMPLING_WIDTH)) {
1049  val_int = CODE_XML_SUBSAMPLING_WIDTH;
1050  }
1051  else if (!strcmp(str, LABEL_XML_SUBSAMPLING_HEIGHT)) {
1052  val_int = CODE_XML_SUBSAMPLING_HEIGHT;
1053  }
1054  else if (!strcmp(str, LABEL_XML_FULL_WIDTH)) {
1055  val_int = CODE_XML_FULL_WIDTH;
1056  }
1057  else if (!strcmp(str, LABEL_XML_FULL_HEIGHT)) {
1058  val_int = CODE_XML_FULL_HEIGHT;
1059  }
1060  else if (!strcmp(str, LABEL_XML_U0)) {
1061  val_int = CODE_XML_U0;
1062  }
1063  else if (!strcmp(str, LABEL_XML_V0)) {
1064  val_int = CODE_XML_V0;
1065  }
1066  else if (!strcmp(str, LABEL_XML_PX)) {
1067  val_int = CODE_XML_PX;
1068  }
1069  else if (!strcmp(str, LABEL_XML_PY)) {
1070  val_int = CODE_XML_PY;
1071  }
1072  else if (!strcmp(str, LABEL_XML_KUD)) {
1073  val_int = CODE_XML_KUD;
1074  }
1075  else if (!strcmp(str, LABEL_XML_KDU)) {
1076  val_int = CODE_XML_KDU;
1077  }
1078  else if (!strcmp(str, LABEL_XML_K1)) {
1079  val_int = CODE_XML_K1;
1080  }
1081  else if (!strcmp(str, LABEL_XML_K2)) {
1082  val_int = CODE_XML_K2;
1083  }
1084  else if (!strcmp(str, LABEL_XML_K3)) {
1085  val_int = CODE_XML_K3;
1086  }
1087  else if (!strcmp(str, LABEL_XML_K4)) {
1088  val_int = CODE_XML_K4;
1089  }
1090  else if (!strcmp(str, LABEL_XML_K5)) {
1091  val_int = CODE_XML_K5;
1092  }
1093  else if (!strcmp(str, LABEL_XML_ADDITIONAL_INFO)) {
1094  val_int = CODE_XML_ADDITIONAL_INFO;
1095  }
1096  else {
1097  val_int = CODE_XML_OTHER;
1098  }
1099  res = val_int;
1100 
1101  return back;
1102  }
1103 
1104  std::string getCameraName() const { return camera_name; }
1105  vpCameraParameters getCameraParameters() const { return camera; }
1106  unsigned int getHeight() const { return image_height; }
1107  unsigned int getSubsampling_width() const { return subsampling_width; }
1108  unsigned int getSubsampling_height() const { return subsampling_height; }
1109  unsigned int getWidth() const { return image_width; }
1110 
1111  void setCameraName(const std::string &name) { camera_name = name; }
1112  void setHeight(unsigned int height) { image_height = height; }
1113  void setSubsampling_width(unsigned int subsampling) { subsampling_width = subsampling; }
1114  void setSubsampling_height(unsigned int subsampling) { subsampling_height = subsampling; }
1115  void setWidth(unsigned int width) { image_width = width; }
1116 
1117 private:
1118  vpCameraParameters camera;
1119  std::string camera_name;
1120  unsigned int image_width;
1121  unsigned int image_height;
1122  unsigned int subsampling_width;
1123  unsigned int subsampling_height;
1124  unsigned int full_width;
1125  unsigned int full_height;
1126 
1129  static const int allowedPixelDiffOnImageSize = 15;
1130 };
1131 #endif // DOXYGEN_SHOULD_SKIP_THIS
1132 
1133 vpXmlParserCamera::vpXmlParserCamera() : m_impl(new Impl()) { }
1134 
1136 
1152 int vpXmlParserCamera::parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1153  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
1154  unsigned int im_height, bool verbose)
1155 {
1156  return m_impl->parse(cam, filename, cam_name, projModel, im_width, im_height, verbose);
1157 }
1158 
1203 int vpXmlParserCamera::save(const vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1204  unsigned int im_width, unsigned int im_height, const std::string &additionalInfo, bool verbose)
1205 {
1206  return m_impl->save(cam, filename, cam_name, im_width, im_height, additionalInfo, verbose);
1207 }
1208 
1209 std::string vpXmlParserCamera::getCameraName() const { return m_impl->getCameraName(); }
1210 
1211 vpCameraParameters vpXmlParserCamera::getCameraParameters() const { return m_impl->getCameraParameters(); }
1212 
1213 unsigned int vpXmlParserCamera::getHeight() const { return m_impl->getHeight(); }
1214 
1215 unsigned int vpXmlParserCamera::getSubsampling_width() const { return m_impl->getSubsampling_width(); }
1216 
1217 unsigned int vpXmlParserCamera::getSubsampling_height() const { return m_impl->getSubsampling_height(); }
1218 
1219 unsigned int vpXmlParserCamera::getWidth() const { return m_impl->getWidth(); }
1220 
1221 void vpXmlParserCamera::setCameraName(const std::string &name) { m_impl->setCameraName(name); }
1222 
1223 void vpXmlParserCamera::setHeight(unsigned int height) { m_impl->setHeight(height); }
1224 
1225 void vpXmlParserCamera::setSubsampling_width(unsigned int subsampling) { m_impl->setSubsampling_width(subsampling); }
1226 
1227 void vpXmlParserCamera::setSubsampling_height(unsigned int subsampling) { m_impl->setSubsampling_height(subsampling); }
1228 
1229 void vpXmlParserCamera::setWidth(unsigned int width) { m_impl->setWidth(width); }
1230 
1231 #elif !defined(VISP_BUILD_SHARED_LIBS)
1232 // Work around to avoid warning: libvisp_core.a(vpXmlParserCamera.cpp.o) has no symbols
1233 void dummy_vpXmlParserCamera() { };
1234 
1235 #endif
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
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
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="", bool verbose=true)
vpCameraParameters getCameraParameters() const
unsigned int getWidth() const
void setSubsampling_height(unsigned int subsampling)
void setCameraName(const std::string &name)
void setHeight(unsigned int height)
unsigned int getSubsampling_height() const
unsigned int getSubsampling_width() 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, bool verbose=true)
std::string getCameraName() const
#define vpCERROR
Definition: vpDebug.h:356
#define vpERROR_TRACE
Definition: vpDebug.h:382