Go to the documentation of this file.
19 #undef _UNICODE // JLBC, for xmlParser
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)
50 throw std::runtime_error(
"Cannot find XML node: 'boost_serialization'");
54 throw std::runtime_error(
"Cannot find XML node: 'DB'");
58 throw std::runtime_error(
"Cannot find XML node: 'enabled_'");
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'");
72 for (
int i = 0; i < nEnabled; i++)
76 throw std::runtime_error(
77 "Cannot find the expected number of XML nodes: "
79 const int enable_val = atoi(node_enabled_ith.
getText());
80 if (enable_val) ++enabledCount;
88 throw std::runtime_error(
"Cannot find XML node: 'points_'");
93 if (node_points_item.
isEmpty())
break;
97 throw std::runtime_error(
98 "Cannot find XML node: 'points_::item::px'");
102 throw std::runtime_error(
103 "Cannot find XML node: 'points_::item::px::id_'");
104 const int id = atoi(node_px_id.
getText());
106 if (
id >= enabledCount)
continue;
113 throw std::runtime_error(
114 "Cannot find XML node: "
115 "'points_::item::px::rotCorrection_'");
121 throw std::runtime_error(
122 "Cannot find XML node: "
123 "'points_::item::px::vertCorrection_'");
129 throw std::runtime_error(
130 "Cannot find XML node: "
131 "'points_::item::px::distCorrection_'");
137 throw std::runtime_error(
138 "Cannot find XML node: "
139 "'points_::item::px::vertOffsetCorrection_'");
145 throw std::runtime_error(
146 "Cannot find XML node: "
147 "'points_::item::px::horizOffsetCorrection_'");
175 cerr <<
"[VelodyneCalibration::loadFromXMLText] Error parsing XML "
186 cerr <<
"[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
196 const std::string& velodyne_calibration_xml_filename)
202 velodyne_calibration_xml_filename.c_str(),
nullptr, &
results);
206 cerr <<
"[VelodyneCalibration::loadFromXMLFile] Error loading XML "
216 cerr <<
"[VelodyneCalibration::loadFromXMLFile] Exception:" << endl
239 if (lidar_model ==
"VLP16")
241 else if (lidar_model ==
"HDL32")
247 if (!xml_contents.empty())
250 std::cerr <<
"[VelodyneCalibration::LoadDefaultCalibration] Error "
251 "parsing default XML calibration file for model '"
252 << lidar_model <<
"'\n";
bool loadFromXMLFile(const std::string &velodyne_calibration_xml_filename)
Loads calibration from file, in the format supplied by the manufacturer.
bool internal_loadFromXMLNode(void *node)
const Scalar * const_iterator
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.
char isEmpty() const
is this node Empty?
std::map< std::string, VelodyneCalibration > cache_default_calibs
XMLCSTR getText(int i=0) const
return ith text field
bool empty() const
Returns true if no calibration has been loaded yet.
This namespace contains representation of robot actions and observations.
map< string, CVectorDouble > results
Structure used to obtain error details if the parse fails.
VelodyneCalibration()
Default ctor (leaves all empty)
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.
double verticalOffsetCorrection
double sinVertOffsetCorrection
double distanceCorrection
Velodyne calibration data, for usage in mrpt::obs::CObservationVelodyneScan.
const char * velodyne_default_calib_HDL32
double verticalCorrection
std::vector< PerLaserCalib > laser_corrections
double cosVertOffsetCorrection
#define ASSERT_ABOVEEQ_(__A, __B)
Main Class representing a XML node.
const char * velodyne_default_calib_VLP16
bool loadFromXMLText(const std::string &xml_file_contents)
Loads calibration from a string containing an entire XML calibration file.
static const VelodyneCalibration & LoadDefaultCalibration(const std::string &lidar_model)
Loads default calibration files for common LIDAR models.
GLsizei const GLchar ** string
void clear()
Clear all previous contents.
double horizontalOffsetCorrection
XMLNode getChildNode(int i=0) const
return ith child node
static XMLCSTR getError(XMLError error)
this gives you a
double DEG2RAD(const double x)
Degrees to radians.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |