Visual Servoing Platform version 3.5.0
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
65class vpXmlParserHomogeneousMatrix::Impl
66{
67private:
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
84public:
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 = "";
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
494private:
496 std::string m_name;
497};
498#endif //DOXYGEN_SHOULD_SKIP_THIS
499
501{
502}
503
505{
506 delete m_impl;
507}
508
517int vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name)
518{
519 return m_impl->parse(M, filename, name);
520}
521
530int 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}
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(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)