Main MRPT website > C++ reference for MRPT 1.5.9
VelodyneCalibration.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
13 #include <mrpt/utils/bits.h>
14 #include <iostream>
15 #include <map>
16 
17 #undef _UNICODE // JLBC, for xmlParser
18 #include "xmlparser/xmlParser.h"
19 
20 // ======= Default calibration files ========================
23 // ======= End of default calibration files =================
24 
25 
26 using namespace std;
27 using namespace mrpt::obs;
28 
29 VelodyneCalibration::PerLaserCalib::PerLaserCalib() :
30  azimuthCorrection (.0),
31  verticalCorrection (.0),
32  distanceCorrection (.0),
33  verticalOffsetCorrection (.0),
34  horizontalOffsetCorrection (.0),
35  sinVertCorrection (.0),
36  cosVertCorrection (1.0),
37  sinVertOffsetCorrection (.0),
38  cosVertOffsetCorrection (1.0)
39 {
40 }
41 
44 {
45 }
46 
48 {
49  XMLNode &root = *reinterpret_cast<XMLNode*>(node);
50 
51  XMLNode node_bs = root.getChildNode("boost_serialization");
52  if (node_bs.isEmpty()) throw std::runtime_error("Cannot find XML node: 'boost_serialization'");
53 
54  XMLNode node_DB = node_bs.getChildNode("DB");
55  if (node_DB.isEmpty()) throw std::runtime_error("Cannot find XML node: 'DB'");
56 
57  XMLNode node_enabled_ = node_DB.getChildNode("enabled_");
58  if (node_enabled_.isEmpty()) throw std::runtime_error("Cannot find XML node: 'enabled_'");
59 
60  // Clear previous contents:
61  clear();
62 
63  XMLNode node_enabled_count = node_enabled_.getChildNode("count");
64  if (node_enabled_count.isEmpty()) throw std::runtime_error("Cannot find XML node: 'enabled_::count'");
65  const int nEnabled = atoi( node_enabled_count.getText() );
66  if (nEnabled<=0 || nEnabled>10000)
67  throw std::runtime_error("Senseless value found reading 'enabled_::count'");
68 
69  int enabledCount = 0;
70  for (int i=0;i<nEnabled;i++)
71  {
72  XMLNode node_enabled_ith = node_enabled_.getChildNode("item",i);
73  if (node_enabled_ith.isEmpty()) throw std::runtime_error("Cannot find the expected number of XML nodes: 'enabled_::item'");
74  const int enable_val = atoi( node_enabled_ith.getText() );
75  if (enable_val)
76  ++enabledCount;
77  }
78 
79  // enabledCount = number of lasers in the LIDAR
80  this->laser_corrections.resize(enabledCount);
81 
82  XMLNode node_points_ = node_DB.getChildNode("points_");
83  if (node_points_.isEmpty()) throw std::runtime_error("Cannot find XML node: 'points_'");
84 
85  for (int i=0; ; ++i)
86  {
87  XMLNode node_points_item = node_points_.getChildNode("item",i);
88  if (node_points_item.isEmpty()) break;
89 
90  XMLNode node_px = node_points_item.getChildNode("px");
91  if (node_px.isEmpty()) throw std::runtime_error("Cannot find XML node: 'points_::item::px'");
92 
93  XMLNode node_px_id = node_px.getChildNode("id_");
94  if (node_px_id.isEmpty()) throw std::runtime_error("Cannot find XML node: 'points_::item::px::id_'");
95  const int id = atoi( node_px_id.getText() );
96  ASSERT_ABOVEEQ_(id,0);
97  if (id>=enabledCount)
98  continue; // ignore
99 
101 
102  {
103  XMLNode node = node_px.getChildNode("rotCorrection_");
104  if (node.isEmpty()) throw std::runtime_error("Cannot find XML node: 'points_::item::px::rotCorrection_'");
105  plc->azimuthCorrection = atof( node.getText() );
106  }
107  {
108  XMLNode node = node_px.getChildNode("vertCorrection_");
109  if (node.isEmpty()) throw std::runtime_error("Cannot find XML node: 'points_::item::px::vertCorrection_'");
110  plc->verticalCorrection = atof( node.getText() );
111  }
112  {
113  XMLNode node = node_px.getChildNode("distCorrection_");
114  if (node.isEmpty()) throw std::runtime_error("Cannot find XML node: 'points_::item::px::distCorrection_'");
115  plc->distanceCorrection = 0.01f * atof( node.getText() );
116  }
117  {
118  XMLNode node = node_px.getChildNode("vertOffsetCorrection_");
119  if (node.isEmpty()) throw std::runtime_error("Cannot find XML node: 'points_::item::px::vertOffsetCorrection_'");
120  plc->verticalOffsetCorrection = 0.01f * atof( node.getText() );
121  }
122  {
123  XMLNode node = node_px.getChildNode("horizOffsetCorrection_");
124  if (node.isEmpty()) throw std::runtime_error("Cannot find XML node: 'points_::item::px::horizOffsetCorrection_'");
125  plc->horizontalOffsetCorrection = 0.01f * atof( node.getText() );
126  }
127 
128  plc->sinVertCorrection = std::sin( mrpt::utils::DEG2RAD( plc->verticalCorrection ) );
129  plc->cosVertCorrection = std::cos( mrpt::utils::DEG2RAD( plc->verticalCorrection ) );
130 
133  }
134 
135  return true; // Ok
136 }
137 
138 bool VelodyneCalibration::loadFromXMLText(const std::string & xml_file_contents)
139 {
140  try
141  {
142  XMLResults results;
143  XMLNode root = XMLNode::parseString(xml_file_contents.c_str(), NULL, &results );
144 
145  if (results.error != eXMLErrorNone) {
146  cerr << "[VelodyneCalibration::loadFromXMLText] Error parsing XML content: " <<
147  XMLNode::getError( results.error ) << " at line " << results.nLine << ":" << results.nColumn << endl;
148  return false;
149  }
150 
151  return internal_loadFromXMLNode( reinterpret_cast<void*>(&root) );
152  }
153  catch (exception &e)
154  {
155  cerr << "[VelodyneCalibration::loadFromXMLFile] Exception:" << endl << e.what() << endl;
156  return false;
157  }
158 }
159 
160 /** Loads calibration from file. \return false on any error, true on success */
161 // See reference code in: vtkVelodyneHDLReader::vtkInternal::LoadCorrectionsFile()
162 bool VelodyneCalibration::loadFromXMLFile(const std::string & velodyne_calibration_xml_filename)
163 {
164  try
165  {
166  XMLResults results;
167  XMLNode root = XMLNode::parseFile( velodyne_calibration_xml_filename.c_str(), NULL, &results );
168 
169  if (results.error != eXMLErrorNone) {
170  cerr << "[VelodyneCalibration::loadFromXMLFile] Error loading XML file: " <<
171  XMLNode::getError( results.error ) << " at line " << results.nLine << ":" << results.nColumn << endl;
172  return false;
173  }
174  return internal_loadFromXMLNode( reinterpret_cast<void*>(&root) );
175  }
176  catch (exception &e)
177  {
178  cerr << "[VelodyneCalibration::loadFromXMLFile] Exception:" << endl << e.what() << endl;
179  return false;
180  }
181 }
182 
184  return laser_corrections.empty();
185 }
186 
188  laser_corrections.clear();
189 }
190 
191 
192 std::map<std::string,VelodyneCalibration> cache_default_calibs;
193 
194 // It always return a calibration structure, but it may be empty if the model name is unknown.
196 {
197  // Cached calib data?
199  if (it != cache_default_calibs.end())
200  return it->second;
201 
202  VelodyneCalibration result; // Leave empty to indicate unknown model
203  std::string xml_contents;
204 
205  if (lidar_model=="VLP16")
206  xml_contents = velodyne_default_calib_VLP16;
207  else if (lidar_model=="HDL32")
208  xml_contents = velodyne_default_calib_HDL32;
209  else
210  {}
211 
212 
213  if (!xml_contents.empty()) {
214  if (!result.loadFromXMLText( xml_contents ) )
215  std::cerr << "[VelodyneCalibration::LoadDefaultCalibration] Error parsing default XML calibration file for model '" << lidar_model << "'\n";
216  }
217 
218  cache_default_calibs[lidar_model] = result;
219  return cache_default_calibs[lidar_model];
220 }
221 
Velodyne calibration data, for usage in mrpt::obs::CObservationVelodyneScan.
const char * velodyne_default_calib_HDL32
bool loadFromXMLFile(const std::string &velodyne_calibration_xml_filename)
Loads calibration from file, in the format supplied by the manufacturer.
bool loadFromXMLText(const std::string &xml_file_contents)
Loads calibration from a string containing an entire XML calibration file.
double DEG2RAD(const double x)
Degrees to radians.
Main Class representing a XML node.
Definition: xmlParser.h:275
int nColumn
Definition: xmlParser.h:245
VelodyneCalibration()
Default ctor (leaves all empty)
void clear()
Clear all previous contents.
static XMLNode parseString(XMLCSTR lpXMLString, XMLCSTR tag=NULL, XMLResults *pResults=NULL)
Parse an XML string and return the root of a XMLNode tree representing the string.
Definition: xmlParser.cpp:1645
char isEmpty() const
is this node Empty?
Definition: xmlParser.cpp:2538
std::map< std::string, VelodyneCalibration > cache_default_calibs
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
static XMLCSTR getError(XMLError error)
this gives you a user-friendly explanation of the parsing error
Definition: xmlParser.cpp:73
XMLCSTR getText(int i=0) const
return ith text field
Definition: xmlParser.cpp:2534
std::vector< PerLaserCalib > laser_corrections
This namespace contains representation of robot actions and observations.
GLsizei const GLchar ** string
Definition: glext.h:3919
#define ASSERT_ABOVEEQ_(__A, __B)
bool empty() const
Returns true if no calibration has been loaded yet.
GLuint id
Definition: glext.h:3770
enum XMLError error
Definition: xmlParser.h:244
static XMLNode parseFile(XMLCSTR filename, XMLCSTR tag=NULL, XMLResults *pResults=NULL)
Parse an XML file and return the root of a XMLNode tree representing the file.
Definition: xmlParser.cpp:1708
const char * velodyne_default_calib_VLP16
static const VelodyneCalibration & LoadDefaultCalibration(const std::string &lidar_model)
Loads default calibration files for common LIDAR models.
Structure used to obtain error details if the parse fails.
Definition: xmlParser.h:242
XMLNode getChildNode(int i=0) const
return ith child node
Definition: xmlParser.cpp:2535



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020