Main MRPT website > C++ reference for MRPT 1.9.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-2018, 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/core/bits_math.h>
14 #include <mrpt/core/exceptions.h>
15 #include <iostream>
16 #include <map>
17 #include <cmath>
18 
19 #undef _UNICODE // JLBC, for xmlParser
20 #include "xmlparser/xmlParser.h"
21 
22 // ======= Default calibration files ========================
25 // ======= End of default calibration files =================
26 
27 using namespace std;
28 using namespace mrpt::obs;
29 
30 VelodyneCalibration::PerLaserCalib::PerLaserCalib()
31  : azimuthCorrection(.0),
32  verticalCorrection(.0),
33  distanceCorrection(.0),
34  verticalOffsetCorrection(.0),
35  horizontalOffsetCorrection(.0),
36  sinVertCorrection(.0),
37  cosVertCorrection(1.0),
38  sinVertOffsetCorrection(.0),
39  cosVertOffsetCorrection(1.0)
40 {
41 }
42 
45 {
46  XMLNode& root = *reinterpret_cast<XMLNode*>(node_ptr);
47 
48  XMLNode node_bs = root.getChildNode("boost_serialization");
49  if (node_bs.isEmpty())
50  throw std::runtime_error("Cannot find XML node: 'boost_serialization'");
51 
52  XMLNode node_DB = node_bs.getChildNode("DB");
53  if (node_DB.isEmpty())
54  throw std::runtime_error("Cannot find XML node: 'DB'");
55 
56  XMLNode node_enabled_ = node_DB.getChildNode("enabled_");
57  if (node_enabled_.isEmpty())
58  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())
65  throw std::runtime_error("Cannot find XML node: 'enabled_::count'");
66  const int nEnabled = atoi(node_enabled_count.getText());
67  if (nEnabled <= 0 || nEnabled > 10000)
68  throw std::runtime_error(
69  "Senseless value found reading 'enabled_::count'");
70 
71  int enabledCount = 0;
72  for (int i = 0; i < nEnabled; i++)
73  {
74  XMLNode node_enabled_ith = node_enabled_.getChildNode("item", i);
75  if (node_enabled_ith.isEmpty())
76  throw std::runtime_error(
77  "Cannot find the expected number of XML nodes: "
78  "'enabled_::item'");
79  const int enable_val = atoi(node_enabled_ith.getText());
80  if (enable_val) ++enabledCount;
81  }
82 
83  // enabledCount = number of lasers in the LIDAR
84  this->laser_corrections.resize(enabledCount);
85 
86  XMLNode node_points_ = node_DB.getChildNode("points_");
87  if (node_points_.isEmpty())
88  throw std::runtime_error("Cannot find XML node: 'points_'");
89 
90  for (int i = 0;; ++i)
91  {
92  XMLNode node_points_item = node_points_.getChildNode("item", i);
93  if (node_points_item.isEmpty()) break;
94 
95  XMLNode node_px = node_points_item.getChildNode("px");
96  if (node_px.isEmpty())
97  throw std::runtime_error(
98  "Cannot find XML node: 'points_::item::px'");
99 
100  XMLNode node_px_id = node_px.getChildNode("id_");
101  if (node_px_id.isEmpty())
102  throw std::runtime_error(
103  "Cannot find XML node: 'points_::item::px::id_'");
104  const int id = atoi(node_px_id.getText());
105  ASSERT_ABOVEEQ_(id, 0);
106  if (id >= enabledCount) continue; // ignore
107 
109 
110  {
111  XMLNode node = node_px.getChildNode("rotCorrection_");
112  if (node.isEmpty())
113  throw std::runtime_error(
114  "Cannot find XML node: "
115  "'points_::item::px::rotCorrection_'");
116  plc->azimuthCorrection = atof(node.getText());
117  }
118  {
119  XMLNode node = node_px.getChildNode("vertCorrection_");
120  if (node.isEmpty())
121  throw std::runtime_error(
122  "Cannot find XML node: "
123  "'points_::item::px::vertCorrection_'");
124  plc->verticalCorrection = atof(node.getText());
125  }
126  {
127  XMLNode node = node_px.getChildNode("distCorrection_");
128  if (node.isEmpty())
129  throw std::runtime_error(
130  "Cannot find XML node: "
131  "'points_::item::px::distCorrection_'");
132  plc->distanceCorrection = 0.01f * atof(node.getText());
133  }
134  {
135  XMLNode node = node_px.getChildNode("vertOffsetCorrection_");
136  if (node.isEmpty())
137  throw std::runtime_error(
138  "Cannot find XML node: "
139  "'points_::item::px::vertOffsetCorrection_'");
140  plc->verticalOffsetCorrection = 0.01f * atof(node.getText());
141  }
142  {
143  XMLNode node = node_px.getChildNode("horizOffsetCorrection_");
144  if (node.isEmpty())
145  throw std::runtime_error(
146  "Cannot find XML node: "
147  "'points_::item::px::horizOffsetCorrection_'");
148  plc->horizontalOffsetCorrection = 0.01f * atof(node.getText());
149  }
150 
151  plc->sinVertCorrection =
152  std::sin(mrpt::DEG2RAD(plc->verticalCorrection));
153  plc->cosVertCorrection =
154  std::cos(mrpt::DEG2RAD(plc->verticalCorrection));
155 
160  }
161 
162  return true; // Ok
163 }
164 
165 bool VelodyneCalibration::loadFromXMLText(const std::string& xml_file_contents)
166 {
167  try
168  {
170  XMLNode root =
171  XMLNode::parseString(xml_file_contents.c_str(), nullptr, &results);
172 
173  if (results.error != eXMLErrorNone)
174  {
175  cerr << "[VelodyneCalibration::loadFromXMLText] Error parsing XML "
176  "content: "
177  << XMLNode::getError(results.error) << " at line "
178  << results.nLine << ":" << results.nColumn << endl;
179  return false;
180  }
181 
182  return internal_loadFromXMLNode(reinterpret_cast<void*>(&root));
183  }
184  catch (exception& e)
185  {
186  cerr << "[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
187  << e.what() << endl;
188  return false;
189  }
190 }
191 
192 /** Loads calibration from file. \return false on any error, true on success */
193 // See reference code in:
194 // vtkVelodyneHDLReader::vtkInternal::LoadCorrectionsFile()
196  const std::string& velodyne_calibration_xml_filename)
197 {
198  try
199  {
202  velodyne_calibration_xml_filename.c_str(), nullptr, &results);
203 
204  if (results.error != eXMLErrorNone)
205  {
206  cerr << "[VelodyneCalibration::loadFromXMLFile] Error loading XML "
207  "file: "
208  << XMLNode::getError(results.error) << " at line "
209  << results.nLine << ":" << results.nColumn << endl;
210  return false;
211  }
212  return internal_loadFromXMLNode(reinterpret_cast<void*>(&root));
213  }
214  catch (exception& e)
215  {
216  cerr << "[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
217  << e.what() << endl;
218  return false;
219  }
220 }
221 
222 bool VelodyneCalibration::empty() const { return laser_corrections.empty(); }
224 std::map<std::string, VelodyneCalibration> cache_default_calibs;
225 
226 // It always return a calibration structure, but it may be empty if the model
227 // name is unknown.
229  const std::string& lidar_model)
230 {
231  // Cached calib data?
233  cache_default_calibs.find(lidar_model);
234  if (it != cache_default_calibs.end()) return it->second;
235 
236  VelodyneCalibration result; // Leave empty to indicate unknown model
237  std::string xml_contents;
238 
239  if (lidar_model == "VLP16")
240  xml_contents = velodyne_default_calib_VLP16;
241  else if (lidar_model == "HDL32")
242  xml_contents = velodyne_default_calib_HDL32;
243  else
244  {
245  }
246 
247  if (!xml_contents.empty())
248  {
249  if (!result.loadFromXMLText(xml_contents))
250  std::cerr << "[VelodyneCalibration::LoadDefaultCalibration] Error "
251  "parsing default XML calibration file for model '"
252  << lidar_model << "'\n";
253  }
254 
255  cache_default_calibs[lidar_model] = result;
256  return cache_default_calibs[lidar_model];
257 }
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.
Main Class representing a XML node.
Definition: xmlParser.h:313
VelodyneCalibration()
Default ctor (leaves all empty)
double DEG2RAD(const double x)
Degrees to radians.
static XMLNode parseString(XMLCSTR lpXMLString, XMLCSTR tag=nullptr, XMLResults *pResults=nullptr)
Parse an XML string and return the root of a XMLNode tree representing the string.
Definition: xmlParser.cpp:2216
void clear()
Clear all previous contents.
char isEmpty() const
is this node Empty?
Definition: xmlParser.cpp:3417
STL namespace.
static XMLCSTR getError(XMLError error)
this gives you a
Definition: xmlParser.cpp:83
XMLCSTR getText(int i=0) const
return ith text field
Definition: xmlParser.cpp:3397
map< string, CVectorDouble > results
std::vector< PerLaserCalib > laser_corrections
This namespace contains representation of robot actions and observations.
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
GLsizei const GLchar ** string
Definition: glext.h:4101
std::map< std::string, VelodyneCalibration > cache_default_calibs
static XMLNode parseFile(XMLCSTR filename, XMLCSTR tag=nullptr, XMLResults *pResults=nullptr)
Parse an XML file and return the root of a XMLNode tree representing the file.
Definition: xmlParser.cpp:2283
bool empty() const
Returns true if no calibration has been loaded yet.
GLuint id
Definition: glext.h:3909
const char * velodyne_default_calib_VLP16
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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:274
XMLNode getChildNode(int i=0) const
return ith child node
Definition: xmlParser.cpp:3402



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019