22 #ifndef OPENCV_YAML_PARSER_H 23 #define OPENCV_YAML_PARSER_H 25 #include <Eigen/Eigen> 26 #include <boost/filesystem.hpp> 28 #include <opencv2/opencv.hpp> 30 #if ROS_AVAILABLE == 1 32 #elif ROS_AVAILABLE == 2 33 #include <rclcpp/rclcpp.hpp> 68 if (!fail_if_not_found && !boost::filesystem::exists(config_path)) {
72 if (!boost::filesystem::exists(config_path)) {
74 std::exit(EXIT_FAILURE);
78 config = std::make_shared<cv::FileStorage>(config_path, cv::FileStorage::READ);
79 if (!fail_if_not_found && !
config->isOpened()) {
85 std::exit(EXIT_FAILURE);
89 #if ROS_AVAILABLE == 1 90 void set_node_handler(std::shared_ptr<ros::NodeHandle> nh_) { this->nh = nh_; }
94 #if ROS_AVAILABLE == 2 95 void set_node(std::shared_ptr<rclcpp::Node> &node_) { this->node = node_; }
122 template <
class T>
void parse_config(
const std::string &node_name, T &node_result,
bool required =
true) {
124 #if ROS_AVAILABLE == 1 125 if (nh !=
nullptr && nh->getParam(node_name, node_result)) {
127 nh->param<T>(node_name, node_result);
130 #elif ROS_AVAILABLE == 2 131 if (node !=
nullptr && node->has_parameter(node_name)) {
133 node->get_parameter<T>(node_name, node_result);
158 void parse_external(
const std::string &external_node_name,
const std::string &sensor_name,
const std::string &node_name, T &node_result,
159 bool required =
true) {
161 #if ROS_AVAILABLE == 1 162 std::string rosnode = sensor_name +
"_" + node_name;
163 if (nh !=
nullptr && nh->getParam(rosnode, node_result)) {
165 nh->param<T>(rosnode, node_result);
168 #elif ROS_AVAILABLE == 2 169 std::string rosnode = sensor_name +
"_" + node_name;
170 if (node !=
nullptr && node->has_parameter(rosnode)) {
172 node->get_parameter<T>(rosnode, node_result);
195 void parse_external(
const std::string &external_node_name,
const std::string &sensor_name,
const std::string &node_name,
196 Eigen::Matrix3d &node_result,
bool required =
true) {
198 #if ROS_AVAILABLE == 1 201 std::string rosnode = sensor_name +
"_" + node_name;
202 std::vector<double> matrix_RCtoI;
203 if (nh !=
nullptr && nh->getParam(rosnode, matrix_RCtoI)) {
205 nh->param<std::vector<double>>(rosnode, matrix_RCtoI);
206 node_result << matrix_RCtoI.at(0), matrix_RCtoI.at(1), matrix_RCtoI.at(2), matrix_RCtoI.at(3), matrix_RCtoI.at(4), matrix_RCtoI.at(5),
207 matrix_RCtoI.at(6), matrix_RCtoI.at(7), matrix_RCtoI.at(8);
210 #elif ROS_AVAILABLE == 2 213 std::string rosnode = sensor_name +
"_" + node_name;
214 std::vector<double> matrix_RCtoI;
215 if (node !=
nullptr && node->has_parameter(rosnode)) {
217 node->get_parameter<std::vector<double>>(rosnode, matrix_RCtoI);
218 node_result << matrix_RCtoI.at(0), matrix_RCtoI.at(1), matrix_RCtoI.at(2), matrix_RCtoI.at(3), matrix_RCtoI.at(4), matrix_RCtoI.at(5),
219 matrix_RCtoI.at(6), matrix_RCtoI.at(7), matrix_RCtoI.at(8);
242 void parse_external(
const std::string &external_node_name,
const std::string &sensor_name,
const std::string &node_name,
243 Eigen::Matrix4d &node_result,
bool required =
true) {
245 #if ROS_AVAILABLE == 1 248 std::string rosnode = sensor_name +
"_" + node_name;
249 std::vector<double> matrix_TCtoI;
250 if (nh !=
nullptr && nh->getParam(rosnode, matrix_TCtoI)) {
252 nh->param<std::vector<double>>(rosnode, matrix_TCtoI);
253 node_result << matrix_TCtoI.at(0), matrix_TCtoI.at(1), matrix_TCtoI.at(2), matrix_TCtoI.at(3), matrix_TCtoI.at(4), matrix_TCtoI.at(5),
254 matrix_TCtoI.at(6), matrix_TCtoI.at(7), matrix_TCtoI.at(8), matrix_TCtoI.at(9), matrix_TCtoI.at(10), matrix_TCtoI.at(11),
255 matrix_TCtoI.at(12), matrix_TCtoI.at(13), matrix_TCtoI.at(14), matrix_TCtoI.at(15);
258 #elif ROS_AVAILABLE == 2 261 std::string rosnode = sensor_name +
"_" + node_name;
262 std::vector<double> matrix_TCtoI;
263 if (node !=
nullptr && node->has_parameter(rosnode)) {
265 node->get_parameter<std::vector<double>>(rosnode, matrix_TCtoI);
266 node_result << matrix_TCtoI.at(0), matrix_TCtoI.at(1), matrix_TCtoI.at(2), matrix_TCtoI.at(3), matrix_TCtoI.at(4), matrix_TCtoI.at(5),
267 matrix_TCtoI.at(6), matrix_TCtoI.at(7), matrix_TCtoI.at(8), matrix_TCtoI.at(9), matrix_TCtoI.at(10), matrix_TCtoI.at(11),
268 matrix_TCtoI.at(12), matrix_TCtoI.at(13), matrix_TCtoI.at(14), matrix_TCtoI.at(15);
277 #if ROS_AVAILABLE == 2 278 void parse_config(
const std::string &node_name,
int &node_result,
bool required =
true) {
281 int64_t val = node_result;
282 if (node !=
nullptr && node->has_parameter(node_name)) {
284 node->get_parameter<int64_t>(node_name, val);
285 node_result = (int)val;
292 void parse_external(
const std::string &external_node_name,
const std::string &sensor_name,
const std::string &node_name,
293 std::vector<int> &node_result,
bool required =
true) {
294 std::vector<int64_t> val;
295 for (
auto tmp : node_result)
297 std::string rosnode = sensor_name +
"_" + node_name;
298 if (node !=
nullptr && node->has_parameter(rosnode)) {
300 node->get_parameter<std::vector<int64_t>>(rosnode, val);
303 node_result.push_back((
int)tmp);
320 #if ROS_AVAILABLE == 1 321 std::shared_ptr<ros::NodeHandle> nh;
325 #if ROS_AVAILABLE == 2 326 std::shared_ptr<rclcpp::Node> node =
nullptr;
336 static bool node_found(
const cv::FileNode &file_node,
const std::string &node_name) {
337 bool found_node =
false;
338 for (
const auto &item : file_node) {
339 if (item.name() == node_name) {
357 template <
class T>
void parse(
const cv::FileNode &file_node,
const std::string &node_name, T &node_result,
bool required =
true) {
362 PRINT_WARNING(
YELLOW "the node %s of type [%s] was not found...\n" RESET, node_name.c_str(),
typeid(node_result).name());
363 all_params_found_successfully =
false;
365 PRINT_DEBUG(
"the node %s of type [%s] was not found (not required)...\n", node_name.c_str(),
typeid(node_result).name());
372 file_node[node_name] >> node_result;
376 typeid(node_result).name());
377 all_params_found_successfully =
false;
379 PRINT_DEBUG(
"unable to parse %s node of type [%s] in the config file (not required)\n", node_name.c_str(),
380 typeid(node_result).name());
392 void parse(
const cv::FileNode &file_node,
const std::string &node_name,
bool &node_result,
bool required =
true) {
397 PRINT_WARNING(
YELLOW "the node %s of type [%s] was not found...\n" RESET, node_name.c_str(),
typeid(node_result).name());
398 all_params_found_successfully =
false;
400 PRINT_DEBUG(
"the node %s of type [%s] was not found (not required)...\n", node_name.c_str(),
typeid(node_result).name());
407 if (file_node[node_name].isInt() && (int)file_node[node_name] == 1) {
411 if (file_node[node_name].isInt() && (int)file_node[node_name] == 0) {
418 file_node[node_name] >> value;
419 value = value.substr(0, value.find_first_of(
'#'));
420 value = value.substr(0, value.find_first_of(
' '));
421 if (value ==
"1" || value ==
"true" || value ==
"True" || value ==
"TRUE") {
423 }
else if (value ==
"0" || value ==
"false" || value ==
"False" || value ==
"FALSE") {
427 all_params_found_successfully =
false;
432 typeid(node_result).name());
433 all_params_found_successfully =
false;
435 PRINT_DEBUG(
"unable to parse %s node of type [%s] in the config file (not required)\n", node_name.c_str(),
436 typeid(node_result).name());
448 void parse(
const cv::FileNode &file_node,
const std::string &node_name, Eigen::Matrix3d &node_result,
bool required =
true) {
453 PRINT_WARNING(
YELLOW "the node %s of type [%s] was not found...\n" RESET, node_name.c_str(),
typeid(node_result).name());
454 all_params_found_successfully =
false;
456 PRINT_DEBUG(
"the node %s of type [%s] was not found (not required)...\n", node_name.c_str(),
typeid(node_result).name());
462 node_result = Eigen::Matrix3d::Identity();
464 for (
int r = 0; r < (int)file_node[node_name].size() && r < 3; r++) {
465 for (
int c = 0; c < (int)file_node[node_name][r].size() && c < 3; c++) {
466 node_result(r, c) = (double)file_node[node_name][r][c];
472 typeid(node_result).name());
473 all_params_found_successfully =
false;
475 PRINT_DEBUG(
"unable to parse %s node of type [%s] in the config file (not required)\n", node_name.c_str(),
476 typeid(node_result).name());
488 void parse(
const cv::FileNode &file_node,
const std::string &node_name, Eigen::Matrix4d &node_result,
bool required =
true) {
491 std::string node_name_local = node_name;
492 if (node_name ==
"T_cam_imu" && !
node_found(file_node, node_name)) {
493 PRINT_INFO(
"parameter T_cam_imu not found, trying T_imu_cam instead (will return T_cam_imu still)!\n");
494 node_name_local =
"T_imu_cam";
495 }
else if (node_name ==
"T_imu_cam" && !
node_found(file_node, node_name)) {
496 PRINT_INFO(
"parameter T_imu_cam not found, trying T_cam_imu instead (will return T_imu_cam still)!\n");
497 node_name_local =
"T_cam_imu";
501 if (!
node_found(file_node, node_name_local)) {
503 PRINT_WARNING(
YELLOW "the node %s of type [%s] was not found...\n" RESET, node_name_local.c_str(),
typeid(node_result).name());
504 all_params_found_successfully =
false;
506 PRINT_DEBUG(
"the node %s of type [%s] was not found (not required)...\n", node_name_local.c_str(),
typeid(node_result).name());
512 node_result = Eigen::Matrix4d::Identity();
514 for (
int r = 0; r < (int)file_node[node_name_local].size() && r < 4; r++) {
515 for (
int c = 0; c < (int)file_node[node_name_local][r].size() && c < 4; c++) {
516 node_result(r, c) = (double)file_node[node_name_local][r][c];
522 typeid(node_result).name());
523 all_params_found_successfully =
false;
525 PRINT_DEBUG(
"unable to parse %s node of type [%s] in the config file (not required)\n", node_name.c_str(),
526 typeid(node_result).name());
531 if (node_name_local != node_name) {
532 Eigen::Matrix4d tmp(node_result);
548 template <
class T>
void parse_config_yaml(
const std::string &node_name, T &node_result,
bool required =
true) {
551 if (config ==
nullptr)
556 parse(config->root(), node_name, node_result, required);
559 typeid(node_result).name());
560 all_params_found_successfully =
false;
580 void parse_external_yaml(
const std::string &external_node_name,
const std::string &sensor_name,
const std::string &node_name,
581 T &node_result,
bool required =
true) {
583 if (config ==
nullptr)
588 if (!
node_found(config->root(), external_node_name)) {
589 PRINT_ERROR(
RED "the external node %s could not be found!\n" RESET, external_node_name.c_str());
590 std::exit(EXIT_FAILURE);
592 (*config)[external_node_name] >> path;
593 std::string relative_folder = config_path_.substr(0, config_path_.find_last_of(
'/')) +
"/";
596 auto config_external = std::make_shared<cv::FileStorage>(relative_folder + path, cv::FileStorage::READ);
597 if (!config_external->isOpened()) {
598 PRINT_ERROR(
RED "unable to open the configuration file!\n%s\n" RESET, (relative_folder + path).c_str());
599 std::exit(EXIT_FAILURE);
603 if (!
node_found(config_external->root(), sensor_name)) {
604 PRINT_WARNING(
YELLOW "the sensor %s of type [%s] was not found...\n" RESET, sensor_name.c_str(),
typeid(node_result).name());
605 all_params_found_successfully =
false;
611 parse((*config_external)[sensor_name], node_name, node_result, required);
613 PRINT_WARNING(
YELLOW "unable to parse %s node of type [%s] in [%s] in the external %s config file!\n" RESET, node_name.c_str(),
614 typeid(node_result).name(), sensor_name.c_str(), external_node_name.c_str());
615 all_params_found_successfully =
false;
bool all_params_found_successfully
Record if all parameters were found.
void parse(const cv::FileNode &file_node, const std::string &node_name, Eigen::Matrix3d &node_result, bool required=true)
Custom parser for camera extrinsic 3x3 transformations.
std::string get_config_folder()
Will get the folder this config file is in.
std::shared_ptr< cv::FileStorage > config
Our config file with the data in it.
void parse_config_yaml(const std::string &node_name, T &node_result, bool required=true)
Custom parser for the ESTIMATOR parameters.
void parse_external(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, T &node_result, bool required=true)
Custom parser for the external parameter files with levels.
void parse(const cv::FileNode &file_node, const std::string &node_name, Eigen::Matrix4d &node_result, bool required=true)
Custom parser for camera extrinsic 4x4 transformations.
YamlParser(const std::string &config_path, bool fail_if_not_found=true)
Constructor that loads all three configuration files.
void parse(const cv::FileNode &file_node, const std::string &node_name, T &node_result, bool required=true)
This function will try to get the requested parameter from our config.
Core algorithms for OpenVINS.
bool successful() const
Check to see if all parameters were read succesfully.
#define PRINT_WARNING(x...)
#define PRINT_ERROR(x...)
void parse_config(const std::string &node_name, T &node_result, bool required=true)
Custom parser for the ESTIMATOR parameters.
Eigen::Matrix4d Inv_se3(const Eigen::Matrix4d &T)
SE(3) matrix analytical inverse.
std::string config_path_
Path to the config file.
void parse_external(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, Eigen::Matrix3d &node_result, bool required=true)
Custom parser for Matrix3d in the external parameter files with levels.
void parse_external(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, Eigen::Matrix4d &node_result, bool required=true)
Custom parser for Matrix4d in the external parameter files with levels.
Helper class to do OpenCV yaml parsing from both file and ROS.
#define PRINT_DEBUG(x...)
static bool node_found(const cv::FileNode &file_node, const std::string &node_name)
Given a YAML node object, this check to see if we have a valid key.
void parse_external_yaml(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, T &node_result, bool required=true)
Custom parser for the EXTERNAL parameter files with levels.
void parse(const cv::FileNode &file_node, const std::string &node_name, bool &node_result, bool required=true)
Custom parser for booleans (0,false,False,FALSE=>false and 1,true,True,TRUE=>false) ...