Visual Servoing Platform  version 3.6.1 under development (2024-06-18)
vpXmlParserHomogeneousMatrix.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 Homogeneous Matrix in a XML file
33  *
34  * Authors:
35  * Giovanni Claudio
36  *
37 *****************************************************************************/
38 
45 #include <visp3/core/vpXmlParserHomogeneousMatrix.h>
46 
47 #if defined(VISP_HAVE_PUGIXML)
48 #include <pugixml.hpp>
49 
50 /* ----------------------------- LABEL XML ----------------------------- */
51 /* --------------------------------------------------------------------- */
52 #define LABEL_XML_ROOT "root"
53 #define LABEL_XML_M "homogeneous_transformation"
54 #define LABEL_XML_M_NAME "name"
55 #define LABEL_XML_VALUE "values"
56 #define LABEL_XML_TRANSLATION "translation"
57 #define LABEL_XML_TX "tx"
58 #define LABEL_XML_TY "ty"
59 #define LABEL_XML_TZ "tz"
60 #define LABEL_XML_ROTATION "rotation"
61 #define LABEL_XML_TUX "theta_ux"
62 #define LABEL_XML_TUY "theta_uy"
63 #define LABEL_XML_TUZ "theta_uz"
64 
66 #ifndef DOXYGEN_SHOULD_SKIP_THIS
67 class vpXmlParserHomogeneousMatrix::Impl
68 {
69 private:
70  /* --- XML Code------------------------------------------------------------
71  */
72  enum vpXmlCodeType
73  {
74  CODE_XML_BAD = -1,
75  CODE_XML_OTHER,
76  CODE_XML_M,
77  CODE_XML_M_NAME,
78  CODE_XML_VALUE,
79  CODE_XML_TX,
80  CODE_XML_TY,
81  CODE_XML_TZ,
82  CODE_XML_TUX,
83  CODE_XML_TUY,
84  CODE_XML_TUZ
85  };
86 
87 public:
88  Impl() : m_M(), m_name() { }
89 
90  int parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
91  {
92  pugi::xml_document doc;
93  if (!doc.load_file(filename.c_str())) {
94  std::cerr << std::endl << "ERROR:" << std::endl;
95  std::cerr << " I cannot open the file " << filename << std::endl;
96 
97  return SEQUENCE_ERROR;
98  }
99 
100  pugi::xml_node node = doc.document_element();
101  if (!node) {
102  return SEQUENCE_ERROR;
103  }
104 
105  int ret = read(node, name);
106 
107  M = m_M;
108 
109  return ret;
110  }
111 
119  int read(const pugi::xml_node &node_, const std::string &name)
120  {
121  vpXmlCodeType prop;
122 
124  unsigned int nbM = 0;
125 
126  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
127  if (node.type() != pugi::node_element)
128  continue;
129 
130  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
131  prop = CODE_XML_OTHER;
132  back = SEQUENCE_ERROR;
133  }
134 
135  if (prop == CODE_XML_M) {
136  if (SEQUENCE_OK == read_matrix(node, name)) {
137  nbM++;
138  }
139  }
140  else {
141  back = SEQUENCE_ERROR;
142  }
143  }
144 
145  if (nbM == 0) {
146  back = SEQUENCE_ERROR;
147  std::cerr << "No Homogeneous matrix is available" << std::endl << "with name: " << name << std::endl;
148  }
149  else if (nbM > 1) {
150  back = SEQUENCE_ERROR;
151  std::cerr << nbM << " There are more Homogeneous matrix" << std::endl
152  << "with the same name : " << std::endl
153  << "precise your choice..." << std::endl;
154  }
155 
156  return back;
157  }
158 
167  int read_matrix(const pugi::xml_node &node_, const std::string &name)
168  {
169  vpXmlCodeType prop;
170  /* read value in the XML file. */
171  std::string M_name_tmp = "";
172  vpHomogeneousMatrix M_tmp;
173 
175 
176  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
177  if (node.type() != pugi::node_element)
178  continue;
179 
180  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
181  prop = CODE_XML_OTHER;
182  back = SEQUENCE_ERROR;
183  }
184 
185  switch (prop) {
186  case CODE_XML_M_NAME: {
187  M_name_tmp = node.text().as_string();
188  break;
189  }
190 
191  case CODE_XML_VALUE: // VALUE
192  if (name == M_name_tmp) {
193  std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp << "\"" << std::endl;
194  back = read_values(node, M_tmp);
195  }
196  break;
197 
198  case CODE_XML_BAD:
199  case CODE_XML_OTHER:
200  case CODE_XML_M:
201  case CODE_XML_TX:
202  case CODE_XML_TY:
203  case CODE_XML_TZ:
204  case CODE_XML_TUX:
205  case CODE_XML_TUY:
206  case CODE_XML_TUZ:
207 
208  default:
209  back = SEQUENCE_ERROR;
210  break;
211  }
212  }
213 
214  if (!(name == M_name_tmp)) {
215  back = SEQUENCE_ERROR;
216  }
217  else {
218  this->m_M = M_tmp;
219  // --comment: std::cout << "Convert in Homogeneous Matrix:"<< std::endl;
220  // --comment: std::cout << this-> M << std::endl;
221  this->m_name = M_name_tmp;
222  }
223  return back;
224  }
225 
234  vpXmlCodeSequenceType read_values(const pugi::xml_node &node_, vpHomogeneousMatrix &M)
235  {
236  // counter of the number of read parameters
237  int nb = 0;
238  vpXmlCodeType prop;
239  /* read value in the XML file. */
240 
241  double tx_ = 0.;
242  double ty_ = 0.;
243  double tz_ = 0.;
244  double tux_ = 0.;
245  double tuy_ = 0.;
246  double tuz_ = 0.;
247 
249 
250  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
251  if (node.type() != pugi::node_element)
252  continue;
253 
254  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
255  prop = CODE_XML_OTHER;
256  back = SEQUENCE_ERROR;
257  }
258 
259  switch (prop) {
260  case CODE_XML_TX:
261  tx_ = node.text().as_double();
262  nb++;
263  break;
264  case CODE_XML_TY:
265  ty_ = node.text().as_double();
266  nb++;
267  break;
268  case CODE_XML_TZ:
269  tz_ = node.text().as_double();
270  nb++;
271  break;
272  case CODE_XML_TUX:
273  tux_ = node.text().as_double();
274  nb++;
275  break;
276  case CODE_XML_TUY:
277  tuy_ = node.text().as_double();
278  nb++;
279  break;
280  case CODE_XML_TUZ:
281  tuz_ = node.text().as_double();
282  nb++;
283  break;
284 
285  case CODE_XML_BAD:
286  case CODE_XML_OTHER:
287  case CODE_XML_M:
288  case CODE_XML_M_NAME:
289  case CODE_XML_VALUE:
290 
291  default:
292  back = SEQUENCE_ERROR;
293  break;
294  }
295  }
296 
297  if (nb != 6) {
298  std::cerr << "ERROR in 'model' field:\n";
299  std::cerr << "it must contain 6 parameters\n";
300 
301  return SEQUENCE_ERROR;
302  }
303 
304  // Create the Homogeneous matrix
305  M.build(tx_, ty_, tz_, tux_, tuy_, tuz_);
306 
307  // --comment: std::cout << "Read values from file:" << std::endl;
308  // --comment: std::cout << "tx:" << tx_<< std::endl;
309  // --comment: std::cout << "ty:" << ty_<< std::endl;
310  // --comment: std::cout << "tz:" << tz_<< std::endl;
311  // --comment: std::cout << "tux:" << tux_<< std::endl;
312  // --comment: std::cout << "tuy:" << tuy_<< std::endl;
313  // --comment: std::cout << "tuz:" << tuz_<< std::endl;
314 
315  return back;
316  }
317 
318  int save(const vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
319  {
320  pugi::xml_document doc;
321  pugi::xml_node node;
322 
323  if (!doc.load_file(filename.c_str(), pugi::parse_default | pugi::parse_comments)) {
324  node = doc.append_child(pugi::node_declaration);
325  node.append_attribute("version") = "1.0";
326  node = doc.append_child(LABEL_XML_ROOT);
327  pugi::xml_node nodeComment = node.append_child(pugi::node_comment);
328  nodeComment.set_value("This file stores homogeneous matrix used\n"
329  " in the vpHomogeneousMatrix Class of ViSP available\n"
330  " at https://visp.inria.fr/download/ .\n"
331  " It can be read with the parse method of\n"
332  " the vpXmlParserHomogeneousMatrix class.");
333  }
334 
335  node = doc.document_element();
336  if (!node) {
337  return SEQUENCE_ERROR;
338  }
339 
340  m_M = M;
341 
342  int M_isFound = count(node, name);
343 
344  if (M_isFound) {
345  std::cout << "There is already an homogeneous matrix " << std::endl
346  << "available in the file with the input name: " << name << "." << std::endl
347  << "Please delete it manually from the xml file." << std::endl;
348  return SEQUENCE_ERROR;
349  }
350 
351  write(node, name);
352 
353  doc.save_file(filename.c_str(), PUGIXML_TEXT(" "));
354 
355  return SEQUENCE_OK;
356  }
357 
368  int count(const pugi::xml_node &node_, const std::string &name)
369  {
370  vpXmlCodeType prop;
371  int nbM = 0;
372 
373  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
374  if (node.type() != pugi::node_element)
375  continue;
376 
377  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
378  prop = CODE_XML_OTHER;
379  }
380  if (prop == CODE_XML_M) {
381  if (SEQUENCE_OK == read_matrix(node, name)) {
382  nbM++;
383  }
384  }
385  }
386 
387  return nbM;
388  }
389 
398  int write(pugi::xml_node &node, const std::string &name)
399  {
400  int back = SEQUENCE_OK;
401 
402  pugi::xml_node node_tmp;
403  pugi::xml_node node_matrix;
404  pugi::xml_node node_values;
405 
406  // Convert from Rotational matrix to Theta-U vector
408  m_M.extract(R);
409 
410  vpThetaUVector tu(R);
411 
412  // <homogeneous_transformation>
413  node_tmp = node.append_child(pugi::node_comment);
414  node_tmp.set_value("Homogeneous Matrix");
415  node_matrix = node.append_child(LABEL_XML_M);
416  {
417  //<name>
418  if (!name.empty()) {
419  node_tmp = node_matrix.append_child(pugi::node_comment);
420  node_tmp.set_value("Name of the homogeneous matrix");
421  node_matrix.append_child(LABEL_XML_M_NAME).append_child(pugi::node_pcdata).set_value(name.c_str());
422  }
423 
424  //<values>
425  node_values = node_matrix.append_child(LABEL_XML_VALUE);
426  {
427  node_tmp = node_values.append_child(pugi::node_comment);
428  node_tmp.set_value("Translation vector with values in meters");
429 
430  //<tx>
431  node_values.append_child(LABEL_XML_TX).append_child(pugi::node_pcdata).text() = m_M[0][3];
432 
433  //<ty>
434  node_values.append_child(LABEL_XML_TY).append_child(pugi::node_pcdata).text() = m_M[1][3];
435 
436  //<tz>
437  node_values.append_child(LABEL_XML_TZ).append_child(pugi::node_pcdata).text() = m_M[2][3];
438 
439  node_tmp = node_values.append_child(pugi::node_comment);
440  node_tmp.set_value("Rotational vector expressed in angle axis "
441  "representation with values in radians");
442 
443  //<tux>
444  node_values.append_child(LABEL_XML_TUX).append_child(pugi::node_pcdata).text() = tu[0];
445 
446  //<tuy>
447  node_values.append_child(LABEL_XML_TUY).append_child(pugi::node_pcdata).text() = tu[1];
448 
449  //<tuz>
450  node_values.append_child(LABEL_XML_TUZ).append_child(pugi::node_pcdata).text() = tu[2];
451  }
452  }
453 
454  return back;
455  }
456 
464  vpXmlCodeSequenceType str2xmlcode(const char *str, vpXmlCodeType &res)
465  {
466  vpXmlCodeType val_int = CODE_XML_BAD;
468 
469  if (!strcmp(str, LABEL_XML_M)) {
470  val_int = CODE_XML_M;
471  }
472  else if (!strcmp(str, LABEL_XML_M_NAME)) {
473  val_int = CODE_XML_M_NAME;
474  }
475  else if (!strcmp(str, LABEL_XML_VALUE)) {
476  val_int = CODE_XML_VALUE;
477  }
478  else if (!strcmp(str, LABEL_XML_TX)) {
479  val_int = CODE_XML_TX;
480  }
481  else if (!strcmp(str, LABEL_XML_TY)) {
482  val_int = CODE_XML_TY;
483  }
484  else if (!strcmp(str, LABEL_XML_TZ)) {
485  val_int = CODE_XML_TZ;
486  }
487  else if (!strcmp(str, LABEL_XML_TUX)) {
488  val_int = CODE_XML_TUX;
489  }
490  else if (!strcmp(str, LABEL_XML_TUY)) {
491  val_int = CODE_XML_TUY;
492  }
493  else if (!strcmp(str, LABEL_XML_TUZ)) {
494  val_int = CODE_XML_TUZ;
495  }
496  else {
497  val_int = CODE_XML_OTHER;
498  }
499  res = val_int;
500 
501  return back;
502  }
503 
504  vpHomogeneousMatrix getHomogeneousMatrix() const { return m_M; }
505  std::string getHomogeneousMatrixName() const { return m_name; }
506 
507  void setHomogeneousMatrixName(const std::string &name) { m_name = name; }
508 
509 private:
511  std::string m_name;
512 };
513 #endif // DOXYGEN_SHOULD_SKIP_THIS
514 
516 
518 
527 int vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
528 {
529  return m_impl->parse(M, filename, name);
530 }
531 
540 int vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::string &filename,
541  const std::string &name)
542 {
543  return m_impl->save(M, filename, name);
544 }
545 
547 {
548  return m_impl->getHomogeneousMatrix();
549 }
550 
552 {
553  return m_impl->getHomogeneousMatrixName();
554 }
555 
557 {
558  m_impl->setHomogeneousMatrixName(name);
559 }
561 #elif !defined(VISP_BUILD_SHARED_LIBS)
562 // Work around to avoid warning: libvisp_core.a(vpXmlParserHomogeneousMatrix.cpp.o) has no symbols
563 void dummy_vpXmlParserHomogeneousMatrix() { };
564 
565 #endif
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
vpHomogeneousMatrix getHomogeneousMatrix() const
int parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
int save(const vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
void setHomogeneousMatrixName(const std::string &name)