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