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