12 #include <mrpt/config.h> 22 #include <yaml-cpp/yaml.h> 25 #undef _UNICODE // JLBC, for xmlParser 37 VelodyneCalibration::PerLaserCalib::PerLaserCalib()
41 VelodyneCalibration::VelodyneCalibration() : laser_corrections(0) {}
48 throw std::runtime_error(
"Cannot find XML node: 'boost_serialization'");
52 throw std::runtime_error(
"Cannot find XML node: 'DB'");
56 throw std::runtime_error(
"Cannot find XML node: 'enabled_'");
62 if (node_enabled_count.
isEmpty())
63 throw std::runtime_error(
"Cannot find XML node: 'enabled_::count'");
64 const int nEnabled = atoi(node_enabled_count.
getText());
65 if (nEnabled <= 0 || nEnabled > 10000)
66 throw std::runtime_error(
67 "Senseless value found reading 'enabled_::count'");
70 for (
int i = 0; i < nEnabled; i++)
74 throw std::runtime_error(
75 "Cannot find the expected number of XML nodes: " 77 const int enable_val = atoi(node_enabled_ith.
getText());
78 if (enable_val) ++enabledCount;
86 throw std::runtime_error(
"Cannot find XML node: 'points_'");
91 if (node_points_item.
isEmpty())
break;
95 throw std::runtime_error(
96 "Cannot find XML node: 'points_::item::px'");
100 throw std::runtime_error(
101 "Cannot find XML node: 'points_::item::px::id_'");
102 const int id = atoi(node_px_id.
getText());
104 if (
id >= enabledCount)
continue;
111 throw std::runtime_error(
112 "Cannot find XML node: " 113 "'points_::item::px::rotCorrection_'");
119 throw std::runtime_error(
120 "Cannot find XML node: " 121 "'points_::item::px::vertCorrection_'");
127 throw std::runtime_error(
128 "Cannot find XML node: " 129 "'points_::item::px::distCorrection_'");
135 throw std::runtime_error(
136 "Cannot find XML node: " 137 "'points_::item::px::vertOffsetCorrection_'");
143 throw std::runtime_error(
144 "Cannot find XML node: " 145 "'points_::item::px::horizOffsetCorrection_'");
173 cerr <<
"[VelodyneCalibration::loadFromXMLText] Error parsing XML " 184 cerr <<
"[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
194 const std::string& velodyne_calibration_xml_filename)
200 velodyne_calibration_xml_filename.c_str(),
nullptr, &
results);
204 cerr <<
"[VelodyneCalibration::loadFromXMLFile] Error loading XML " 214 cerr <<
"[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
225 YAML::Node root = YAML::Load(str);
230 const auto num_lasers = root[
"num_lasers"].as<
unsigned int>(0);
235 auto lasers = root[
"lasers"];
238 for (
auto item : lasers)
240 const auto id = item[
"laser_id"].as<
unsigned int>(9999999);
250 item[
"vert_offset_correction"].as<
double>();
252 item[
"horiz_offset_correction"].as<
double>();
265 catch (
const std::exception& e)
267 std::cerr <<
"[VelodyneCalibration::loadFromYAMLText]" << e.what()
281 std::ifstream f(filename);
287 (std::istreambuf_iterator<char>(f)),
288 std::istreambuf_iterator<char>());
292 catch (
const std::exception& e)
294 std::cerr <<
"[VelodyneCalibration::loadFromYAMLFile]" << e.what()
319 if (lidar_model ==
"VLP16")
321 else if (lidar_model ==
"HDL32")
323 else if (lidar_model ==
"HDL64")
326 if (!xml_contents.empty())
328 if (!result.loadFromXMLText(xml_contents))
329 std::cerr <<
"[VelodyneCalibration::LoadDefaultCalibration] Error " 330 "parsing default XML calibration file for model '" 331 << lidar_model <<
"'\n";
333 else if (!yaml_contents.empty())
335 if (!result.loadFromYAMLText(yaml_contents))
336 std::cerr <<
"[VelodyneCalibration::LoadDefaultCalibration] Error " 337 "parsing default YAML calibration file for model '" 338 << lidar_model <<
"'\n";
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.
double verticalOffsetCorrection
bool loadFromXMLText(const std::string &xml_file_contents)
Loads calibration from a string containing an entire XML calibration file.
#define THROW_EXCEPTION(msg)
Main Class representing a XML node.
const std::string velodyne_default_calib_HDL64E_S3
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.
void clear()
Clear all previous contents.
char isEmpty() const
is this node Empty?
bool internal_loadFromXMLNode(void *node)
static XMLCSTR getError(XMLError error)
this gives you a
XMLCSTR getText(int i=0) const
return ith text field
#define ASSERT_(f)
Defines an assertion mechanism.
map< string, CVectorDouble > results
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
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 ASSERT_ABOVEEQ_(__A, __B)
GLsizei const GLchar ** string
static 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.
double sinVertOffsetCorrection
bool empty() const
Returns true if no calibration has been loaded yet.
double distanceCorrection
double verticalCorrection
double horizontalOffsetCorrection
double cosVertOffsetCorrection
const char * velodyne_default_calib_VLP16
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
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.
Structure used to obtain error details if the parse fails.
XMLNode getChildNode(int i=0) const
return ith child node