Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
46 #include <visp3/core/vpXmlParserHomogeneousMatrix.h>
47 #ifdef VISP_HAVE_XML2
48 
49 #include <stdlib.h>
50 #include <string.h>
51 
52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpThetaUVector.h>
54 /* --------------------------------------------------------------------------
55  */
56 /* --- LABEL XML ------------------------------------------------------------
57  */
58 /* --------------------------------------------------------------------------
59  */
60 
61 #define LABEL_XML_ROOT "root"
62 #define LABEL_XML_M "homogeneous_transformation"
63 #define LABEL_XML_M_NAME "name"
64 #define LABEL_XML_VALUE "values"
65 #define LABEL_XML_TRANSLATION "translation"
66 #define LABEL_XML_TX "tx"
67 #define LABEL_XML_TY "ty"
68 #define LABEL_XML_TZ "tz"
69 #define LABEL_XML_ROTATION "rotation"
70 #define LABEL_XML_TUX "theta_ux"
71 #define LABEL_XML_TUY "theta_uy"
72 #define LABEL_XML_TUZ "theta_uz"
73 
83  : vpXmlParser(twinParser), m_M(), m_name()
84 {
85  *this = twinParser;
86 }
87 
94 {
95  this->m_M = twinParser.m_M;
96  this->m_name = twinParser.m_name;
97 
98  return *this;
99 }
100 
109 int vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
110 {
111  xmlDocPtr doc;
112  xmlNodePtr node;
113 
114  doc = xmlParseFile(filename.c_str());
115  if (doc == NULL) {
116  std::cerr << std::endl << "ERROR:" << std::endl;
117  std::cerr << " I cannot open the file " << filename << std::endl;
118 
119  return SEQUENCE_ERROR;
120  }
121 
122  node = xmlDocGetRootElement(doc);
123  if (node == NULL) {
124  xmlFreeDoc(doc);
125  return SEQUENCE_ERROR;
126  }
127 
128  int ret = this->read(doc, node, name);
129 
130  M = m_M;
131 
132  xmlFreeDoc(doc);
133 
134  return ret;
135 }
136 
145 int vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::string &filename,
146  const std::string &name)
147 {
148  xmlDocPtr doc;
149  xmlNodePtr node;
150 
151  doc = xmlReadFile(filename.c_str(), NULL, XML_PARSE_NOWARNING + XML_PARSE_NOERROR + XML_PARSE_NOBLANKS);
152  if (doc == NULL) {
153  doc = xmlNewDoc((xmlChar *)"1.0");
154  node = xmlNewNode(NULL, (xmlChar *)LABEL_XML_ROOT);
155  xmlDocSetRootElement(doc, node);
156  xmlNodePtr node_tmp = xmlNewComment((xmlChar *)"This file stores homogeneous matrix used\n"
157  " in the vpHomogeneousMatrix Class of ViSP available\n"
158  " at https://visp.inria.fr/download/ .\n"
159  " It can be read with the parse method of\n"
160  " the vpXmlParserHomogeneousMatrix class.");
161  xmlAddChild(node, node_tmp);
162  }
163 
164  node = xmlDocGetRootElement(doc);
165  if (node == NULL) {
166  xmlFreeDoc(doc);
167  return SEQUENCE_ERROR;
168  }
169 
170  this->m_M = M;
171 
172  int M_isFound = count(doc, node, name);
173 
174  if (M_isFound > 0) {
175  // vpCERROR
176  std::cout << "There is already an homogeneous matrix " << std::endl
177  << "available in the file with the input name: " << name << "." << std::endl
178  << "Please delete it manually from the xml file." << std::endl;
179  xmlFreeDoc(doc);
180  return SEQUENCE_ERROR;
181  }
182 
183  write(node, name);
184 
185  xmlSaveFormatFile(filename.c_str(), doc, 1);
186  xmlFreeDoc(doc);
187  // std::cout << "Homogeneous matrix '"<< name << "' saved in the file named
188  // "<< filename << " correctly." << std::endl;
189 
190  return SEQUENCE_OK;
191 }
192 
201 int vpXmlParserHomogeneousMatrix::read(xmlDocPtr doc, xmlNodePtr node, const std::string &name)
202 {
203  // char * val_char;
204  vpXmlCodeType prop;
205 
207  unsigned int nbM = 0;
208 
209  for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
210  if (node->type != XML_ELEMENT_NODE)
211  continue;
212  if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
213  prop = CODE_XML_OTHER;
214  back = SEQUENCE_ERROR;
215  }
216 
217  if (prop == CODE_XML_M) {
218  if (SEQUENCE_OK == this->read_matrix(doc, node, name))
219  nbM++;
220  } else
221  back = SEQUENCE_ERROR;
222  }
223 
224  if (nbM == 0) {
225  back = SEQUENCE_ERROR;
226  vpCERROR << "No Homogeneous matrix is available" << std::endl << "with name: " << name << std::endl;
227  } else if (nbM > 1) {
228  back = SEQUENCE_ERROR;
229  vpCERROR << nbM << " There are more Homogeneous matrix" << std::endl
230  << "with the same name : " << std::endl
231  << "precise your choice..." << std::endl;
232  }
233 
234  return back;
235 }
247 int vpXmlParserHomogeneousMatrix::count(xmlDocPtr doc, xmlNodePtr node, const std::string &name)
248 {
249  // char * val_char;
250  vpXmlCodeType prop;
251  int nbM = 0;
252 
253  for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
254  if (node->type != XML_ELEMENT_NODE)
255  continue;
256  if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
257  prop = CODE_XML_OTHER;
258  }
259  if (prop == CODE_XML_M) {
260  if (SEQUENCE_OK == this->read_matrix(doc, node, name))
261  nbM++;
262  }
263  }
264 
265  return nbM;
266 }
267 
278 int vpXmlParserHomogeneousMatrix::read_matrix(xmlDocPtr doc, xmlNodePtr node, const std::string &name)
279 {
280  vpXmlCodeType prop;
281  /* read value in the XML file. */
282  std::string M_name_tmp = "";
283  vpHomogeneousMatrix M_tmp;
284 
286 
287  for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
288  // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
289  if (node->type != XML_ELEMENT_NODE)
290  continue;
291  if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
292  prop = CODE_XML_OTHER;
293  back = SEQUENCE_ERROR;
294  }
295 
296  switch (prop) {
297  case CODE_XML_M_NAME: {
298  char *val_char = xmlReadCharChild(doc, node);
299  M_name_tmp = val_char;
300  xmlFree(val_char);
301  break;
302  }
303 
304  case CODE_XML_VALUE: // VALUE
305  if (name == M_name_tmp) {
306  std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp << "\"" << std::endl;
307  back = read_values(doc, node, M_tmp);
308  }
309  break;
310 
311  case CODE_XML_BAD:
312  case CODE_XML_OTHER:
313  case CODE_XML_M:
314  case CODE_XML_TX:
315  case CODE_XML_TY:
316  case CODE_XML_TZ:
317  case CODE_XML_TUX:
318  case CODE_XML_TUY:
319  case CODE_XML_TUZ:
320 
321  default:
322  back = SEQUENCE_ERROR;
323  break;
324  }
325  }
326 
327  if (!(name == M_name_tmp)) {
328  back = SEQUENCE_ERROR;
329  } else {
330  this->m_M = M_tmp;
331  // std::cout << "Convert in Homogeneous Matrix:"<< std::endl;
332  // std::cout << this-> M << std::endl;
333  this->m_name = M_name_tmp;
334  }
335  return back;
336 }
337 
349 vpXmlParserHomogeneousMatrix::read_values(xmlDocPtr doc, xmlNodePtr node, vpHomogeneousMatrix &M)
350 {
351  // counter of the number of read parameters
352  int nb = 0;
353  vpXmlCodeType prop;
354  /* read value in the XML file. */
355 
356  double tx_ = 0.;
357  double ty_ = 0.;
358  double tz_ = 0.;
359  double tux_ = 0.;
360  double tuy_ = 0.;
361  double tuz_ = 0.;
362 
364  // int validation = 0;
365 
366  for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
367  // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
368  if (node->type != XML_ELEMENT_NODE)
369  continue;
370  if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
371  prop = CODE_XML_OTHER;
372  back = SEQUENCE_ERROR;
373  }
374 
375  switch (prop) {
376 
377  case CODE_XML_TX:
378  tx_ = xmlReadDoubleChild(doc, node);
379  nb++;
380  break;
381  case CODE_XML_TY:
382  ty_ = xmlReadDoubleChild(doc, node);
383  nb++;
384  break;
385  case CODE_XML_TZ:
386  tz_ = xmlReadDoubleChild(doc, node);
387  nb++;
388  break;
389  case CODE_XML_TUX:
390  tux_ = xmlReadDoubleChild(doc, node);
391  nb++;
392  break;
393  case CODE_XML_TUY:
394  tuy_ = xmlReadDoubleChild(doc, node);
395  nb++;
396  break;
397  case CODE_XML_TUZ:
398  tuz_ = xmlReadDoubleChild(doc, node);
399  nb++;
400  break;
401 
402  case CODE_XML_BAD:
403  case CODE_XML_OTHER:
404  case CODE_XML_M:
405  case CODE_XML_M_NAME:
406  case CODE_XML_VALUE:
407 
408  default:
409  back = SEQUENCE_ERROR;
410  break;
411  }
412  }
413 
414  if (nb != 6) {
415  vpCERROR << "ERROR in 'model' field:\n";
416  vpCERROR << "it must contain 6 parameters\n";
417 
418  return SEQUENCE_ERROR;
419  }
420 
421  // Create the Homogeneous matrix
422  M.buildFrom(tx_, ty_, tz_, tux_, tuy_, tuz_);
423 
424  // std::cout << "Read values from file:" << std::endl;
425  // std::cout << "tx:" << tx_<< std::endl;
426  // std::cout << "ty:" << ty_<< std::endl;
427  // std::cout << "tz:" << tz_<< std::endl;
428  // std::cout << "tux:" << tux_<< std::endl;
429  // std::cout << "tuy:" << tuy_<< std::endl;
430  // std::cout << "tuz:" << tuz_<< std::endl;
431 
432  return back;
433 }
434 
444 int vpXmlParserHomogeneousMatrix::write(xmlNodePtr node, const std::string &name)
445 {
446  int back = SEQUENCE_OK;
447 
448  xmlNodePtr node_tmp;
449  xmlNodePtr node_matrix;
450  xmlNodePtr node_values;
451 
452  // Convert from Rotational matrix to Theta-U vector
454  m_M.extract(R);
455 
456  vpThetaUVector tu(R);
457 
458  // <homogeneous_transformation>
459  node_tmp = xmlNewComment((xmlChar *)"Homogeneous Matrix");
460  xmlAddChild(node, node_tmp);
461  node_matrix = xmlNewNode(NULL, (xmlChar *)LABEL_XML_M);
462  xmlAddChild(node, node_matrix);
463  {
464  //<name>
465 
466  if (!name.empty()) {
467  node_tmp = xmlNewComment((xmlChar *)"Name of the homogeneous matrix");
468  xmlAddChild(node_matrix, node_tmp);
469  xmlNewTextChild(node_matrix, NULL, (xmlChar *)LABEL_XML_M_NAME, (xmlChar *)name.c_str());
470  }
471 
472  //<values>
473 
474  node_values = xmlNewNode(NULL, (xmlChar *)LABEL_XML_VALUE);
475  xmlAddChild(node_matrix, node_values);
476  {
477  char str[11];
478 
479  node_tmp = xmlNewComment((xmlChar *)"Translation vector with values in meters");
480  xmlAddChild(node_values, node_tmp);
481 
482  //<tx>
483  sprintf(str, "%f", m_M[0][3]);
484  xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TX, (xmlChar *)str);
485 
486  //<ty>
487  sprintf(str, "%f", m_M[1][3]);
488  xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TY, (xmlChar *)str);
489 
490  //<tz>
491  sprintf(str, "%f", m_M[2][3]);
492  xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TZ, (xmlChar *)str);
493 
494  node_tmp = xmlNewComment((xmlChar *)"Rotational vector expressed in angle axis "
495  "representation with values in radians");
496  xmlAddChild(node_values, node_tmp);
497 
498  //<tux>
499  sprintf(str, "%f", tu[0]);
500  xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUX, (xmlChar *)str);
501 
502  //<tuy>
503  sprintf(str, "%f", tu[1]);
504  xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUY, (xmlChar *)str);
505 
506  //<tuz>
507  sprintf(str, "%f", tu[2]);
508  xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUZ, (xmlChar *)str);
509  }
510  }
511  return back;
512 }
513 
522 vpXmlParserHomogeneousMatrix::vpXmlCodeSequenceType vpXmlParserHomogeneousMatrix::str2xmlcode(char *str,
523  vpXmlCodeType &res)
524 {
525  vpXmlCodeType val_int = CODE_XML_BAD;
527 
528  // DEBUG_TRACE (9, "# Entree :str=%s.", str);
529 
530  if (!strcmp(str, LABEL_XML_M)) {
531  val_int = CODE_XML_M;
532  } else if (!strcmp(str, LABEL_XML_M_NAME)) {
533  val_int = CODE_XML_M_NAME;
534  } else if (!strcmp(str, LABEL_XML_VALUE)) {
535  val_int = CODE_XML_VALUE;
536  } else if (!strcmp(str, LABEL_XML_TX)) {
537  val_int = CODE_XML_TX;
538  } else if (!strcmp(str, LABEL_XML_TY)) {
539  val_int = CODE_XML_TY;
540  } else if (!strcmp(str, LABEL_XML_TZ)) {
541  val_int = CODE_XML_TZ;
542  } else if (!strcmp(str, LABEL_XML_TUX)) {
543  val_int = CODE_XML_TUX;
544  } else if (!strcmp(str, LABEL_XML_TUY)) {
545  val_int = CODE_XML_TUY;
546  } else if (!strcmp(str, LABEL_XML_TUZ)) {
547  val_int = CODE_XML_TUZ;
548  } else {
549  val_int = CODE_XML_OTHER;
550  }
551  res = val_int;
552 
553  return back;
554 }
555 #elif !defined(VISP_BUILD_SHARED_LIBS)
556 // Work arround to avoid warning:
557 // libvisp_core.a(vpXmlParserHomogeneousMatrix.cpp.o) has no symbols
558 void dummy_vpXmlParserHomogeneousMatrix(){};
559 #endif // VISP_HAVE_XML2
int parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
#define vpCERROR
Definition: vpDebug.h:365
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpXmlParserHomogeneousMatrix & operator=(const vpXmlParserHomogeneousMatrix &twinparser)
double xmlReadDoubleChild(xmlDocPtr doc, xmlNodePtr node)
char * xmlReadCharChild(xmlDocPtr doc, xmlNodePtr node)
Implementation of a rotation matrix and operations on such kind of matrices.
This class intends to simplify the creation of xml parser based on the libxml2 third party library...
Definition: vpXmlParser.h:177
int save(const vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
XML parser to load and save an homogeneous matrix in a file.
void extract(vpRotationMatrix &R) const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Implementation of a rotation vector as axis-angle minimal representation.