Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpXmlParserHomogeneousMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * XML parser to load and save Homogeneous Matrix in a XML file
33  *
34  * Authors:
35  * Giovanni Claudio
36  *
37  *****************************************************************************/
38 
45 #include <visp3/core/vpXmlParserHomogeneousMatrix.h>
46 
47 #ifdef VISP_HAVE_PUGIXML
48 
49 #include <pugixml.hpp>
50 
51 /* ----------------------------- LABEL XML ----------------------------- */
52 /* --------------------------------------------------------------------- */
53 #define LABEL_XML_ROOT "root"
54 #define LABEL_XML_M "homogeneous_transformation"
55 #define LABEL_XML_M_NAME "name"
56 #define LABEL_XML_VALUE "values"
57 #define LABEL_XML_TRANSLATION "translation"
58 #define LABEL_XML_TX "tx"
59 #define LABEL_XML_TY "ty"
60 #define LABEL_XML_TZ "tz"
61 #define LABEL_XML_ROTATION "rotation"
62 #define LABEL_XML_TUX "theta_ux"
63 #define LABEL_XML_TUY "theta_uy"
64 #define LABEL_XML_TUZ "theta_uz"
65 
66 #ifndef DOXYGEN_SHOULD_SKIP_THIS
67 class vpXmlParserHomogeneousMatrix::Impl
68 {
69 private:
70  /* --- XML Code------------------------------------------------------------
71  */
72  enum vpXmlCodeType {
73  CODE_XML_BAD = -1,
74  CODE_XML_OTHER,
75  CODE_XML_M,
76  CODE_XML_M_NAME,
77  CODE_XML_VALUE,
78  CODE_XML_TX,
79  CODE_XML_TY,
80  CODE_XML_TZ,
81  CODE_XML_TUX,
82  CODE_XML_TUY,
83  CODE_XML_TUZ
84  } ;
85 
86 public:
87  Impl() : m_M(), m_name()
88  {
89  }
90 
91  int parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
92  {
93  pugi::xml_document doc;
94  if (!doc.load_file(filename.c_str())) {
95  std::cerr << std::endl << "ERROR:" << std::endl;
96  std::cerr << " I cannot open the file " << filename << std::endl;
97 
98  return SEQUENCE_ERROR;
99  }
100 
101  pugi::xml_node node = doc.document_element();
102  if (!node) {
103  return SEQUENCE_ERROR;
104  }
105 
106  int ret = read(node, name);
107 
108  M = m_M;
109 
110  return ret;
111  }
112 
120  int read(const pugi::xml_node &node_, const std::string &name)
121  {
122  vpXmlCodeType prop;
123 
125  unsigned int nbM = 0;
126 
127  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
128  if (node.type() != pugi::node_element)
129  continue;
130 
131  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
132  prop = CODE_XML_OTHER;
133  back = SEQUENCE_ERROR;
134  }
135 
136  if (prop == CODE_XML_M) {
137  if (SEQUENCE_OK == read_matrix(node, name))
138  nbM++;
139  } else
140  back = SEQUENCE_ERROR;
141  }
142 
143  if (nbM == 0) {
144  back = SEQUENCE_ERROR;
145  std::cerr << "No Homogeneous matrix is available" << std::endl << "with name: " << name << std::endl;
146  } else if (nbM > 1) {
147  back = SEQUENCE_ERROR;
148  std::cerr << nbM << " There are more Homogeneous matrix" << std::endl
149  << "with the same name : " << std::endl
150  << "precise your choice..." << std::endl;
151  }
152 
153  return back;
154  }
155 
164  int read_matrix(const pugi::xml_node &node_, const std::string &name)
165  {
166  vpXmlCodeType prop;
167  /* read value in the XML file. */
168  std::string M_name_tmp = "";
169  vpHomogeneousMatrix M_tmp;
170 
172 
173  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
174  if (node.type() != pugi::node_element)
175  continue;
176 
177  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
178  prop = CODE_XML_OTHER;
179  back = SEQUENCE_ERROR;
180  }
181 
182  switch (prop) {
183  case CODE_XML_M_NAME: {
184  M_name_tmp = node.text().as_string();
185  break;
186  }
187 
188  case CODE_XML_VALUE: // VALUE
189  if (name == M_name_tmp) {
190  std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp << "\"" << std::endl;
191  back = read_values(node, M_tmp);
192  }
193  break;
194 
195  case CODE_XML_BAD:
196  case CODE_XML_OTHER:
197  case CODE_XML_M:
198  case CODE_XML_TX:
199  case CODE_XML_TY:
200  case CODE_XML_TZ:
201  case CODE_XML_TUX:
202  case CODE_XML_TUY:
203  case CODE_XML_TUZ:
204 
205  default:
206  back = SEQUENCE_ERROR;
207  break;
208  }
209  }
210 
211  if (!(name == M_name_tmp)) {
212  back = SEQUENCE_ERROR;
213  } else {
214  this->m_M = M_tmp;
215  // std::cout << "Convert in Homogeneous Matrix:"<< std::endl;
216  // std::cout << this-> M << std::endl;
217  this->m_name = M_name_tmp;
218  }
219  return back;
220  }
221 
230  vpXmlCodeSequenceType read_values(const pugi::xml_node &node_, vpHomogeneousMatrix &M)
231  {
232  // counter of the number of read parameters
233  int nb = 0;
234  vpXmlCodeType prop;
235  /* read value in the XML file. */
236 
237  double tx_ = 0.;
238  double ty_ = 0.;
239  double tz_ = 0.;
240  double tux_ = 0.;
241  double tuy_ = 0.;
242  double tuz_ = 0.;
243 
245 
246  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
247  if (node.type() != pugi::node_element)
248  continue;
249 
250  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
251  prop = CODE_XML_OTHER;
252  back = SEQUENCE_ERROR;
253  }
254 
255  switch (prop) {
256  case CODE_XML_TX:
257  tx_ = node.text().as_double();
258  nb++;
259  break;
260  case CODE_XML_TY:
261  ty_ = node.text().as_double();
262  nb++;
263  break;
264  case CODE_XML_TZ:
265  tz_ = node.text().as_double();
266  nb++;
267  break;
268  case CODE_XML_TUX:
269  tux_ = node.text().as_double();
270  nb++;
271  break;
272  case CODE_XML_TUY:
273  tuy_ = node.text().as_double();
274  nb++;
275  break;
276  case CODE_XML_TUZ:
277  tuz_ = node.text().as_double();
278  nb++;
279  break;
280 
281  case CODE_XML_BAD:
282  case CODE_XML_OTHER:
283  case CODE_XML_M:
284  case CODE_XML_M_NAME:
285  case CODE_XML_VALUE:
286 
287  default:
288  back = SEQUENCE_ERROR;
289  break;
290  }
291  }
292 
293  if (nb != 6) {
294  std::cerr << "ERROR in 'model' field:\n";
295  std::cerr << "it must contain 6 parameters\n";
296 
297  return SEQUENCE_ERROR;
298  }
299 
300  // Create the Homogeneous matrix
301  M.buildFrom(tx_, ty_, tz_, tux_, tuy_, tuz_);
302 
303  // std::cout << "Read values from file:" << std::endl;
304  // std::cout << "tx:" << tx_<< std::endl;
305  // std::cout << "ty:" << ty_<< std::endl;
306  // std::cout << "tz:" << tz_<< std::endl;
307  // std::cout << "tux:" << tux_<< std::endl;
308  // std::cout << "tuy:" << tuy_<< std::endl;
309  // std::cout << "tuz:" << tuz_<< std::endl;
310 
311  return back;
312  }
313 
314  int save(const vpHomogeneousMatrix &M, const std::string &filename,
315  const std::string &name)
316  {
317  pugi::xml_document doc;
318  pugi::xml_node node;
319 
320  if (!doc.load_file(filename.c_str(), pugi::parse_default | pugi::parse_comments)) {
321  node = doc.append_child(pugi::node_declaration);
322  node.append_attribute("version") = "1.0";
323  node = doc.append_child(LABEL_XML_ROOT);
324  pugi::xml_node nodeComment = node.append_child(pugi::node_comment);
325  nodeComment.set_value("This file stores homogeneous matrix used\n"
326  " in the vpHomogeneousMatrix Class of ViSP available\n"
327  " at https://visp.inria.fr/download/ .\n"
328  " It can be read with the parse method of\n"
329  " the vpXmlParserHomogeneousMatrix class.");
330  }
331 
332  node = doc.document_element();
333  if (!node) {
334  return SEQUENCE_ERROR;
335  }
336 
337  m_M = M;
338 
339  int M_isFound = count(node, name);
340 
341  if (M_isFound) {
342  std::cout << "There is already an homogeneous matrix " << std::endl
343  << "available in the file with the input name: " << name << "." << std::endl
344  << "Please delete it manually from the xml file." << std::endl;
345  return SEQUENCE_ERROR;
346  }
347 
348  write(node, name);
349 
350  doc.save_file(filename.c_str(), PUGIXML_TEXT(" "));
351 
352  return SEQUENCE_OK;
353  }
354 
365  int count(const pugi::xml_node &node_, const std::string &name)
366  {
367  vpXmlCodeType prop;
368  int nbM = 0;
369 
370  for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) {
371  if (node.type() != pugi::node_element)
372  continue;
373 
374  if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) {
375  prop = CODE_XML_OTHER;
376  }
377  if (prop == CODE_XML_M) {
378  if (SEQUENCE_OK == read_matrix(node, name))
379  nbM++;
380  }
381  }
382 
383  return nbM;
384  }
385 
394  int write(pugi::xml_node &node, const std::string &name)
395  {
396  int back = SEQUENCE_OK;
397 
398  pugi::xml_node node_tmp;
399  pugi::xml_node node_matrix;
400  pugi::xml_node node_values;
401 
402  // Convert from Rotational matrix to Theta-U vector
404  m_M.extract(R);
405 
406  vpThetaUVector tu(R);
407 
408  // <homogeneous_transformation>
409  node_tmp = node.append_child(pugi::node_comment);
410  node_tmp.set_value("Homogeneous Matrix");
411  node_matrix = node.append_child(LABEL_XML_M);
412  {
413  //<name>
414  if (!name.empty()) {
415  node_tmp = node_matrix.append_child(pugi::node_comment);
416  node_tmp.set_value("Name of the homogeneous matrix");
417  node_matrix.append_child(LABEL_XML_M_NAME).append_child(pugi::node_pcdata).set_value(name.c_str());
418  }
419 
420  //<values>
421  node_values = node_matrix.append_child(LABEL_XML_VALUE);
422  {
423  node_tmp = node_values.append_child(pugi::node_comment);
424  node_tmp.set_value("Translation vector with values in meters");
425 
426  //<tx>
427  node_values.append_child(LABEL_XML_TX).append_child(pugi::node_pcdata).text() = m_M[0][3];
428 
429  //<ty>
430  node_values.append_child(LABEL_XML_TY).append_child(pugi::node_pcdata).text() = m_M[1][3];
431 
432  //<tz>
433  node_values.append_child(LABEL_XML_TZ).append_child(pugi::node_pcdata).text() = m_M[2][3];
434 
435  node_tmp = node_values.append_child(pugi::node_comment);
436  node_tmp.set_value("Rotational vector expressed in angle axis "
437  "representation with values in radians");
438 
439  //<tux>
440  node_values.append_child(LABEL_XML_TUX).append_child(pugi::node_pcdata).text() = tu[0];
441 
442  //<tuy>
443  node_values.append_child(LABEL_XML_TUY).append_child(pugi::node_pcdata).text() = tu[1];
444 
445  //<tuz>
446  node_values.append_child(LABEL_XML_TUZ).append_child(pugi::node_pcdata).text() = tu[2];
447  }
448  }
449 
450  return back;
451  }
452 
460  vpXmlCodeSequenceType str2xmlcode(const char *str, vpXmlCodeType &res)
461  {
462  vpXmlCodeType val_int = CODE_XML_BAD;
464 
465  if (!strcmp(str, LABEL_XML_M)) {
466  val_int = CODE_XML_M;
467  } else if (!strcmp(str, LABEL_XML_M_NAME)) {
468  val_int = CODE_XML_M_NAME;
469  } else if (!strcmp(str, LABEL_XML_VALUE)) {
470  val_int = CODE_XML_VALUE;
471  } else if (!strcmp(str, LABEL_XML_TX)) {
472  val_int = CODE_XML_TX;
473  } else if (!strcmp(str, LABEL_XML_TY)) {
474  val_int = CODE_XML_TY;
475  } else if (!strcmp(str, LABEL_XML_TZ)) {
476  val_int = CODE_XML_TZ;
477  } else if (!strcmp(str, LABEL_XML_TUX)) {
478  val_int = CODE_XML_TUX;
479  } else if (!strcmp(str, LABEL_XML_TUY)) {
480  val_int = CODE_XML_TUY;
481  } else if (!strcmp(str, LABEL_XML_TUZ)) {
482  val_int = CODE_XML_TUZ;
483  } else {
484  val_int = CODE_XML_OTHER;
485  }
486  res = val_int;
487 
488  return back;
489  }
490 
491  vpHomogeneousMatrix getHomogeneousMatrix() const { return m_M; }
492  std::string getHomogeneousMatrixName() const { return m_name; }
493 
494  void setHomogeneousMatrixName(const std::string &name) { m_name = name; }
495 
496 private:
498  std::string m_name;
499 };
500 #endif //DOXYGEN_SHOULD_SKIP_THIS
501 
503 {
504 }
505 
507 {
508  delete m_impl;
509 }
510 
519 int vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
520 {
521  return m_impl->parse(M, filename, name);
522 }
523 
532 int vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::string &filename,
533  const std::string &name)
534 {
535  return m_impl->save(M, filename, name);
536 }
537 
539 {
540  return m_impl->getHomogeneousMatrix();
541 }
542 
544 {
545  return m_impl->getHomogeneousMatrixName();
546 }
547 
549 {
550  m_impl->setHomogeneousMatrixName(name);
551 }
552 #elif !defined(VISP_BUILD_SHARED_LIBS)
553 // Work arround to avoid warning:
554 // libvisp_core.a(vpXmlParserHomogeneousMatrix.cpp.o) has no symbols
555 void dummy_vpXmlParserHomogeneousMatrix(){};
556 #endif //VISP_HAVE_PUGIXML
int parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
void setHomogeneousMatrixName(const std::string &name)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a rotation matrix and operations on such kind of matrices.
int save(const vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix getHomogeneousMatrix() const
Implementation of a rotation vector as axis-angle minimal representation.