MRPT  2.0.1
VelodyneCalibration.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/core/bits_math.h>
13 #include <mrpt/core/exceptions.h>
15 #include <cmath>
16 #include <fstream>
17 #include <iostream>
18 #include <map>
19 #include <sstream>
20 
21 #include <mrpt/config.h>
22 #if MRPT_HAS_YAMLCPP
23 #include <yaml-cpp/yaml.h>
24 #endif
25 #if MRPT_HAS_TINYXML2
26 #include <tinyxml2.h>
27 #endif
28 
29 // ======= Default calibration files ========================
33 // ======= End of default calibration files =================
34 
35 using namespace std;
36 using namespace mrpt::obs;
37 
38 VelodyneCalibration::PerLaserCalib::PerLaserCalib() = default;
39 
40 #if MRPT_HAS_TINYXML2
41 static const tinyxml2::XMLElement* get_xml_children(
42  const tinyxml2::XMLNode* e, const char* name)
43 {
44  ASSERT_(e != nullptr);
45  // It's ok if name is nullptr for tinyxml2
46 
47  auto ret = e->FirstChildElement(name);
48  if (!ret)
49  throw std::runtime_error(
50  mrpt::format("Cannot find XML tag `%s`", name));
51  return ret;
52 }
53 static const char* get_xml_children_as_str(
54  const tinyxml2::XMLNode* e, const char* name)
55 {
56  auto n = get_xml_children(e, name);
57  ASSERTMSG_(n, mrpt::format("Cannot find XML tag `%s`", name));
58  const char* txt = n->GetText();
59  ASSERTMSG_(
60  txt, mrpt::format("Cannot convert XML tag `%s` to string", name));
61  return txt;
62 }
63 static double get_xml_children_as_double(
64  const tinyxml2::XMLNode* e, const char* name)
65 {
66  return ::atof(get_xml_children_as_str(e, name));
67 }
68 static int get_xml_children_as_int(const tinyxml2::XMLNode* e, const char* name)
69 {
70  return ::atoi(get_xml_children_as_str(e, name));
71 }
72 #endif
73 
74 VelodyneCalibration::VelodyneCalibration() : laser_corrections(0) {}
76 {
77 #if MRPT_HAS_TINYXML2
78 
79  ASSERT_(node_ptr != nullptr);
80 
81  const tinyxml2::XMLDocument& root =
82  *reinterpret_cast<tinyxml2::XMLDocument*>(node_ptr);
83 
84  auto node_bs = get_xml_children(&root, "boost_serialization");
85  auto node_DB = get_xml_children(node_bs, "DB");
86  auto node_enabled_ = get_xml_children(node_DB, "enabled_");
87 
88  // Clear previous contents:
89  clear();
90 
91  const int nEnabled = get_xml_children_as_int(node_enabled_, "count");
92  ASSERT_ABOVE_(nEnabled, 0);
93  ASSERT_BELOW_(nEnabled, 10000);
94 
95  int enabledCount = 0;
96  const tinyxml2::XMLElement* node_enabled_ith = nullptr;
97  for (int i = 0; i < nEnabled; i++)
98  {
99  if (node_enabled_ith)
100  node_enabled_ith = node_enabled_ith->NextSiblingElement("item");
101  else
102  {
103  ASSERT_EQUAL_(i, 0);
104  node_enabled_ith = node_enabled_->FirstChildElement("item");
105  }
106 
107  if (!node_enabled_ith)
108  throw std::runtime_error(
109  "Cannot find the expected number of XML nodes: "
110  "'enabled_::item'");
111  const int enable_val = atoi(node_enabled_ith->GetText());
112  if (enable_val) ++enabledCount;
113  }
114 
115  // enabledCount = number of lasers in the LIDAR
116  this->laser_corrections.resize(enabledCount);
117 
118  auto node_points_ = get_xml_children(node_DB, "points_");
119  const tinyxml2::XMLElement* node_points_item = nullptr;
120 
121  for (int i = 0;; ++i)
122  {
123  if (!node_points_item)
124  node_points_item = node_points_->FirstChildElement("item");
125  else
126  node_points_item = node_points_item->NextSiblingElement("item");
127 
128  if (!node_points_item) break;
129 
130  auto node_px = get_xml_children(node_points_item, "px");
131  auto node_px_id = get_xml_children(node_px, "id_");
132  const int id = atoi(node_px_id->GetText());
133  ASSERT_ABOVEEQ_(id, 0);
134  if (id >= enabledCount) continue; // ignore
135 
136  PerLaserCalib& plc = laser_corrections[id];
137 
138  plc.azimuthCorrection =
139  get_xml_children_as_double(node_px, "rotCorrection_");
140  plc.verticalCorrection =
141  get_xml_children_as_double(node_px, "vertCorrection_");
142 
143  plc.distanceCorrection =
144  0.01 * get_xml_children_as_double(node_px, "distCorrection_");
146  0.01 * get_xml_children_as_double(node_px, "vertOffsetCorrection_");
148  0.01 *
149  get_xml_children_as_double(node_px, "horizOffsetCorrection_");
150 
153 
158  }
159 
160  return true; // Ok
161 #else
162  return false;
163 #endif
164 }
165 
166 bool VelodyneCalibration::loadFromXMLText(const std::string& xml_file_contents)
167 {
168  try
169  {
170 #if MRPT_HAS_TINYXML2
171  tinyxml2::XMLDocument doc;
172  const auto err = doc.Parse(xml_file_contents.c_str());
173 
174  if (err != tinyxml2::XML_SUCCESS)
175  {
176  std::cerr
177  << "[VelodyneCalibration::loadFromXMLText] Error parsing XML "
178  "content: "
179 #if TIXML2_MAJOR_VERSION >= 7 || \
180  (TIXML2_MAJOR_VERSION >= 6 && TIXML2_MINOR_VERSION >= 2)
181  << tinyxml2::XMLDocument::ErrorIDToName(err)
182 #else
183  << doc.ErrorName()
184 #endif
185  << std::endl;
186  return false;
187  }
188 
189  return internal_loadFromXMLNode(reinterpret_cast<void*>(&doc));
190 #else
191  THROW_EXCEPTION("MRPT built without tinyxml2");
192 #endif
193  }
194  catch (exception& e)
195  {
196  cerr << "[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
197  << e.what() << endl;
198  return false;
199  }
200 }
201 
202 /** Loads calibration from file. \return false on any error, true on success */
203 // See reference code in:
204 // vtkVelodyneHDLReader::vtkInternal::LoadCorrectionsFile()
206  const std::string& velodyne_calibration_xml_fil)
207 {
208  try
209  {
210 #if MRPT_HAS_TINYXML2
211  tinyxml2::XMLDocument doc;
212  const auto err = doc.LoadFile(velodyne_calibration_xml_fil.c_str());
213 
214  if (err != tinyxml2::XML_SUCCESS)
215  {
216  std::cerr << "[VelodyneCalibration::loadFromXMLFile] Error loading "
217  "XML file: "
218 #if TIXML2_MAJOR_VERSION >= 7 || \
219  (TIXML2_MAJOR_VERSION >= 6 && TIXML2_MINOR_VERSION >= 2)
220  << tinyxml2::XMLDocument::ErrorIDToName(err)
221 #else
222  << doc.ErrorName()
223 #endif
224  << std::endl;
225  return false;
226  }
227  return internal_loadFromXMLNode(reinterpret_cast<void*>(&doc));
228 #else
229  THROW_EXCEPTION("MRPT built without tinyxml2");
230 #endif
231  }
232  catch (exception& e)
233  {
234  cerr << "[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
235  << e.what() << endl;
236  return false;
237  }
238 }
239 
240 bool VelodyneCalibration::loadFromYAMLText(const std::string& str)
241 {
242 #if MRPT_HAS_YAMLCPP
243  try
244  {
245  YAML::Node root = YAML::Load(str);
246 
247  // Clear previous contents:
248  clear();
249 
250  const auto num_lasers = root["num_lasers"].as<unsigned int>(0);
251  ASSERT_(num_lasers > 0);
252 
253  this->laser_corrections.resize(num_lasers);
254 
255  auto lasers = root["lasers"];
256  ASSERT_EQUAL_(lasers.size(), num_lasers);
257 
258  for (auto item : lasers)
259  {
260  const auto id = item["laser_id"].as<unsigned int>(9999999);
261  ASSERT_(id < num_lasers);
262 
263  PerLaserCalib& plc = laser_corrections[id];
264 
265  plc.azimuthCorrection = item["rot_correction"].as<double>();
266  plc.verticalCorrection = item["vert_correction"].as<double>();
267 
268  plc.distanceCorrection = item["dist_correction"].as<double>();
270  item["vert_offset_correction"].as<double>();
272  item["horiz_offset_correction"].as<double>();
273 
274  plc.sinVertCorrection = std::sin(plc.verticalCorrection);
275  plc.cosVertCorrection = std::cos(plc.verticalCorrection);
276 
281  }
282 
283  return true; // all ok
284  }
285  catch (const std::exception& e)
286  {
287  std::cerr << "[VelodyneCalibration::loadFromYAMLText]" << e.what()
288  << "\n";
289  return false;
290  }
291 #else
292  THROW_EXCEPTION("This method requires building MRPT with YAML-CPP.");
293 #endif
294 }
295 
296 bool VelodyneCalibration::loadFromYAMLFile(const std::string& filename)
297 {
298 #if MRPT_HAS_YAMLCPP
299  try
300  {
301  std::ifstream f(filename);
302  if (!f.is_open())
303  THROW_EXCEPTION_FMT("Cannot open file: '%s'", filename.c_str());
304 
305  // Load file:
306  std::string str(
307  (std::istreambuf_iterator<char>(f)),
308  std::istreambuf_iterator<char>());
309 
310  return loadFromYAMLText(str);
311  }
312  catch (const std::exception& e)
313  {
314  std::cerr << "[VelodyneCalibration::loadFromYAMLFile]" << e.what()
315  << "\n";
316  return false;
317  }
318 #else
319  THROW_EXCEPTION("This method requires building MRPT with YAML-CPP.");
320 #endif
321 }
322 
323 bool VelodyneCalibration::empty() const { return laser_corrections.empty(); }
325 static std::map<std::string, VelodyneCalibration> cache_default_calibs;
326 
327 // It always return a calibration structure, but it may be empty if the model
328 // name is unknown.
330  const std::string& lidar_model)
331 {
332  // Cached calib data?
333  auto it = cache_default_calibs.find(lidar_model);
334  if (it != cache_default_calibs.end()) return it->second;
335 
336  VelodyneCalibration result; // Leave empty to indicate unknown model
337  std::string xml_contents, yaml_contents;
338 
339  if (lidar_model == "VLP16")
340  xml_contents = velodyne_default_calib_VLP16;
341  else if (lidar_model == "HDL32")
342  xml_contents = velodyne_default_calib_HDL32;
343  else if (lidar_model == "HDL64")
344  yaml_contents = velodyne_default_calib_HDL64E_S3;
345 
346  if (!xml_contents.empty())
347  {
348  if (!result.loadFromXMLText(xml_contents))
349  std::cerr << "[VelodyneCalibration::LoadDefaultCalibration] Error "
350  "parsing default XML calibration file for model '"
351  << lidar_model << "'\n";
352  }
353  else if (!yaml_contents.empty())
354  {
355  if (!result.loadFromYAMLText(yaml_contents))
356  std::cerr << "[VelodyneCalibration::LoadDefaultCalibration] Error "
357  "parsing default YAML calibration file for model '"
358  << lidar_model << "'\n";
359  }
360 
361  cache_default_calibs[lidar_model] = result;
362  return cache_default_calibs[lidar_model];
363 }
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.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
const std::string velodyne_default_calib_HDL64E_S3
void clear()
Clear all previous contents.
STL namespace.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
constexpr double DEG2RAD(const double x)
Degrees to radians.
std::vector< PerLaserCalib > laser_corrections
bool loadFromYAMLText(const std::string &yaml_file_contents)
Loads calibration from a string containing an entire YAML calibration file.
This namespace contains representation of robot actions and observations.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
static std::map< std::string, VelodyneCalibration > cache_default_calibs
bool empty() const
Returns true if no calibration has been loaded yet.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
const char * velodyne_default_calib_VLP16
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
bool loadFromYAMLFile(const std::string &velodyne_calib_yaml_filename)
Loads calibration from a YAML calibration file.
static const VelodyneCalibration & LoadDefaultCalibration(const std::string &lidar_model)
Loads default calibration files for common LIDAR models.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020