21 #include <mrpt/config.h>
23 #include <yaml-cpp/yaml.h>
38 VelodyneCalibration::PerLaserCalib::PerLaserCalib() =
default;
41 static const tinyxml2::XMLElement* get_xml_children(
42 const tinyxml2::XMLNode* e,
const char* name)
47 auto ret = e->FirstChildElement(name);
49 throw std::runtime_error(
53 static const char* get_xml_children_as_str(
54 const tinyxml2::XMLNode* e,
const char* name)
56 auto n = get_xml_children(e, name);
58 const char* txt = n->GetText();
60 txt,
mrpt::format(
"Cannot convert XML tag `%s` to string", name));
63 static double get_xml_children_as_double(
64 const tinyxml2::XMLNode* e,
const char* name)
66 return ::atof(get_xml_children_as_str(e, name));
68 static int get_xml_children_as_int(
const tinyxml2::XMLNode* e,
const char* name)
70 return ::atoi(get_xml_children_as_str(e, name));
74 VelodyneCalibration::VelodyneCalibration() : laser_corrections(0) {}
81 const tinyxml2::XMLDocument& root =
82 *
reinterpret_cast<tinyxml2::XMLDocument*
>(node_ptr);
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_");
91 const int nEnabled = get_xml_children_as_int(node_enabled_,
"count");
96 const tinyxml2::XMLElement* node_enabled_ith =
nullptr;
97 for (
int i = 0; i < nEnabled; i++)
100 node_enabled_ith = node_enabled_ith->NextSiblingElement(
"item");
104 node_enabled_ith = node_enabled_->FirstChildElement(
"item");
107 if (!node_enabled_ith)
108 throw std::runtime_error(
109 "Cannot find the expected number of XML nodes: "
111 const int enable_val = atoi(node_enabled_ith->GetText());
112 if (enable_val) ++enabledCount;
118 auto node_points_ = get_xml_children(node_DB,
"points_");
119 const tinyxml2::XMLElement* node_points_item =
nullptr;
121 for (
int i = 0;; ++i)
123 if (!node_points_item)
124 node_points_item = node_points_->FirstChildElement(
"item");
126 node_points_item = node_points_item->NextSiblingElement(
"item");
128 if (!node_points_item)
break;
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());
134 if (
id >= enabledCount)
continue;
139 get_xml_children_as_double(node_px,
"rotCorrection_");
141 get_xml_children_as_double(node_px,
"vertCorrection_");
144 0.01 * get_xml_children_as_double(node_px,
"distCorrection_");
146 0.01 * get_xml_children_as_double(node_px,
"vertOffsetCorrection_");
149 get_xml_children_as_double(node_px,
"horizOffsetCorrection_");
170 #if MRPT_HAS_TINYXML2
171 tinyxml2::XMLDocument doc;
172 const auto err = doc.Parse(xml_file_contents.c_str());
174 if (err != tinyxml2::XML_SUCCESS)
177 <<
"[VelodyneCalibration::loadFromXMLText] Error parsing XML "
179 #if TIXML2_MAJOR_VERSION >= 7 || \
180 (TIXML2_MAJOR_VERSION >= 6 && TIXML2_MINOR_VERSION >= 2)
181 << tinyxml2::XMLDocument::ErrorIDToName(err)
196 cerr <<
"[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
206 const std::string& velodyne_calibration_xml_fil)
210 #if MRPT_HAS_TINYXML2
211 tinyxml2::XMLDocument doc;
212 const auto err = doc.LoadFile(velodyne_calibration_xml_fil.c_str());
214 if (err != tinyxml2::XML_SUCCESS)
216 std::cerr <<
"[VelodyneCalibration::loadFromXMLFile] Error loading "
218 #if TIXML2_MAJOR_VERSION >= 7 || \
219 (TIXML2_MAJOR_VERSION >= 6 && TIXML2_MINOR_VERSION >= 2)
220 << tinyxml2::XMLDocument::ErrorIDToName(err)
234 cerr <<
"[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
245 YAML::Node root = YAML::Load(str);
250 const auto num_lasers = root[
"num_lasers"].as<
unsigned int>(0);
255 auto lasers = root[
"lasers"];
258 for (
auto item : lasers)
260 const auto id = item[
"laser_id"].as<
unsigned int>(9999999);
270 item[
"vert_offset_correction"].as<
double>();
272 item[
"horiz_offset_correction"].as<
double>();
285 catch (
const std::exception& e)
287 std::cerr <<
"[VelodyneCalibration::loadFromYAMLText]" << e.what()
301 std::ifstream f(filename);
307 (std::istreambuf_iterator<char>(f)),
308 std::istreambuf_iterator<char>());
312 catch (
const std::exception& e)
314 std::cerr <<
"[VelodyneCalibration::loadFromYAMLFile]" << e.what()
330 const std::string& lidar_model)
337 std::string xml_contents, yaml_contents;
339 if (lidar_model ==
"VLP16")
341 else if (lidar_model ==
"HDL32")
343 else if (lidar_model ==
"HDL64")
346 if (!xml_contents.empty())
349 std::cerr <<
"[VelodyneCalibration::LoadDefaultCalibration] Error "
350 "parsing default XML calibration file for model '"
351 << lidar_model <<
"'\n";
353 else if (!yaml_contents.empty())
356 std::cerr <<
"[VelodyneCalibration::LoadDefaultCalibration] Error "
357 "parsing default YAML calibration file for model '"
358 << lidar_model <<
"'\n";