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