Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
vpXmlParserCamera.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * XML parser to load and save camera intrinsic parameters.
32  *
33 *****************************************************************************/
34 
41 #include <visp3/core/vpXmlParserCamera.h>
42 
43 #if defined(VISP_HAVE_PUGIXML)
44 #include <pugixml.hpp>
45 
46 /* --------------------------------------------------------------------------
47  */
48  /* --- LABEL XML ------------------------------------------------------------
49  */
50  /* --------------------------------------------------------------------------
51  */
52 
53 #define LABEL_XML_ROOT "root"
54 #define LABEL_XML_CAMERA "camera"
55 #define LABEL_XML_CAMERA_NAME "name"
56 #define LABEL_XML_WIDTH "image_width"
57 #define LABEL_XML_HEIGHT "image_height"
58 #define LABEL_XML_SUBSAMPLING_WIDTH "subsampling_width"
59 #define LABEL_XML_SUBSAMPLING_HEIGHT "subsampling_height"
60 #define LABEL_XML_FULL_WIDTH "full_width"
61 #define LABEL_XML_FULL_HEIGHT "full_height"
62 #define LABEL_XML_MODEL "model"
63 #define LABEL_XML_MODEL_TYPE "type"
64 #define LABEL_XML_U0 "u0"
65 #define LABEL_XML_V0 "v0"
66 #define LABEL_XML_PX "px"
67 #define LABEL_XML_PY "py"
68 #define LABEL_XML_KUD "kud"
69 #define LABEL_XML_KDU "kdu"
70 #define LABEL_XML_K1 "k1"
71 #define LABEL_XML_K2 "k2"
72 #define LABEL_XML_K3 "k3"
73 #define LABEL_XML_K4 "k4"
74 #define LABEL_XML_K5 "k5"
75 
76 #define LABEL_XML_MODEL_WITHOUT_DISTORTION "perspectiveProjWithoutDistortion"
77 #define LABEL_XML_MODEL_WITH_DISTORTION "perspectiveProjWithDistortion"
78 #define LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION "ProjWithKannalaBrandtDistortion"
79 
80 #define LABEL_XML_ADDITIONAL_INFO "additional_information"
81 
82 BEGIN_VISP_NAMESPACE
83 #ifndef DOXYGEN_SHOULD_SKIP_THIS
84 class vpXmlParserCamera::Impl
85 {
86 public: /* --- XML Code------------------------------------------------------------
87  */
88  enum vpXmlCodeType
89  {
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  Impl()
117  : camera(), camera_name(), image_width(0), image_height(0), subsampling_width(0), subsampling_height(0),
118  full_width(0), full_height(0)
119  { }
120 
121  int parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
122  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
123  unsigned int im_height, bool verbose)
124  {
125  pugi::xml_document doc;
126  if (!doc.load_file(filename.c_str())) {
127  return SEQUENCE_ERROR;
128  }
129 
130  pugi::xml_node node = doc.document_element();
131  if (!node) {
132  return SEQUENCE_ERROR;
133  }
134 
135  int ret = read(node, cam_name, projModel, im_width, im_height, verbose);
136 
137  cam = camera;
138 
139  return ret;
140  }
141 
159  int read(const pugi::xml_node &node_, const std::string &cam_name,
160  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
161  unsigned int im_height, bool verbose, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
162  {
163  vpXmlCodeType prop;
165  unsigned int nbCamera = 0;
166 
167  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
168  if (node.type() == pugi::node_element) {
169  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
170  prop = CODE_XML_OTHER;
171  back = SEQUENCE_ERROR;
172  }
173  if (prop == CODE_XML_CAMERA) {
174  if (SEQUENCE_OK == read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height, verbose)) {
175  ++nbCamera;
176  }
177  }
178  else {
179  back = SEQUENCE_ERROR;
180  }
181  }
182  }
183 
184  if (nbCamera == 0) {
185  back = SEQUENCE_ERROR;
186  std::cout << "Warning: No camera parameters is available" << std::endl << "with your specifications" << std::endl;
187  }
188  else if (nbCamera > 1) {
189  back = SEQUENCE_ERROR;
190  std::cout << "Warning: " << nbCamera << " sets of camera parameters are available" << std::endl
191  << "with your specifications : " << std::endl
192  << "precise your choice..." << std::endl;
193  }
194 
195  return back;
196  }
197 
215  int read_camera(const pugi::xml_node &node_, const std::string &cam_name,
216  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
217  unsigned int im_height, unsigned int subsampl_width, unsigned int subsampl_height, bool verbose)
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 same_proj_model = false;
230 
231  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
232  if (node.type() == pugi::node_element) {
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  if (verbose) {
242  std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl;
243  }
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  same_proj_model = true; // Same projection model
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 
291  break;
292  }
293  }
294  }
295  // Create a specific test for subsampling_width and subsampling_height to
296  // ensure that division by zero is not possible in the next test
297  bool test_subsampling_width = true;
298  bool test_subsampling_height = true;
299 
300  if (subsampling_width) {
301  test_subsampling_width = (abs(static_cast<int>(subsampl_width) - static_cast<int>(subsampling_width_tmp)) <
302  (allowedPixelDiffOnImageSize * static_cast<int>(subsampling_width_tmp / subsampling_width)));
303  }
304  if (subsampling_height) {
305  test_subsampling_height = (abs(static_cast<int>(subsampl_height) - static_cast<int>(subsampling_height_tmp)) <
306  (allowedPixelDiffOnImageSize * static_cast<int>(subsampling_height_tmp / subsampling_height)));
307  }
308  // if same name && same projection model && same image size camera already exists, we return SEQUENCE_OK
309  // otherwise it is a new camera that need to be updated and we return SEQUENCE_OK
310  bool same_name = (cam_name.empty() || (cam_name == camera_name_tmp));
311  bool imWidthOk = (abs(static_cast<int>(im_width) - static_cast<int>(image_width_tmp)) < allowedPixelDiffOnImageSize) || (im_width == 0);
312  bool imHeightOk = (abs(static_cast<int>(im_height) - static_cast<int>(image_height_tmp)) < allowedPixelDiffOnImageSize) || (im_height == 0);
313  bool imSizeOk = imWidthOk && imHeightOk;
314  bool same_img_size = imSizeOk && test_subsampling_width && test_subsampling_height;
315  if (same_name && same_img_size && same_proj_model) {
316  back = SEQUENCE_OK; // Camera exists
317  camera = cam_tmp;
318  camera_name = camera_name_tmp;
319  image_width = image_width_tmp;
320  image_height = image_height_tmp;
321  subsampling_width = subsampling_width_tmp;
322  subsampling_height = subsampling_height_tmp;
323  full_width = subsampling_width_tmp * image_width_tmp;
324  full_height = subsampling_height_tmp * image_height_tmp;
325  }
326  else {
327 
328  back = SEQUENCE_ERROR; // Camera doesn't exist yet in the file
329  }
330 #if 0
331  if (!((projModelFound == true) &&
332  (abs(static_cast<int>(im_width) - static_cast<int>(image_width_tmp)) < allowedPixelDiffOnImageSize || im_width == 0) &&
333  (abs(static_cast<int>(im_height) - static_cast<int>(image_height_tmp)) < allowedPixelDiffOnImageSize || im_height == 0) &&
334  (test_subsampling_width) && (test_subsampling_height))) {
335  // Same images size, we need to check if the camera have the same name
336  if (!cam_name.empty() && (cam_name != camera_name_tmp)) {
337  back = SEQUENCE_ERROR; // Camera doesn't exist yet in the file
338  }
339  else {
340  back = SEQUENCE_OK; // Camera already found
341  }
342  }
343  else {
344  camera = cam_tmp;
345  camera_name = camera_name_tmp;
346  image_width = image_width_tmp;
347  image_height = image_height_tmp;
348  subsampling_width = subsampling_width_tmp;
349  subsampling_height = subsampling_height_tmp;
350  full_width = subsampling_width_tmp * image_width_tmp;
351  full_height = subsampling_height_tmp * image_height_tmp;
352  back = SEQUENCE_ERROR; // Camera doesn't exist yet in the file
353  }
354 #endif
355  return back;
356  }
357 
364  vpXmlCodeSequenceType read_camera_model(const pugi::xml_node &node_, vpCameraParameters &cam_tmp)
365  {
366  // counter of the number of read parameters
367  int nb = 0;
368  vpXmlCodeType prop;
369  /* read value in the XML file. */
370 
371  std::string model_type = "";
372  double u0 = cam_tmp.get_u0();
373  double v0 = cam_tmp.get_v0();
374  double px = cam_tmp.get_px();
375  double py = cam_tmp.get_py();
376  double kud = cam_tmp.get_kud();
377  double kdu = cam_tmp.get_kdu();
378  std::vector<double> distortion_coeffs;
380  unsigned int validation = 0;
381 
382  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
383  if (node.type() == pugi::node_element) {
384  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
385  prop = CODE_XML_OTHER;
386  back = SEQUENCE_ERROR;
387  }
388 
389  switch (prop) {
390  case CODE_XML_MODEL_TYPE: {
391  model_type = node.text().as_string();
392  ++nb;
393  validation = validation | 0x01;
394  } break;
395  case CODE_XML_U0:
396  u0 = node.text().as_double();
397  ++nb;
398  validation = validation | 0x02;
399  break;
400  case CODE_XML_V0:
401  v0 = node.text().as_double();
402  ++nb;
403  validation = validation | 0x04;
404  break;
405  case CODE_XML_PX:
406  px = node.text().as_double();
407  ++nb;
408  validation = validation | 0x08;
409  break;
410  case CODE_XML_PY:
411  py = node.text().as_double();
412  ++nb;
413  validation = validation | 0x10;
414  break;
415  case CODE_XML_KUD:
416  kud = node.text().as_double();
417  ++nb;
418  validation = validation | 0x20;
419  break;
420  case CODE_XML_KDU:
421  kdu = node.text().as_double();
422  ++nb;
423  validation = validation | 0x40;
424  break;
425  case CODE_XML_K1:
426  distortion_coeffs.push_back(node.text().as_double());
427  ++nb;
428  validation = validation | 0x20;
429  break;
430  case CODE_XML_K2:
431  distortion_coeffs.push_back(node.text().as_double());
432  ++nb;
433  validation = validation | 0x40;
434  break;
435  case CODE_XML_K3:
436  distortion_coeffs.push_back(node.text().as_double());
437  ++nb;
438  validation = validation | 0x80;
439  break;
440  case CODE_XML_K4:
441  distortion_coeffs.push_back(node.text().as_double());
442  ++nb;
443  validation = validation | 0x100;
444  break;
445  case CODE_XML_K5:
446  distortion_coeffs.push_back(node.text().as_double());
447  ++nb;
448  validation = validation | 0x200;
449  break;
450  case CODE_XML_BAD:
451  case CODE_XML_OTHER:
452  case CODE_XML_CAMERA:
453  case CODE_XML_CAMERA_NAME:
454  case CODE_XML_HEIGHT:
455  case CODE_XML_WIDTH:
456  case CODE_XML_SUBSAMPLING_WIDTH:
457  case CODE_XML_SUBSAMPLING_HEIGHT:
458  case CODE_XML_FULL_HEIGHT:
459  case CODE_XML_FULL_WIDTH:
460  case CODE_XML_MODEL:
461  case CODE_XML_ADDITIONAL_INFO:
462  default:
463  back = SEQUENCE_ERROR;
464  break;
465  }
466  }
467  }
468 
469  if (model_type.empty()) {
470  std::cout << "Warning: projection model type doesn't match with any known model !" << std::endl;
471  return SEQUENCE_ERROR;
472  }
473 
474  if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITHOUT_DISTORTION)) {
475  if (nb != 5 || validation != 0x001F) {
476  std::cout << "ERROR in 'model' field:\n";
477  std::cout << "it must contain 5 parameters\n";
478 
479  return SEQUENCE_ERROR;
480  }
481  cam_tmp.initPersProjWithoutDistortion(px, py, u0, v0);
482  }
483  else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_DISTORTION)) {
484  if (nb != 7 || validation != 0x7F) {
485  std::cout << "ERROR in 'model' field:\n";
486  std::cout << "it must contain 7 parameters\n";
487 
488  return SEQUENCE_ERROR;
489  }
490  cam_tmp.initPersProjWithDistortion(px, py, u0, v0, kud, kdu);
491  }
492  else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION)) {
493  if (nb != 10 || validation != 0x3FF) { // at least one coefficient is missing. We should know which one
494  std::cout << "ERROR in 'model' field:\n";
495  std::cout << "it must contain 10 parameters\n";
496 
497  std::vector<double> fixed_distortion_coeffs;
498 
499  // In case distortion coefficients are missing, we should complete them with 0 values
500  // Since 0x3FF is 0011|1111|1111 and we are interested in the most significant 1s shown below
501  // -- ---
502  // If we divide by 32 (>> 2^5 : 5 remaining least significant bits), we will have to check 5 bits only
503  const int dividerForBitCheck = 32;
504  int check = validation / dividerForBitCheck;
505  int j = 0;
506 
507  const int nbRemainingBits = 5;
508  const int moduloForOddity = 2;
509  const int dividerForRightShift = 2;
510  for (int i = 0; i < nbRemainingBits; ++i) {
511  int bit = check % moduloForOddity; // if bit == 1 => the corresponding distortion coefficient is present.
512  if (!bit) {
513  fixed_distortion_coeffs.push_back(0.);
514  }
515  else {
516  fixed_distortion_coeffs.push_back(distortion_coeffs[j++]);
517  }
518  check /= dividerForRightShift;
519  }
520 
521  cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, fixed_distortion_coeffs);
522  return SEQUENCE_ERROR;
523  }
524  cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, distortion_coeffs);
525  }
526  else {
527  std::cout << "Warning: projection model type doesn't match with any known model !" << std::endl;
528 
529  return SEQUENCE_ERROR;
530  }
531  return back;
532  }
533 
534  int save(const vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
535  unsigned int im_width, unsigned int im_height, const std::string &additionalInfo, bool verbose)
536  {
537  pugi::xml_document doc;
538  pugi::xml_node node;
539 
540  if (!doc.load_file(filename.c_str(), pugi::parse_default | pugi::parse_comments)) {
541  node = doc.append_child(pugi::node_declaration);
542  node.append_attribute("version") = "1.0";
543  node = doc.append_child(LABEL_XML_ROOT);
544  pugi::xml_node nodeComment = node.append_child(pugi::node_comment);
545  nodeComment.set_value("This file stores intrinsic camera parameters used\n"
546  " in the vpCameraParameters Class of ViSP available\n"
547  " at https://visp.inria.fr/download/ .\n"
548  " It can be read with the parse method of\n"
549  " the vpXmlParserCamera class.");
550  }
551 
552  node = doc.document_element();
553  if (!node) {
554  return SEQUENCE_ERROR;
555  }
556 
557  camera = cam;
558 
559  int nbCamera = count(node, cam_name, cam.get_projModel(), im_width, im_height, verbose);
560  if (nbCamera) {
561  return SEQUENCE_ERROR;
562  }
563 
564  pugi::xml_node nodeCamera = find_camera(node, cam_name, im_width, im_height);
565  if (!nodeCamera) {
566  write(node, cam_name, im_width, im_height);
567  }
568  else {
569  write_camera(nodeCamera);
570  }
571 
572  if (!additionalInfo.empty()) {
573  // Get camera node pointer
574  nodeCamera = find_camera(node, cam_name, im_width, im_height);
575 
576  // Additional information provided by the user
577  pugi::xml_node nodeAdditionalInfo = find_additional_info(nodeCamera);
578 
579  if (!nodeAdditionalInfo) {
580  // Create the additional information node
581  pugi::xml_node node_comment = nodeCamera.append_child(pugi::node_comment);
582  node_comment.set_value("Additional information");
583 
584  nodeAdditionalInfo = nodeCamera.append_child(LABEL_XML_ADDITIONAL_INFO);
585  }
586 
587  if (nodeAdditionalInfo) {
588  // Add the information in this specific node
589  pugi::xml_document tmpDoc;
590  if (tmpDoc.load_string(additionalInfo.c_str())) {
591  for (node = tmpDoc.first_child(); node; node = node.next_sibling()) {
592  nodeAdditionalInfo.append_copy(node);
593  }
594  }
595  }
596  }
597 
598  doc.save_file(filename.c_str(), PUGIXML_TEXT(" "));
599 
600  return SEQUENCE_OK;
601  }
602 
621  int count(const pugi::xml_node &node_, const std::string &cam_name,
622  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
623  unsigned int im_height, bool verbose, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
624  {
625  vpXmlCodeType prop;
626  int nbCamera = 0;
627 
628  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
629  if (node.type() == pugi::node_element) {
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  }
641 
642  return nbCamera;
643  }
644 
661  pugi::xml_node find_camera(const pugi::xml_node &node_, const std::string &cam_name, unsigned int im_width,
662  unsigned int im_height, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
663  {
664  vpXmlCodeType prop;
665  pugi::xml_node resNode = pugi::xml_node();
666 
667  pugi::xml_node node = node_.first_child();
668  bool hasNotFoundCam = true;
669  while (node && hasNotFoundCam) {
670  if (node.type() == pugi::node_element) {
671  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
672  prop = CODE_XML_OTHER;
673  }
674  if (prop == CODE_XML_CAMERA) {
675  if (SEQUENCE_OK == read_camera_header(node, cam_name, im_width, im_height, subsampl_width, subsampl_height)) {
676  resNode = node;
677  hasNotFoundCam = false;
678  }
679  }
680  }
681  node = node.next_sibling();
682  }
683  return resNode;
684  }
685 
701  int read_camera_header(const pugi::xml_node &node_, const std::string &cam_name, unsigned int im_width,
702  unsigned int im_height, unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
703  {
704  vpXmlCodeType prop;
705  /* read value in the XML file. */
706  std::string camera_name_tmp = "";
707  unsigned int image_height_tmp = 0;
708  unsigned int image_width_tmp = 0;
709  unsigned int subsampling_width_tmp = 0;
710  unsigned int subsampling_height_tmp = 0;
712 
713  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
714  if (node.type() == pugi::node_element) {
715  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
716  prop = CODE_XML_OTHER;
717  back = SEQUENCE_ERROR;
718  }
719 
720  switch (prop) {
721  case CODE_XML_CAMERA_NAME:
722  camera_name_tmp = node.text().as_string();
723  break;
724 
725  case CODE_XML_WIDTH:
726  image_width_tmp = node.text().as_uint();
727  break;
728 
729  case CODE_XML_HEIGHT:
730  image_height_tmp = node.text().as_uint();
731  break;
732 
733  case CODE_XML_SUBSAMPLING_WIDTH:
734  subsampling_width_tmp = node.text().as_uint();
735  break;
736 
737  case CODE_XML_SUBSAMPLING_HEIGHT:
738  subsampling_height_tmp = node.text().as_uint();
739  break;
740 
741  case CODE_XML_MODEL:
742  break;
743 
744  case CODE_XML_ADDITIONAL_INFO:
745  break;
746 
747  case CODE_XML_BAD:
748  case CODE_XML_OTHER:
749  case CODE_XML_CAMERA:
750  case CODE_XML_FULL_HEIGHT:
751  case CODE_XML_FULL_WIDTH:
752  case CODE_XML_MODEL_TYPE:
753  case CODE_XML_U0:
754  case CODE_XML_V0:
755  case CODE_XML_PX:
756  case CODE_XML_PY:
757  case CODE_XML_KUD:
758  case CODE_XML_KDU:
759  default:
760  back = SEQUENCE_ERROR;
761  break;
762  }
763  }
764  }
765  bool imHeightOK = (im_height == image_height_tmp) || (im_height == 0);
766  bool imWidthOK = (im_width == image_width_tmp) || (im_width == 0);
767  bool imSizeEqual = imHeightOK && imWidthOK;
768  bool subsampleHeightOK = (subsampl_height == subsampling_height_tmp) || (subsampl_height == 0);
769  bool subsampleWidthOK = (subsampl_width == subsampling_width_tmp) || (subsampl_width == 0);
770  bool subsampleOK = subsampleHeightOK && subsampleWidthOK;
771  if (!((cam_name == camera_name_tmp) && imSizeEqual && subsampleOK)) {
772  back = SEQUENCE_ERROR;
773  }
774  return back;
775  }
776 
792  int write(pugi::xml_node &node, const std::string &cam_name, unsigned int im_width, unsigned int im_height,
793  unsigned int subsampl_width = 0, unsigned int subsampl_height = 0)
794  {
795  int back = SEQUENCE_OK;
796 
797  // <camera>
798  pugi::xml_node node_camera = node.append_child(LABEL_XML_CAMERA);
799 
800  pugi::xml_node node_tmp;
801  {
802  //<name>
803  if (!cam_name.empty()) {
804  node_tmp = node_camera.append_child(pugi::node_comment);
805  node_tmp.set_value("Name of the camera");
806  node_tmp = node_camera.append_child(LABEL_XML_CAMERA_NAME);
807  node_tmp.append_child(pugi::node_pcdata).set_value(cam_name.c_str());
808  }
809 
810  if (im_width != 0 || im_height != 0) {
811  node_tmp = node_camera.append_child(pugi::node_comment);
812  node_tmp.set_value("Size of the image on which camera "
813  "calibration was performed");
814 
815  //<image_width>
816  node_tmp = node_camera.append_child(LABEL_XML_WIDTH);
817  node_tmp.append_child(pugi::node_pcdata).text() = im_width;
818 
819  //<image_height>
820  node_tmp = node_camera.append_child(LABEL_XML_HEIGHT);
821  node_tmp.append_child(pugi::node_pcdata).text() = im_height;
822  if (subsampling_width != 0 || subsampling_height != 0) {
823  node_tmp = node_camera.append_child(pugi::node_comment);
824  node_tmp.set_value("Subsampling used to obtain the "
825  "current size of the image.");
826 
827  //<subsampling_width>
828  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_WIDTH);
829  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_width;
830  //<subsampling_height>
831  node_tmp = node_camera.append_child(LABEL_XML_SUBSAMPLING_HEIGHT);
832  node_tmp.append_child(pugi::node_pcdata).text() = subsampl_height;
833  node_tmp = node_camera.append_child(pugi::node_comment);
834  node_tmp.set_value("The full size is the sensor size actually used to "
835  "grab the image. full_width = subsampling_width * "
836  "image_width");
837 
838  //<full_width>
839  node_tmp = node_camera.append_child(LABEL_XML_FULL_WIDTH);
840  node_tmp.append_child(pugi::node_pcdata).text() = im_width * subsampl_width;
841  //<full_height>
842  node_tmp = node_camera.append_child(LABEL_XML_FULL_HEIGHT);
843  node_tmp.append_child(pugi::node_pcdata).text() = im_height * subsampl_height;
844  }
845  }
846 
847  node_tmp = node_camera.append_child(pugi::node_comment);
848  node_tmp.set_value("Intrinsic camera parameters "
849  "computed for each projection model");
850 
851  back = write_camera(node_camera);
852  }
853  return back;
854  }
855 
861  int write_camera(pugi::xml_node &node_camera)
862  {
863  pugi::xml_node node_model;
864  pugi::xml_node node_tmp;
865 
866  int back = SEQUENCE_OK;
867 
868  switch (camera.get_projModel()) {
870  writeCameraWithoutDistortion(node_camera);
871  break;
872 
874  writeCameraWithDistortion(node_camera);
875  break;
876 
878  writeCameraWithKannalaBrandt(node_camera);
879  break;
880  default:
881  break;
882  }
883  return back;
884  }
885 
892  pugi::xml_node find_additional_info(const pugi::xml_node &node_)
893  {
894  vpXmlCodeType prop;
895  pugi::xml_node resNode = pugi::xml_node();
896 
897  pugi::xml_node node = node_.first_child();
898  bool hasNotFoundInfo = true;
899  while (node && hasNotFoundInfo) {
900  if (node.type() == pugi::node_element) {
901  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
902  prop = CODE_XML_OTHER;
903  }
904 
905  if (prop == CODE_XML_ADDITIONAL_INFO) {
906  // We found the node
907  resNode = node;
908  hasNotFoundInfo = false;
909  }
910  }
911 
912  node = node.next_sibling();
913  }
914 
915  return resNode;
916  }
917 
924  vpXmlCodeSequenceType str2xmlcode(const char *str, vpXmlCodeType &res)
925  {
926  vpXmlCodeType val_int = CODE_XML_BAD;
928 
929  if (!strcmp(str, LABEL_XML_CAMERA)) {
930  val_int = CODE_XML_CAMERA;
931  }
932  else if (!strcmp(str, LABEL_XML_CAMERA_NAME)) {
933  val_int = CODE_XML_CAMERA_NAME;
934  }
935  else if (!strcmp(str, LABEL_XML_MODEL)) {
936  val_int = CODE_XML_MODEL;
937  }
938  else if (!strcmp(str, LABEL_XML_MODEL_TYPE)) {
939  val_int = CODE_XML_MODEL_TYPE;
940  }
941  else if (!strcmp(str, LABEL_XML_WIDTH)) {
942  val_int = CODE_XML_WIDTH;
943  }
944  else if (!strcmp(str, LABEL_XML_HEIGHT)) {
945  val_int = CODE_XML_HEIGHT;
946  }
947  else if (!strcmp(str, LABEL_XML_SUBSAMPLING_WIDTH)) {
948  val_int = CODE_XML_SUBSAMPLING_WIDTH;
949  }
950  else if (!strcmp(str, LABEL_XML_SUBSAMPLING_HEIGHT)) {
951  val_int = CODE_XML_SUBSAMPLING_HEIGHT;
952  }
953  else if (!strcmp(str, LABEL_XML_FULL_WIDTH)) {
954  val_int = CODE_XML_FULL_WIDTH;
955  }
956  else if (!strcmp(str, LABEL_XML_FULL_HEIGHT)) {
957  val_int = CODE_XML_FULL_HEIGHT;
958  }
959  else if (!strcmp(str, LABEL_XML_U0)) {
960  val_int = CODE_XML_U0;
961  }
962  else if (!strcmp(str, LABEL_XML_V0)) {
963  val_int = CODE_XML_V0;
964  }
965  else if (!strcmp(str, LABEL_XML_PX)) {
966  val_int = CODE_XML_PX;
967  }
968  else if (!strcmp(str, LABEL_XML_PY)) {
969  val_int = CODE_XML_PY;
970  }
971  else if (!strcmp(str, LABEL_XML_KUD)) {
972  val_int = CODE_XML_KUD;
973  }
974  else if (!strcmp(str, LABEL_XML_KDU)) {
975  val_int = CODE_XML_KDU;
976  }
977  else if (!strcmp(str, LABEL_XML_K1)) {
978  val_int = CODE_XML_K1;
979  }
980  else if (!strcmp(str, LABEL_XML_K2)) {
981  val_int = CODE_XML_K2;
982  }
983  else if (!strcmp(str, LABEL_XML_K3)) {
984  val_int = CODE_XML_K3;
985  }
986  else if (!strcmp(str, LABEL_XML_K4)) {
987  val_int = CODE_XML_K4;
988  }
989  else if (!strcmp(str, LABEL_XML_K5)) {
990  val_int = CODE_XML_K5;
991  }
992  else if (!strcmp(str, LABEL_XML_ADDITIONAL_INFO)) {
993  val_int = CODE_XML_ADDITIONAL_INFO;
994  }
995  else {
996  val_int = CODE_XML_OTHER;
997  }
998  res = val_int;
999 
1000  return back;
1001  }
1002 
1003  std::string getCameraName() const { return camera_name; }
1004  vpCameraParameters getCameraParameters() const { return camera; }
1005  unsigned int getHeight() const { return image_height; }
1006  unsigned int getSubsampling_width() const { return subsampling_width; }
1007  unsigned int getSubsampling_height() const { return subsampling_height; }
1008  unsigned int getWidth() const { return image_width; }
1009 
1010  void setCameraName(const std::string &name) { camera_name = name; }
1011  void setHeight(unsigned int height) { image_height = height; }
1012  void setSubsampling_width(unsigned int subsampling) { subsampling_width = subsampling; }
1013  void setSubsampling_height(unsigned int subsampling) { subsampling_height = subsampling; }
1014  void setWidth(unsigned int width) { image_width = width; }
1015 
1016 private:
1021  void writeCameraWithoutDistortion(pugi::xml_node &node_camera)
1022  {
1023  pugi::xml_node node_model;
1024  pugi::xml_node node_tmp;
1025  //<model>
1026  node_model = node_camera.append_child(LABEL_XML_MODEL);
1027  node_tmp = node_model.append_child(pugi::node_comment);
1028  node_tmp.set_value("Projection model type");
1029 
1030  //<type>without_distortion</type>
1031  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
1032  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITHOUT_DISTORTION);
1033 
1034  node_tmp = node_model.append_child(pugi::node_comment);
1035  node_tmp.set_value("Pixel ratio");
1036  //<px>
1037  node_tmp = node_model.append_child(LABEL_XML_PX);
1038  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
1039  //<py>
1040  node_tmp = node_model.append_child(LABEL_XML_PY);
1041  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
1042 
1043  node_tmp = node_model.append_child(pugi::node_comment);
1044  node_tmp.set_value("Principal point");
1045 
1046  //<u0>
1047  node_tmp = node_model.append_child(LABEL_XML_U0);
1048  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
1049  //<v0>
1050  node_tmp = node_model.append_child(LABEL_XML_V0);
1051  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
1052  }
1053 
1058  void writeCameraWithDistortion(pugi::xml_node &node_camera)
1059  {
1060  pugi::xml_node node_model;
1061  pugi::xml_node node_tmp;
1062  //<model>
1063  node_model = node_camera.append_child(LABEL_XML_MODEL);
1064  node_tmp = node_model.append_child(pugi::node_comment);
1065  node_tmp.set_value("Projection model type");
1066  //<type>with_distortion</type>
1067  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
1068  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_DISTORTION);
1069 
1070  node_tmp = node_model.append_child(pugi::node_comment);
1071  node_tmp.set_value("Pixel ratio");
1072  //<px>
1073  node_tmp = node_model.append_child(LABEL_XML_PX);
1074  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
1075  //<py>
1076  node_tmp = node_model.append_child(LABEL_XML_PY);
1077  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
1078 
1079  node_tmp = node_model.append_child(pugi::node_comment);
1080  node_tmp.set_value("Principal point");
1081  //<u0>
1082  node_tmp = node_model.append_child(LABEL_XML_U0);
1083  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
1084  //<v0>
1085  node_tmp = node_model.append_child(LABEL_XML_V0);
1086  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
1087 
1088  //<kud>
1089  node_tmp = node_model.append_child(pugi::node_comment);
1090  node_tmp.set_value("Undistorted to distorted distortion parameter");
1091  node_tmp = node_model.append_child(LABEL_XML_KUD);
1092  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kud();
1093 
1094  //<kud>
1095  node_tmp = node_model.append_child(pugi::node_comment);
1096  node_tmp.set_value("Distorted to undistorted distortion parameter");
1097  node_tmp = node_model.append_child(LABEL_XML_KDU);
1098  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kdu();
1099  }
1100 
1105  void writeCameraWithKannalaBrandt(pugi::xml_node &node_camera)
1106  {
1107  const unsigned int index_0 = 0;
1108  const unsigned int index_1 = 1;
1109  const unsigned int index_2 = 2;
1110  const unsigned int index_3 = 3;
1111  const unsigned int index_4 = 4;
1112  const unsigned int requiredNbCoeff = 5;
1113  pugi::xml_node node_model;
1114  pugi::xml_node node_tmp;
1115  //<model>
1116  node_model = node_camera.append_child(LABEL_XML_MODEL);
1117  node_tmp = node_model.append_child(pugi::node_comment);
1118  node_tmp.set_value("Projection model type");
1119  //<type>with_KannalaBrandt_distortion</type>
1120  node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE);
1121  node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION);
1122 
1123  node_tmp = node_model.append_child(pugi::node_comment);
1124  node_tmp.set_value("Pixel ratio");
1125  //<px>
1126  node_tmp = node_model.append_child(LABEL_XML_PX);
1127  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px();
1128  //<py>
1129  node_tmp = node_model.append_child(LABEL_XML_PY);
1130  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py();
1131 
1132  node_tmp = node_model.append_child(pugi::node_comment);
1133  node_tmp.set_value("Principal point");
1134  //<u0>
1135  node_tmp = node_model.append_child(LABEL_XML_U0);
1136  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0();
1137  //<v0>
1138  node_tmp = node_model.append_child(LABEL_XML_V0);
1139  node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0();
1140 
1141  std::vector<double> distortion_coefs = camera.getKannalaBrandtDistortionCoefficients();
1142 
1143  if (distortion_coefs.size() != requiredNbCoeff) {
1144  std::cout << "Make sure to have 5 distortion coefficients for Kannala-Brandt distortions." << std::endl;
1145  }
1146 
1147  node_tmp = node_model.append_child(pugi::node_comment);
1148  node_tmp.set_value("Distortion coefficients");
1149  node_tmp = node_model.append_child(LABEL_XML_K1);
1150  distortion_coefs.size() == index_0 ? (node_tmp.append_child(pugi::node_pcdata).text() = 0)
1151  : (node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_0]);
1152  node_tmp = node_model.append_child(LABEL_XML_K2);
1153  distortion_coefs.size() <= index_1 ? (node_tmp.append_child(pugi::node_pcdata).text() = 0)
1154  : (node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_1]);
1155  node_tmp = node_model.append_child(LABEL_XML_K3);
1156  distortion_coefs.size() <= index_2 ? (node_tmp.append_child(pugi::node_pcdata).text() = 0)
1157  : (node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_2]);
1158  node_tmp = node_model.append_child(LABEL_XML_K4);
1159  distortion_coefs.size() <= index_3 ? (node_tmp.append_child(pugi::node_pcdata).text() = 0)
1160  : (node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_3]);
1161  node_tmp = node_model.append_child(LABEL_XML_K5);
1162  distortion_coefs.size() <= index_4 ? (node_tmp.append_child(pugi::node_pcdata).text() = 0)
1163  : (node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_4]);
1164  }
1165 
1166  vpCameraParameters camera;
1167  std::string camera_name;
1168  unsigned int image_width;
1169  unsigned int image_height;
1170  unsigned int subsampling_width;
1171  unsigned int subsampling_height;
1172  unsigned int full_width;
1173  unsigned int full_height;
1174 
1177  static const int allowedPixelDiffOnImageSize = 15;
1178 };
1179 #endif // DOXYGEN_SHOULD_SKIP_THIS
1180 
1181 vpXmlParserCamera::vpXmlParserCamera() : m_impl(new Impl()) { }
1182 
1184 
1200 int vpXmlParserCamera::parse(vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1201  const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int im_width,
1202  unsigned int im_height, bool verbose)
1203 {
1204  return m_impl->parse(cam, filename, cam_name, projModel, im_width, im_height, verbose);
1205 }
1206 
1255 int vpXmlParserCamera::save(const vpCameraParameters &cam, const std::string &filename, const std::string &cam_name,
1256  unsigned int im_width, unsigned int im_height, const std::string &additionalInfo, bool verbose)
1257 {
1258  return m_impl->save(cam, filename, cam_name, im_width, im_height, additionalInfo, verbose);
1259 }
1260 
1261 std::string vpXmlParserCamera::getCameraName() const { return m_impl->getCameraName(); }
1262 
1263 vpCameraParameters vpXmlParserCamera::getCameraParameters() const { return m_impl->getCameraParameters(); }
1264 
1265 unsigned int vpXmlParserCamera::getHeight() const { return m_impl->getHeight(); }
1266 
1267 unsigned int vpXmlParserCamera::getSubsampling_width() const { return m_impl->getSubsampling_width(); }
1268 
1269 unsigned int vpXmlParserCamera::getSubsampling_height() const { return m_impl->getSubsampling_height(); }
1270 
1271 unsigned int vpXmlParserCamera::getWidth() const { return m_impl->getWidth(); }
1272 
1273 void vpXmlParserCamera::setCameraName(const std::string &name) { m_impl->setCameraName(name); }
1274 
1275 void vpXmlParserCamera::setHeight(unsigned int height) { m_impl->setHeight(height); }
1276 
1277 void vpXmlParserCamera::setSubsampling_width(unsigned int subsampling) { m_impl->setSubsampling_width(subsampling); }
1278 
1279 void vpXmlParserCamera::setSubsampling_height(unsigned int subsampling) { m_impl->setSubsampling_height(subsampling); }
1280 
1281 void vpXmlParserCamera::setWidth(unsigned int width) { m_impl->setWidth(width); }
1282 END_VISP_NAMESPACE
1283 #elif !defined(VISP_BUILD_SHARED_LIBS)
1284 // Work around to avoid warning: libvisp_core.a(vpXmlParserCamera.cpp.o) has no symbols
1285 void dummy_vpXmlParserCamera() { };
1286 
1287 #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