NukleiObservationIO.cpp
Go to the documentation of this file.
1 // (C) Copyright Renaud Detry 2007-2015.
2 // Distributed under the GNU General Public License and under the
3 // BSD 3-Clause License (See accompanying file LICENSE.txt).
4 
5 /** @file */
6 
7 #include <fstream>
8 #include <cassert>
9 #include <algorithm>
10 #include <cmath>
11 #include <boost/tuple/tuple.hpp>
12 
13 #include <nuklei/NukleiObservationIO.h>
14 #include <nuklei/NukleiObservation.h>
15 #ifdef NUKLEI_USE_TICPP
16 #define TIXML_USE_TICPP
17 #include "ticpp.h"
18 #else
19 #include <tinyxml2.h>
20 namespace tt = tinyxml2;
21 #endif
22 
23 namespace nuklei {
24 
25 
26 
27  NukleiReader::NukleiReader(const std::string &observationFileName) :
28  observationFileName_(observationFileName)
29  {
30  NUKLEI_TRACE_BEGIN();
31 #ifdef NUKLEI_USE_TICPP
32  in_ = boost::shared_ptr<ticpp::Document>(new ticpp::Document(observationFileName));
33 #else
34  in_ = boost::shared_ptr<tt::XMLDocument>(new tt::XMLDocument());
35 #endif
36  NUKLEI_TRACE_END();
37  }
38 
39  NukleiReader::~NukleiReader()
40  {
41  }
42 
43 
44  void NukleiReader::init_()
45  {
46  NUKLEI_TRACE_BEGIN();
47 #ifdef NUKLEI_USE_TICPP
48  try {
49  in_->LoadFile();
50  ticpp::Element* kc = in_->FirstChildElement( "kernelCollection" );
51  NUKLEI_ASSERT(kc != NULL);
52  e_ = boost::shared_ptr<ElementIterator>(new ElementIterator( "kernel" ));
53  *e_ = e_->begin(kc);
54  } catch (ticpp::Exception& e) {
55  throw ObservationIOError(e.what());
56  }
57 #else
58  in_->LoadFile(observationFileName_.c_str());
59  if (in_->ErrorID() != 0)
60  throw ObservationIOError("Error parsing with TinyXML-2");
61  tt::XMLElement* kc = in_->FirstChildElement( "kernelCollection" );
62  if (kc == NULL)
63  throw ObservationIOError("Cannot find element kernelCollection");
64  e_ = kc->FirstChildElement( "kernel" );
65 #endif
66  NUKLEI_TRACE_END();
67  }
68 
69  void NukleiReader::reset()
70  {
71  NUKLEI_TRACE_BEGIN();
72  init();
73  NUKLEI_TRACE_END();
74  }
75 
76  NUKLEI_UNIQUE_PTR<Observation> NukleiReader::readObservation_()
77  {
78  NUKLEI_TRACE_BEGIN();
79 #ifdef NUKLEI_USE_TICPP
80  if (!e_) NUKLEI_THROW("Reader does not seem inited.");
81 
82  typedef ticpp::Element* ElementPtr;
83 
84  while (*e_ != e_->end())
85  {
86  ElementIterator kernel = *e_;
87  ++*e_;
88 
89  NUKLEI_UNIQUE_PTR<NukleiObservation> observation(new NukleiObservation);
90 
91  ElementIterator kernelElement;
92  kernelElement = kernelElement.begin(&*kernel);
93  NUKLEI_ASSERT(kernelElement != kernelElement.end());
94 
95  weight_t w = 1;
96 
97  if (kernelElement->Value() == "weight")
98  {
99  kernelElement->GetText(&w);
100  kernelElement++; NUKLEI_ASSERT(kernelElement != kernelElement.end());
101  }
102 
103  Vector3 loc;
104  coord_t loc_h = 0;
105 
106  {
107  NUKLEI_ASSERT(kernelElement->Value() == "location");
108  ElementIterator li;
109  li = li.begin(&*kernelElement);
110  NUKLEI_ASSERT(li != li.end());
111  {
112  ticpp::Element* el = li.Get();
113  NUKLEI_ASSERT(el->Value() == "vector3");
114  std::string ls;
115  el->GetText(&ls);
116  loc = numify<Vector3>(ls);
117  }
118  li++;
119  if (li != li.end())
120  {
121  ticpp::Element* el = li.Get();
122  NUKLEI_ASSERT(el->Value() == "width");
123  std::string ws;
124  el->GetText(&ws);
125  loc_h = numify<coord_t>(ws);
126  li++;
127  NUKLEI_ASSERT(li == li.end());
128  }
129  }
130 
131  kernel::base::ptr k;
132 
133  kernelElement++;
134 
135  if (kernelElement != kernelElement.end())
136  {
137  NUKLEI_ASSERT(kernelElement->Value() == "orientation");
138  std::string domain = kernelElement->GetAttribute("domain");
139  // domain == "se3" is to support files wirtten with a buggy Nuklei build.
140  if (domain == "se3" || domain == "so3")
141  {
142  kernel::se3 se3k;
143  se3k.loc_ = loc;
144  se3k.loc_h_ = loc_h;
145 
146  ElementIterator oi;
147  oi = oi.begin(&*kernelElement);
148  NUKLEI_ASSERT(oi != oi.end());
149  {
150  ticpp::Element* el = &*oi;
151  NUKLEI_ASSERT(el->Value() == "quaternion");
152  std::string os;
153  el->GetText(&os);
154  se3k.ori_ = la::normalized(numify<Quaternion>(os));
155  }
156  oi++;
157  if (oi != oi.end())
158  {
159  ticpp::Element* el = &*oi;
160  NUKLEI_ASSERT(el->Value() == "width");
161  std::string ws;
162  el->GetText(&ws);
163  se3k.ori_h_ = numify<coord_t>(ws);
164  oi++;
165  NUKLEI_ASSERT(oi == oi.end());
166  }
167  k.reset(new kernel::se3(se3k));
168  }
169  else if (domain == "s2p")
170  {
171  kernel::r3xs2p r3xs2pk;
172  r3xs2pk.loc_ = loc;
173  r3xs2pk.loc_h_ = loc_h;
174 
175  ElementIterator oi;
176  oi = oi.begin(&*kernelElement);
177  NUKLEI_ASSERT(oi != oi.end());
178  {
179  ticpp::Element* el = &*oi;
180  NUKLEI_ASSERT(el->Value() == "vector3");
181  std::string os;
182  el->GetText(&os);
183  r3xs2pk.dir_ = la::normalized(numify<Vector3>(os));
184  }
185  oi++;
186  if (oi != oi.end())
187  {
188  ticpp::Element* el = &*oi;
189  NUKLEI_ASSERT(el->Value() == "width");
190  std::string ws;
191  el->GetText(&ws);
192  r3xs2pk.dir_h_ = numify<coord_t>(ws);
193  oi++;
194  NUKLEI_ASSERT(oi == oi.end());
195  }
196  k.reset(new kernel::r3xs2p(r3xs2pk));
197  }
198  else if (domain == "s2")
199  {
200  kernel::r3xs2 r3xs2k;
201  r3xs2k.loc_ = loc;
202  r3xs2k.loc_h_ = loc_h;
203 
204  ElementIterator oi;
205  oi = oi.begin(&*kernelElement);
206  NUKLEI_ASSERT(oi != oi.end());
207  {
208  ticpp::Element* el = &*oi;
209  NUKLEI_ASSERT(el->Value() == "vector3");
210  std::string os;
211  el->GetText(&os);
212  r3xs2k.dir_ = la::normalized(numify<Vector3>(os));
213  }
214  oi++;
215  if (oi != oi.end())
216  {
217  ticpp::Element* el = &*oi;
218  NUKLEI_ASSERT(el->Value() == "width");
219  std::string ws;
220  el->GetText(&ws);
221  r3xs2k.dir_h_ = numify<coord_t>(ws);
222  oi++;
223  NUKLEI_ASSERT(oi == oi.end());
224  }
225  k.reset(new kernel::r3xs2(r3xs2k));
226  }
227  }
228  else
229  {
230  kernel::r3 r3k;
231  r3k.loc_ = loc;
232  r3k.loc_h_ = loc_h;
233  k.reset(new kernel::r3(r3k));
234  }
235  k->setWeight(w);
236 
237  //TwoFingerDescriptor d;
238  //k->setDescriptor(d);
239 
240  observation->setKernel(*k);
241 
242  oc.incLabel("input");
243 
244  return NUKLEI_UNIQUE_PTR<Observation>(NUKLEI_MOVE(observation));
245  }
246 
247  // End of file reached.
248  return NUKLEI_UNIQUE_PTR<Observation>();
249 #else
250  if (!in_->RootElement()) NUKLEI_THROW("Reader does not seem inited.");
251 
252  using namespace tt;
253  while (e_)
254  {
255  XMLElement* kernel = e_;
256  e_ = kernel->NextSiblingElement( "kernel" );
257 
258  NUKLEI_UNIQUE_PTR<NukleiObservation> observation(new NukleiObservation);
259 
260  XMLElement* kernelIter = kernel->FirstChildElement();
261  NUKLEI_ASSERT(kernelIter);
262 
263  weight_t w = 1;
264 
265  if (std::string(kernelIter->Name()) == "weight")
266  {
267  NUKLEI_ASSERT(kernelIter->QueryDoubleText(&w) == 0);
268  kernelIter = kernelIter->NextSiblingElement();
269  NUKLEI_ASSERT(kernelIter);
270  }
271 
272  Vector3 loc;
273  coord_t loc_h = 0;
274 
275  {
276  NUKLEI_ASSERT(std::string(kernelIter->Name()) == "location");
277  XMLElement* li = kernelIter->FirstChildElement();
278  NUKLEI_ASSERT(li);
279  {
280  NUKLEI_ASSERT(std::string(li->Name()) == "vector3");
281  loc = numify<Vector3>(li->GetText());
282  }
283  li = li->NextSiblingElement();
284  if (li)
285  {
286  NUKLEI_ASSERT(std::string(li->Name()) == "width");
287  NUKLEI_ASSERT(li->QueryDoubleText(&loc_h) == 0);
288  NUKLEI_ASSERT(li->NextSiblingElement() == NULL);
289  }
290  }
291 
292  kernel::base::ptr k;
293 
294  kernelIter = kernelIter->NextSiblingElement();
295 
296  if (kernelIter)
297  {
298  NUKLEI_ASSERT(std::string(kernelIter->Value()) == "orientation");
299  std::string domain;
300  {
301  const char* tmp = kernelIter->Attribute("domain");
302  NUKLEI_ASSERT(tmp);
303  domain = tmp;
304  }
305  // domain == "se3" is to support files wirtten with a buggy Nuklei build.
306  if (domain == "se3" || domain == "so3")
307  {
308  kernel::se3 se3k;
309  se3k.loc_ = loc;
310  se3k.loc_h_ = loc_h;
311 
312  XMLElement* oi = kernelIter->FirstChildElement();
313  NUKLEI_ASSERT(oi);
314  {
315  NUKLEI_ASSERT(std::string(oi->Name()) == "quaternion");
316  se3k.ori_ = la::normalized(numify<Quaternion>(oi->GetText()));
317  }
318  oi = oi->NextSiblingElement();
319  if (oi)
320  {
321  NUKLEI_ASSERT(std::string(oi->Name()) == "width");
322  NUKLEI_ASSERT(oi->QueryDoubleText(&se3k.ori_h_) == 0);
323  NUKLEI_ASSERT(oi->NextSiblingElement() == NULL);
324  }
325 
326  k.reset(new kernel::se3(se3k));
327  }
328  else if (domain == "s2p")
329  {
330  kernel::r3xs2p r3xs2pk;
331  r3xs2pk.loc_ = loc;
332  r3xs2pk.loc_h_ = loc_h;
333 
334  XMLElement* oi = kernelIter->FirstChildElement();
335  NUKLEI_ASSERT(oi);
336  {
337  NUKLEI_ASSERT(std::string(oi->Name()) == "vector3");
338  r3xs2pk.dir_ = la::normalized(numify<Vector3>(oi->GetText()));
339  }
340  oi = oi->NextSiblingElement();
341  if (oi)
342  {
343  NUKLEI_ASSERT(std::string(oi->Name()) == "width");
344  NUKLEI_ASSERT(oi->QueryDoubleText(&r3xs2pk.dir_h_) == 0);
345  NUKLEI_ASSERT(oi->NextSiblingElement() == NULL);
346  }
347 
348  k.reset(new kernel::r3xs2p(r3xs2pk));
349  }
350  else if (domain == "s2")
351  {
352  kernel::r3xs2 r3xs2k;
353  r3xs2k.loc_ = loc;
354  r3xs2k.loc_h_ = loc_h;
355 
356  XMLElement* oi = kernelIter->FirstChildElement();
357  NUKLEI_ASSERT(oi);
358  {
359  NUKLEI_ASSERT(std::string(oi->Name()) == "vector3");
360  r3xs2k.dir_ = la::normalized(numify<Vector3>(oi->GetText()));
361  }
362  oi = oi->NextSiblingElement();
363  if (oi)
364  {
365  NUKLEI_ASSERT(std::string(oi->Name()) == "width");
366  NUKLEI_ASSERT(oi->QueryDoubleText(&r3xs2k.dir_h_) == 0);
367  NUKLEI_ASSERT(oi->NextSiblingElement() == NULL);
368  }
369 
370  k.reset(new kernel::r3xs2(r3xs2k));
371  }
372  }
373  else
374  {
375  kernel::r3 r3k;
376  r3k.loc_ = loc;
377  r3k.loc_h_ = loc_h;
378  k.reset(new kernel::r3(r3k));
379  }
380  k->setWeight(w);
381 
382  //TwoFingerDescriptor d;
383  //k->setDescriptor(d);
384 
385  observation->setKernel(*k);
386 
387  oc.incLabel("input");
388 
389  return NUKLEI_UNIQUE_PTR<Observation>(NUKLEI_MOVE(observation));
390  }
391  // End of file reached.
392  return NUKLEI_UNIQUE_PTR<Observation>();
393 #endif
394  NUKLEI_TRACE_END();
395  }
396 
397 
398 
399 
400 
401 
402 
403 
404 
405 
406 
407 
408 
409 
410  NukleiWriter::NukleiWriter(const std::string &observationFileName) :
411  observationFileName_(observationFileName), totalWeight_(-1)
412  {
413  NUKLEI_TRACE_BEGIN();
414  NUKLEI_TRACE_END();
415  }
416 
417  NukleiWriter::~NukleiWriter()
418  {
419  }
420 
421  void NukleiWriter::init()
422  {
423  NUKLEI_TRACE_BEGIN();
424 #ifdef NUKLEI_USE_TICPP
425  totalWeight_ = -1;
426  try {
427  out_.reset(new ticpp::Document(observationFileName_));
428  } catch (ticpp::Exception& e) {
429  throw ObservationIOError(e.what());
430  }
431  ticpp::Declaration dec("1.0", "UTF-8", "");
432  out_->InsertEndChild(dec);
433  ticpp::Element kc;
434  kc.SetValue( "kernelCollection" );
435  kc.SetAttribute( "version", "1.0" );
436  kc_ = out_->InsertEndChild(kc)->ToElement();
437 #else
438  totalWeight_ = -1;
439  out_.reset(new tt::XMLDocument());
440 
441  out_->InsertEndChild(out_->NewDeclaration());
442 
443  kc_ = out_->NewElement("kernelCollection");
444  kc_->SetAttribute( "version", "1.0" );
445  out_->InsertEndChild(kc_);
446 #endif
447  NUKLEI_TRACE_END();
448  }
449 
450  void NukleiWriter::reset()
451  {
452  NUKLEI_TRACE_BEGIN();
453  init();
454  NUKLEI_TRACE_END();
455  }
456 
457  void NukleiWriter::writeBuffer()
458  {
459  NUKLEI_TRACE_BEGIN();
460 #ifdef NUKLEI_USE_TICPP
461  out_->SaveFile();
462 #else
463  NUKLEI_ASSERT(out_->SaveFile(observationFileName_.c_str()) == 0);
464 #endif
465  NUKLEI_TRACE_END();
466  }
467 
468 #ifdef NUKLEI_USE_TICPP
469  static ticpp::Element* append(ticpp::Element* parent, const std::string& value)
470  {
471  NUKLEI_TRACE_BEGIN();
472  ticpp::Element child(value);
473  NUKLEI_ASSERT(parent != NULL);
474  return parent->InsertEndChild(child)->ToElement();
475  NUKLEI_TRACE_END();
476  }
477 #else
478  static tt::XMLElement* append(tt::XMLElement* parent, const std::string& value)
479  {
480  NUKLEI_TRACE_BEGIN();
481  NUKLEI_ASSERT(parent != NULL);
482  return parent->InsertEndChild(parent->GetDocument()->NewElement(value.c_str()))->ToElement();
483  NUKLEI_TRACE_END();
484  }
485 #endif
486 
487  void NukleiWriter::writeObservation(const Observation &o)
488  {
489  NUKLEI_TRACE_BEGIN();
490 #ifdef NUKLEI_USE_TICPP
491  if (!out_) NUKLEI_THROW("Writer does not seem inited.");
492 
493  typedef ticpp::Element* ElementPtr;
494  const Observation& observation = dynamic_cast<const Observation&>(o);
495 
496  kernel::base::ptr k = observation.getKernel();
497 
498  ElementPtr kernel = append(kc_, "kernel");
499 
500  if (totalWeight_ == -1) totalWeight_ = k->getWeight();
501  else totalWeight_ += k->getWeight();
502 
503  {
504  ElementPtr w = append(kernel, "weight");
505  w->SetText(stringify(k->getWeight(), PRECISION));
506  }
507  {
508  ElementPtr loc = append(kernel, "location");
509  loc->SetAttribute("domain", "r3");
510  ElementPtr v3 = append(loc, "vector3");
511  v3->SetText(stringify(k->getLoc(), PRECISION));
512  ElementPtr width = append(loc, "width");
513  width->SetText(stringify(k->getLocH(), PRECISION));
514  }
515 
516  if (k->polyType() == kernel::base::SE3)
517  {
518  const kernel::se3& se3k = dynamic_cast<const kernel::se3&>(*k);
519  ElementPtr ori = append(kernel, "orientation");
520  ori->SetAttribute("domain", "so3");
521  ElementPtr q = append(ori, "quaternion");
522  q->SetAttribute("format", "wxyz");
523  q->SetText(stringify(se3k.ori_, PRECISION));
524  ElementPtr width = append(ori, "width");
525  width->SetText(stringify(se3k.ori_h_, PRECISION));
526  }
527  else if (k->polyType() == kernel::base::R3XS2P)
528  {
529  const kernel::r3xs2p& r3xs2pk = dynamic_cast<const kernel::r3xs2p&>(*k);
530  ElementPtr ori = append(kernel, "orientation");
531  ori->SetAttribute("domain", "s2p");
532  ElementPtr q = append(ori, "vector3");
533  q->SetText(stringify(r3xs2pk.dir_, PRECISION));
534  ElementPtr width = append(ori, "width");
535  width->SetText(stringify(r3xs2pk.dir_h_, PRECISION));
536  }
537  else if (k->polyType() == kernel::base::R3XS2)
538  {
539  const kernel::r3xs2& r3xs2k = dynamic_cast<const kernel::r3xs2&>(*k);
540  ElementPtr ori = append(kernel, "orientation");
541  ori->SetAttribute("domain", "s2");
542  ElementPtr q = append(ori, "vector3");
543  q->SetText(stringify(r3xs2k.dir_, PRECISION));
544  ElementPtr width = append(ori, "width");
545  width->SetText(stringify(r3xs2k.dir_h_, PRECISION));
546  }
547 #else
548  if (!out_) NUKLEI_THROW("Writer does not seem inited.");
549 
550  using namespace tt;
551 
552  const Observation& observation = dynamic_cast<const Observation&>(o);
553 
554  kernel::base::ptr k = observation.getKernel();
555 
556  XMLElement* kernel = append(kc_, "kernel");
557 
558  if (totalWeight_ == -1) totalWeight_ = k->getWeight();
559  else totalWeight_ += k->getWeight();
560 
561  {
562  XMLElement* w = append(kernel, "weight");
563  w->SetText(stringify(k->getWeight(), PRECISION).c_str());
564  }
565  {
566  XMLElement* loc = append(kernel, "location");
567  loc->SetAttribute("domain", "r3");
568  XMLElement* v3 = append(loc, "vector3");
569  v3->SetText(stringify(k->getLoc(), PRECISION).c_str());
570  XMLElement* width = append(loc, "width");
571  width->SetText(stringify(k->getLocH(), PRECISION).c_str());
572  }
573 
574  if (k->polyType() == kernel::base::SE3)
575  {
576  const kernel::se3& se3k = dynamic_cast<const kernel::se3&>(*k);
577  XMLElement* ori = append(kernel, "orientation");
578  ori->SetAttribute("domain", "so3");
579  XMLElement* q = append(ori, "quaternion");
580  q->SetAttribute("format", "wxyz");
581  q->SetText(stringify(se3k.ori_, PRECISION).c_str());
582  XMLElement* width = append(ori, "width");
583  width->SetText(stringify(se3k.ori_h_, PRECISION).c_str());
584  }
585  else if (k->polyType() == kernel::base::R3XS2P)
586  {
587  const kernel::r3xs2p& r3xs2pk = dynamic_cast<const kernel::r3xs2p&>(*k);
588  XMLElement* ori = append(kernel, "orientation");
589  ori->SetAttribute("domain", "s2p");
590  XMLElement* q = append(ori, "vector3");
591  q->SetText(stringify(r3xs2pk.dir_, PRECISION).c_str());
592  XMLElement* width = append(ori, "width");
593  width->SetText(stringify(r3xs2pk.dir_h_, PRECISION).c_str());
594  }
595  else if (k->polyType() == kernel::base::R3XS2)
596  {
597  const kernel::r3xs2& r3xs2k = dynamic_cast<const kernel::r3xs2&>(*k);
598  XMLElement* ori = append(kernel, "orientation");
599  ori->SetAttribute("domain", "s2");
600  XMLElement* q = append(ori, "vector3");
601  q->SetText(stringify(r3xs2k.dir_, PRECISION).c_str());
602  XMLElement* width = append(ori, "width");
603  width->SetText(stringify(r3xs2k.dir_h_, PRECISION).c_str());
604  }
605 #endif
606  NUKLEI_TRACE_END();
607  }
608 
609 }
nuklei
Public namespace.
Definition: Color.cpp:9
nuklei::weight_t
double weight_t
Type for particle weights.
Definition: Definitions.h:31
nuklei::stringify
std::string stringify(const T &x, int precision=-1, int width=0)
Converts an object to a std::string using operator<<.
Definition: Common.h:333
nuklei::kernel::r3xs2p
r3xs2_base< groupS::s2p > r3xs2p
Definition: Kernel.h:623
NUKLEI_ASSERT
#define NUKLEI_ASSERT(expression)
Throws an Error if expression is not true.
Definition: Common.h:113
nuklei::kernel::r3xs2
r3xs2_base< groupS::s2 > r3xs2
Definition: Kernel.h:618
nuklei::kernel::base::ptr
NUKLEI_UNIQUE_PTR< kernel::base > ptr
NUKLEI_UNIQUE_PTR for kernel::base.
Definition: Kernel.h:50
nuklei::coord_t
double coord_t
Type for point coordinates.
Definition: Definitions.h:25
NukleiObservationIO.h
NukleiObservation.h
NUKLEI_THROW
#define NUKLEI_THROW(x)
Throws an Error.
Definition: Common.h:94
© Copyright 2007-2013 Renaud Detry.
Distributed under the terms of the GNU General Public License (GPL).
(See accompanying file LICENSE.txt or copy at http://www.gnu.org/copyleft/gpl.html.)
Revised Sun Sep 13 2020 19:10:06.