CoViS3DObservationIO.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/CoViS3DObservationIO.h>
14 #include <nuklei/CoViS3DObservation.h>
15 #ifdef NUKLEI_USE_TICPP
16 #define TIXML_USE_TICPP
17 #include "ticpp.h"
18 #endif
19 
20 namespace nuklei {
21 
22 
23 
24  CoViS3DReader::CoViS3DReader(const std::string &observationFileName) :
25  observationFileName(observationFileName)
26  {
27  NUKLEI_TRACE_BEGIN();
28  NUKLEI_TRACE_END();
29  }
30 
31  CoViS3DReader::~CoViS3DReader()
32  {
33  }
34 
35  CoViS3DXMLReader::CoViS3DXMLReader(const std::string &observationFileName) :
36  CoViS3DReader(observationFileName)
37  {
38  NUKLEI_TRACE_BEGIN();
39 #ifdef NUKLEI_USE_TICPP
40  in_ = boost::shared_ptr<ticpp::Document>(new ticpp::Document(observationFileName));
41 #else
42  NUKLEI_THROW("This function requires TICPP.");
43 #endif
44  NUKLEI_TRACE_END();
45  }
46 
47  CoViS3DXMLReader::~CoViS3DXMLReader()
48  {
49  }
50 
51  CoViS3DWandererReader::CoViS3DWandererReader(const std::string &observationFileName) :
52  CoViS3DReader(observationFileName)
53  {
54  NUKLEI_TRACE_BEGIN();
55  NUKLEI_TRACE_END();
56  }
57 
58  CoViS3DWandererReader::~CoViS3DWandererReader()
59  {
60  }
61 
62 
63  void CoViS3DReader::init_()
64  {
65  NUKLEI_TRACE_BEGIN();
66  std::string errorsCat = std::string("Error in CoViS3DReader::init.") +
67  "\nErrors at each format attempt were:";
68 
69  try {
70  reader_ = boost::shared_ptr<CoViS3DReader>(new CoViS3DXMLReader(observationFileName));
71  reader_->init();
72  return;
73  } catch (ObservationIOError &e) {
74  errorsCat += "\n" + std::string(e.what());
75  }
76  try {
77  reader_ = boost::shared_ptr<CoViS3DReader>(new CoViS3DWandererReader(observationFileName));
78  reader_->init();
79  return;
80  } catch (ObservationIOError &e) {
81  errorsCat += "\n" + std::string(e.what());
82  }
83  throw ObservationIOError(errorsCat);
84  NUKLEI_TRACE_END();
85  }
86 
87  void CoViS3DXMLReader::init_()
88  {
89  NUKLEI_TRACE_BEGIN();
90 #ifdef NUKLEI_USE_TICPP
91  try {
92  in_->LoadFile();
93  ticpp::Element* scene = in_->FirstChildElement( "Scene" );
94  NUKLEI_ASSERT(scene != NULL);
95  e_ = boost::shared_ptr<ElementIterator>(new ElementIterator( "Primitive3D" ));
96  *e_ = e_->begin(scene);
97  } catch (ticpp::Exception& e) {
98  throw ObservationIOError(e.what());
99  }
100 #else
101  NUKLEI_THROW("This function requires TICPP.");
102 #endif
103  NUKLEI_TRACE_END();
104  }
105 
106  void CoViS3DWandererReader::init_()
107  {
108  NUKLEI_TRACE_BEGIN();
109  NUKLEI_ASSERT(!in_.is_open());
110  in_.open(observationFileName.c_str(), std::ios::in);
111  if (!in_.is_open())
112  throw ObservationIOError(std::string("Could not open file ") +
113  observationFileName + " for reading.");
114  int c = in_.peek();
115  if (c != 'l')
116  {
117  in_.close();
118  throw ObservationIOError
119  (std::string("Unexpected first char `") +
120  char(c) +
121  "' in file " + observationFileName + ".");
122  }
123  NUKLEI_TRACE_END();
124  }
125 
126 
127  void CoViS3DReader::reset()
128  {
129  NUKLEI_TRACE_BEGIN();
130  init();
131  NUKLEI_TRACE_END();
132  }
133 
134  void CoViS3DXMLReader::reset()
135  {
136  NUKLEI_TRACE_BEGIN();
137  init();
138  NUKLEI_TRACE_END();
139  }
140 
141  void CoViS3DWandererReader::reset()
142  {
143  NUKLEI_TRACE_BEGIN();
144  in_.close();
145  init();
146  NUKLEI_TRACE_END();
147  }
148 
149  NUKLEI_UNIQUE_PTR<Observation> CoViS3DReader::readObservation_()
150  {
151  NUKLEI_TRACE_BEGIN();
152  return reader_->readObservation_();
153  NUKLEI_TRACE_END();
154  }
155 
156  NUKLEI_UNIQUE_PTR<Observation> CoViS3DXMLReader::readObservation_()
157  {
158  NUKLEI_TRACE_BEGIN();
159 #ifdef NUKLEI_USE_TICPP
160  if (!e_) NUKLEI_THROW("Reader does not seem inited.");
161 
162  typedef ticpp::Element* ElementPtr;
163 
164  while (*e_ != e_->end())
165  {
166  ElementIterator primitive = *e_;
167  ++*e_;
168 
169  NUKLEI_UNIQUE_PTR<CoViS3DObservation> observation(new CoViS3DObservation);
170 
171  ElementIterator primitiveElement;
172  primitiveElement = primitiveElement.begin(&*primitive);
173  NUKLEI_ASSERT(primitiveElement != primitiveElement.end());
174 
175  while (primitiveElement->Value() != "Location")
176  {
177  primitiveElement++; NUKLEI_ASSERT(primitiveElement != primitiveElement.end());
178  }
179 
180  {
181  NUKLEI_ASSERT(primitiveElement->Value() == "Location");
182  {
183  ticpp::Element* el = primitiveElement->FirstChildElement("Cartesian3D");
184  Vector3 l;
185  el->GetAttribute("x", &l[0]);
186  el->GetAttribute("y", &l[1]);
187  el->GetAttribute("z", &l[2]);
188  observation->setLoc(l);
189  }
190  {
191  ticpp::Element* el = primitiveElement->FirstChildElement("Cartesian3DCovariance");
192  Matrix3 cov;
193  std::string covString;
194  el->GetText(&covString);
195  std::istringstream iss(covString);
196  for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j)
197  NUKLEI_ASSERT(iss >> cov(i,j));
198  observation->setCovMatrix(cov);
199  }
200  }
201 
202  primitiveElement++; NUKLEI_ASSERT(primitiveElement != primitiveElement.end());
203 
204  while (primitiveElement->Value() != "Orientation")
205  {
206  primitiveElement++; NUKLEI_ASSERT(primitiveElement != primitiveElement.end());
207  }
208 
209  {
210  NUKLEI_ASSERT(primitiveElement->Value() == "Orientation");
211  ElementPtr dirGamma = primitiveElement->FirstChildElement("DirGammaOrientation");
212  {
213  ElementPtr el = dirGamma->FirstChildElement("Direction");
214  el = el->FirstChildElement("Spherical");
215  coord_t phi, psi;
216  el->GetAttribute("phi", &phi);
217  el->GetAttribute("psi", &psi);
218  observation->setPhiPsi(phi, psi);
219  }
220  {
221  ElementPtr el = dirGamma->FirstChildElement("GammaVector");
222  el = el->FirstChildElement("Cartesian3D");
223  Vector3 l;
224  el->GetAttribute("x", &l[0]);
225  el->GetAttribute("y", &l[1]);
226  el->GetAttribute("z", &l[2]);
227  observation->setGamma(l);
228  }
229  }
230 
231  primitiveElement++; NUKLEI_ASSERT(primitiveElement != primitiveElement.end());
232 
233  //IntrinsicDimensionality
234 
235  primitiveElement++; NUKLEI_ASSERT(primitiveElement != primitiveElement.end());
236 
237  //Source2D?
238 
239  if (primitiveElement->Value() == "Source2D")
240  primitiveElement++; NUKLEI_ASSERT(primitiveElement != primitiveElement.end());
241 
242  if (false)
243  {
244  NUKLEI_ASSERT(primitiveElement->Value() == "Phase");
245  ticpp::Element* el = primitiveElement->FirstChildElement("Angle");
246  appear_t phase;
247  el->GetText(&phase);
248  //observation->setPhase(phase);
249  }
250 
251  primitiveElement++; NUKLEI_ASSERT(primitiveElement != primitiveElement.end());
252 
253  {
254  NUKLEI_ASSERT(primitiveElement->Value() == "Colors");
255  RGBColor::ptr color(new RGBColor);
256  {
257  ElementPtr el = primitiveElement->FirstChildElement("Left");
258  el = el->FirstChildElement("RGB");
259  el->GetAttribute("r", &color->R());
260  el->GetAttribute("g", &color->G());
261  el->GetAttribute("b", &color->B());
262  observation->setLeftColor(*color);
263  }
264  {
265  ElementPtr el = primitiveElement->FirstChildElement("Right");
266  el = el->FirstChildElement("RGB");
267  el->GetAttribute("r", &color->R());
268  el->GetAttribute("g", &color->G());
269  el->GetAttribute("b", &color->B());
270  observation->setRightColor(*color);
271  }
272  if (false)
273  {
274  ElementPtr el = primitiveElement->FirstChildElement("Middle");
275  el = el->FirstChildElement("RGB");
276  el->GetAttribute("r", &color->R());
277  el->GetAttribute("g", &color->G());
278  el->GetAttribute("b", &color->B());
279  //observation->setMiddleColor(color);
280  }
281  }
282 
283  oc.incLabel("input");
284 
285  return NUKLEI_UNIQUE_PTR<Observation>(NUKLEI_MOVE(observation));
286  }
287 
288  // End of file reached.
289  return NUKLEI_UNIQUE_PTR<Observation>();
290 #else
291  NUKLEI_THROW("This function requires TICPP.");
292 #endif
293  NUKLEI_TRACE_END();
294  }
295 
296  NUKLEI_UNIQUE_PTR<Observation> CoViS3DWandererReader::readObservation_()
297  {
298  NUKLEI_TRACE_BEGIN();
299  if (!in_.is_open()) NUKLEI_THROW("Reader does not seem inited.");
300  float shaft;
301 
302  NUKLEI_ASSERT(in_.good());
303 
304  while (in_.peek() != EOF)
305  {
306  NUKLEI_ASSERT(in_.good());
307 
308  NUKLEI_UNIQUE_PTR<CoViS3DObservation> observation(new CoViS3DObservation);
309 
310  // Careful: LS.normal_vector_plane_C is a Vector!
311 
312  std::string s;
313  Vector3 l;
314  NUKLEI_ASSERT(in_ >> s && s == "l");
315 
316  //2 3 4: LS.X() LS.Y() LS.Z()
317  for (int d = 0; d < 3; d++)
318  NUKLEI_ASSERT(in_ >> l[d]);
319  observation->setLoc(l);
320 
321  //5 6: LS.len LS.conf()
322  //NUKLEI_ASSERT( in_ >> shaft && in_ >> shaft );
323 
324  //7 8 9: LS.confIDim0C LS.confIDim1C LS.confIDim2C
325  {
326  appear_t c0, c1, c2;
327  NUKLEI_ASSERT(in_ >> c0 >> c1 >> c2);
328  //observation->setIntrinsicDim(c0, c1, c2);
329  }
330  // confidence_C
331  NUKLEI_ASSERT(in_ >> shaft);
332 
333 
334  //11 12 13: LS.normal_vector_plane_C
335  for (int d = 0; d < 3; d++)
336  NUKLEI_ASSERT(in_ >> l[d]);
337  //observation->setGamma(l);
338 
339 
340  // length_C
341  NUKLEI_ASSERT(in_ >> shaft);
342 
343 
344  //0: LS.source2DLineSegmentsC.at(0)
345  //1: LS.source2DLineSegmentsC.at(1)
346  NUKLEI_ASSERT(in_ >> shaft && in_ >> shaft);
347 
348  //10: LS.deviation1PixelC
349  NUKLEI_ASSERT(in_ >> shaft);
350 
351 
352  // 14 15 16: 3 x dummy
353  //directionVectorLineSegmentL[0]
354  //directionVectorLineSegmentL[1]
355  //directionVectorLineSegmentL[2]
356  Vector3 dummy;
357  NUKLEI_ASSERT(in_ >> dummy[0] && in_ >> dummy[1] && in_ >> dummy[2]);
358 
359 
360  //17 18 19: LS.PhiRad LS.PsiRad LS.oriConfC
361  {
362  coord_t phi, psi;
363  NUKLEI_ASSERT(in_ >> phi && in_ >> psi && in_ >> shaft);
364  observation->setPhiPsi(phi, psi);
365  }
366 
367  NUKLEI_ASSERT(rfe(observation->getDirection().Dot(dummy), 1));
368 
369  //20 21: LS.phaseC LS.dPhaseConf
370  {
371  appear_t phase;
372  NUKLEI_ASSERT(in_ >> phase && in_ >> shaft );
373  //observation->setPhase(phase);
374  }
375 
376  //LS.colorTripletLeft().at(0)
377  //LS.colorTripletLeft().at(1)
378  //LS.colorTripletLeft().at(2)
379  //LS.colConfL()
380  //
381  //LS.colorTripletMiddle().at(0)
382  //LS.colorTripletMiddle().at(1)
383  //LS.colorTripletMiddle().at(2)
384  //LS.colConfM()
385  //
386  //LS.colorTripletRight().at(0)
387  //LS.colorTripletRight().at(1)
388  //LS.colorTripletRight().at(2)
389  //LS.colConfR()
390 
391 
392  RGBColor::ptr color(new RGBColor);
393  for (int d = 0; d < 3; d++)
394  NUKLEI_ASSERT(in_ >> color->at(d));
395  observation->setLeftColor(*color);
396  NUKLEI_ASSERT(in_ >> shaft);
397  for (int d = 0; d < 3; d++)
398  NUKLEI_ASSERT(in_ >> color->at(d));
399  //observation->setMiddleColor(color);
400  NUKLEI_ASSERT(in_ >> shaft);
401  for (int d = 0; d < 3; d++)
402  NUKLEI_ASSERT(in_ >> color->at(d));
403  observation->setRightColor(*color);
404  NUKLEI_ASSERT(in_ >> shaft);
405 
406  char c;
407  NUKLEI_ASSERT(in_.get(c));
408  NUKLEI_ASSERT(c == '\t');
409  NUKLEI_ASSERT(in_.get(c));
410  NUKLEI_ASSERT(c == '\n');
411 
412  NUKLEI_ASSERT( in_.good() );
413 
414  oc.incLabel("input");
415 
416  return NUKLEI_UNIQUE_PTR<Observation>(NUKLEI_MOVE(observation));
417  }
418 
419  // End of file reached.
420  return NUKLEI_UNIQUE_PTR<Observation>();
421  NUKLEI_TRACE_END();
422  }
423 
424  CoViS3DXMLWriter::CoViS3DXMLWriter(const std::string &observationFileName) :
425  observationFileName(observationFileName)
426  {
427  }
428 
429  CoViS3DXMLWriter::~CoViS3DXMLWriter()
430  {
431  }
432 
433  void CoViS3DXMLWriter::init()
434  {
435  NUKLEI_TRACE_BEGIN();
436 #ifdef NUKLEI_USE_TICPP
437  try {
438  out_.reset(new ticpp::Document(observationFileName));
439  ticpp::Declaration dec("1.0", "UTF-8", "");
440  out_->InsertEndChild(dec);
441  ticpp::Element scene;
442  scene.SetValue( "Scene" );
443  scene.SetAttribute( "version", "1.0" );
444  scene_ = out_->InsertEndChild(scene)->ToElement();
445  } catch (ticpp::Exception& e) {
446  throw ObservationIOError(e.what());
447  }
448 #else
449  NUKLEI_THROW("This function requires TICPP.");
450 #endif
451  NUKLEI_TRACE_END();
452  }
453 
454  void CoViS3DXMLWriter::reset()
455  {
456  NUKLEI_TRACE_BEGIN();
457  init();
458  NUKLEI_TRACE_END();
459  }
460 
461  void CoViS3DXMLWriter::writeBuffer()
462  {
463  NUKLEI_TRACE_BEGIN();
464 #ifdef NUKLEI_USE_TICPP
465  out_->SaveFile();
466 #else
467  NUKLEI_THROW("This function requires TICPP.");
468 #endif
469  NUKLEI_TRACE_END();
470  }
471 
472 #ifdef NUKLEI_USE_TICPP
473  static ticpp::Element* append(ticpp::Element* parent, const std::string& value)
474  {
475  NUKLEI_TRACE_BEGIN();
476  ticpp::Element child(value);
477  return parent->InsertEndChild(child)->ToElement();
478  NUKLEI_TRACE_END();
479  }
480 #endif
481 
482  void CoViS3DXMLWriter::writeObservation(const Observation &o)
483  {
484  NUKLEI_TRACE_BEGIN();
485 #ifdef NUKLEI_USE_TICPP
486  if (!out_) NUKLEI_THROW("Writer does not seem inited.");
487 
488  typedef ticpp::Element* ElementPtr;
489  const CoViS3DObservation& observation = dynamic_cast<const CoViS3DObservation&>(o);
490 
491  ElementPtr primitive = append(scene_, "Primitive3D");
492  primitive->SetAttribute("type", "l");
493  primitive->SetAttribute("confidence", observation.getWeight());
494  primitive->SetAttribute("length", 4);
495 
496  {
497  ElementPtr loc = append(primitive, "Location");
498  ElementPtr el = append(loc, "Cartesian3D");
499  Vector3 l = observation.getLoc();
500  Matrix3 cov;
501  if (observation.getCovMatrix().isDefined()) cov = *observation.getCovMatrix();
502  else cov = Matrix3::IDENTITY;
503  el->SetAttribute("x", l[0]);
504  el->SetAttribute("y", l[1]);
505  el->SetAttribute("z", l[2]);
506  el = append(loc, "Cartesian3DCovariance");
507  el->SetText(stringify(Vector3(cov.GetRow(0))) + " " +
508  stringify(Vector3(cov.GetRow(1))) + " " +
509  stringify(Vector3(cov.GetRow(2))));
510  }
511 
512  {
513  ElementPtr ori = append(primitive, "Orientation");
514  ElementPtr dirGamma = append(ori, "DirGammaOrientation");
515  {
516  ElementPtr el = append(append(dirGamma, "GammaVector"), "Cartesian3D");
517  kernel::se3::ptr se3k = observation.getKernel()->polySe3Proj();
518  Matrix3 ori;
519  la::copyRotation(ori, se3k->ori_);
520  Vector3 l = ori.GetColumn(1);
521  //Vector3 l = observation.getGamma();
522  el->SetAttribute("x", l[0]);
523  el->SetAttribute("y", l[1]);
524  el->SetAttribute("z", l[2]);
525  }
526  {
527  ElementPtr dir = append(dirGamma, "Direction");
528  ElementPtr el = append(dir, "Spherical");
529  coord_t phi, psi;
530  boost::tie(phi, psi) = observation.getPhiPsi();
531  el->SetAttribute("phi", phi);
532  el->SetAttribute("psi", psi);
533  append(dir, "Conf")->SetText(-1);
534  }
535  append(ori, "Cartesian3DCovariance")->SetText
536  (stringify(Vector3::UNIT_X) + " " +
537  stringify(Vector3::UNIT_Y) + " " +
538  stringify(Vector3::UNIT_Z));
539  }
540 
541  {
542  ElementPtr el = append(append(primitive, "IntrinsicDimensionality"),
543  "Barycentric");
544  el->SetAttribute("c0", 0);
545  el->SetAttribute("c1", 1);
546  el->SetAttribute("c2", 0);
547  }
548 
549  {
550  // Source2D is OPTIONAL in MoInSScene.rng
551  // However CoViS crashes if it doesn't find it.
552  ElementPtr phase = append(primitive, "Source2D");
553  ElementPtr first = append(phase, "First");
554  first->SetText(0);
555  ElementPtr second = append(phase, "Second");
556  second->SetText(0);
557  }
558 
559  {
560  ElementPtr phase = append(primitive, "Phase");
561  ElementPtr el = append(phase, "Angle");
562  el->SetText(0);
563  //el->SetText(observation.getPhase());
564  el = append(phase, "Conf");
565  el->SetText(0);
566  }
567 
568  {
569  ElementPtr colors = append(primitive, "Colors");
570  {
571  const RGBColor c(observation.getLeftColor());
572  ElementPtr side = append(colors, "Left");
573  ElementPtr rgb = append(side, "RGB");
574  rgb->SetAttribute("r", c.at(0));
575  rgb->SetAttribute("g", c.at(1));
576  rgb->SetAttribute("b", c.at(2));
577  append(side, "Conf")->SetText(1);
578  }
579  {
580  const RGBColor c(observation.getRightColor());
581  ElementPtr side = append(colors, "Right");
582  ElementPtr rgb = append(side, "RGB");
583  rgb->SetAttribute("r", c.at(0));
584  rgb->SetAttribute("g", c.at(1));
585  rgb->SetAttribute("b", c.at(2));
586  append(side, "Conf")->SetText(1);
587  }
588  {
589  ElementPtr side = append(colors, "Middle");
590  ElementPtr rgb = append(side, "RGB");
591  rgb->SetAttribute("r", 0);
592  rgb->SetAttribute("g", 0);
593  rgb->SetAttribute("b", 0);
594  append(side, "Conf")->SetText(1);
595  }
596 
597  }
598 #else
599  NUKLEI_THROW("This function requires TICPP.");
600 #endif
601  NUKLEI_TRACE_END();
602  }
603 
604 }
nuklei
Public namespace.
Definition: Color.cpp:9
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_ASSERT
#define NUKLEI_ASSERT(expression)
Throws an Error if expression is not true.
Definition: Common.h:113
nuklei::coord_t
double coord_t
Type for point coordinates.
Definition: Definitions.h:25
nuklei::appear_t
double appear_t
Type for appearance-related values (e.g., color)
Definition: Definitions.h:29
CoViS3DObservationIO.h
CoViS3DObservation.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.