25 #include <boost/lexical_cast.hpp> 26 #include <ros_uri/ros_uri.hpp> 29 #include <rapidxml.hpp> 30 #include <rapidxml_utils.hpp> 37 ObjectDescriptor::ObjectDescriptor(std::string folder_path, std::string xml_path,
int instance_count,
bool use_pose_val) : valid_object_(false), instance_count_(instance_count), use_pose_val_(use_pose_val)
39 std::string package_path = folder_path;
40 folder_path = ros_uri::absolute_path(folder_path);
42 rapidxml::file<> xmlFile(xml_path.c_str());
43 rapidxml::xml_document<> doc;
44 doc.parse<0>(xmlFile.data());
47 rapidxml::xml_node<> *first_node = doc.first_node();
49 rapidxml::xml_node<> *node_name = first_node->first_node(
"name");
50 rapidxml::xml_node<> *node_surface_model = first_node->first_node(
"surface_model");
51 rapidxml::xml_node<> *node_mesh = first_node->first_node(
"mesh");
52 rapidxml::xml_node<> *node_rotation_type = first_node->first_node(
"rotation_model_type");
53 rapidxml::xml_node<> *node_orientation = first_node->first_node(
"model_orientation");
54 rapidxml::xml_node<> *node_diameter = first_node->first_node(
"diameter");
55 rapidxml::xml_node<> *node_score3D = first_node->first_node(
"score3D");
56 rapidxml::xml_node<> *node_desc_models = first_node->first_node(
"descriptor_models");
59 if (node_name && node_surface_model && node_mesh && node_rotation_type && node_orientation && node_diameter && node_score3D && node_desc_models) {
61 name_ = node_name->value();
62 surface_model_ = HalconCpp::HSurfaceModel((folder_path + node_surface_model->value()).c_str());
64 mesh_name_ = package_path + node_mesh->value();
67 diameter_ = boost::lexical_cast<
double>(node_diameter->value());
68 score_3D_ = boost::lexical_cast<
double>(node_score3D->value());
69 std::vector<ObjectViewDescriptor> object_descs;
70 for (rapidxml::xml_node<> *node_desc_model = node_desc_models->first_node(
"descriptor_model"); node_desc_model; node_desc_model = node_desc_model->next_sibling(
"descriptor_model")) {
71 rapidxml::xml_node<> *node_desc_model_name = node_desc_model->first_node(
"name");
72 rapidxml::xml_node<> *node_desc_model_orientation = node_desc_model->first_node(
"view_orientation");
73 rapidxml::xml_node<> *node_desc_model_score2D = node_desc_model->first_node(
"score2D");
74 rapidxml::xml_node<> *node_desc_model_use_color = node_desc_model->first_node(
"use_color");
75 rapidxml::xml_node<> *node_desc_model_vertical_offset = node_desc_model->first_node(
"vertical_texture_offset");
76 rapidxml::xml_node<> *node_desc_model_horizontal_offset = node_desc_model->first_node(
"horizontal_texture_offset");
77 rapidxml::xml_node<> *node_desc_model_invertible = node_desc_model->first_node(
"invertible");
78 rapidxml::xml_node<> *node_desc_model_bounding_box = node_desc_model->first_node(
"bounding_box");
79 if (node_desc_model_name && node_desc_model_orientation && node_desc_model_score2D && node_desc_model_use_color && node_desc_model_use_color && node_desc_model_vertical_offset && node_desc_model_horizontal_offset && node_desc_model_invertible && node_desc_model_bounding_box) {
80 std::vector<RotationAxis> axes;
81 std::vector<Eigen::Vector2i> box_corners;
83 rapidxml::xml_node<> *node_desc_model_axes = node_desc_model->first_node(
"rotation_axes");
84 if (node_desc_model_axes) {
85 rapidxml::xml_node<> *axis_1 = node_desc_model_axes->first_node(
"axis_1");
86 if (axis_1 && axis_1->first_attribute(
"angle")) {
87 double angle_1 = boost::lexical_cast<
double>(axis_1->first_attribute(
"angle")->value());
89 axes.push_back(rot_axis_1);
95 rapidxml::xml_node<> *axis_2 = node_desc_model_axes->first_node(
"axis_2");
96 if (axis_2 && axis_2->first_attribute(
"angle")) {
97 double angle_2 = boost::lexical_cast<
double>(axis_2->first_attribute(
"angle")->value());
99 axes.push_back(rot_axis_2);
115 for (rapidxml::xml_node<> *node_corner_point = node_desc_model_bounding_box->first_node(
"corner_point"); node_corner_point; node_corner_point = node_corner_point->next_sibling(
"corner_point")) {
118 if (box_corners.size() != 4) {
123 HalconCpp::HDescriptorModel desc_mdl((folder_path + node_desc_model_name->value()).c_str());
124 Eigen::Vector3d desc_orientation =
parseStringVector(node_desc_model_orientation->value(),
" ,");
125 double desc_score2D = boost::lexical_cast<
double>(node_desc_model_score2D->value());
126 bool desc_use_color = boost::lexical_cast<
bool>(node_desc_model_use_color->value());
127 int desc_vertical_offset = boost::lexical_cast<
int>(node_desc_model_vertical_offset->value());
128 int desc_horizontal_offset = boost::lexical_cast<
int>(node_desc_model_horizontal_offset->value());
129 bool desc_invertible = boost::lexical_cast<
bool>(node_desc_model_invertible->value());
130 object_descs.push_back(
ObjectViewDescriptor(desc_mdl, desc_orientation, desc_score2D, desc_use_color, desc_vertical_offset, desc_horizontal_offset, desc_invertible, axes, box_corners));
136 ROS_ERROR_STREAM(
"Could not create object: " << folder_path <<
" (node missing)");
139 }
catch(std::runtime_error err) {
140 ROS_ERROR_STREAM(
"Could not create object: " << folder_path <<
" (XML-parser error)");
141 }
catch(HalconCpp::HException exc) {
143 ROS_ERROR_STREAM(
"Could not create object: " << folder_path <<
" (Halcon error)");
144 }
catch(boost::bad_lexical_cast) {
145 ROS_ERROR_STREAM(
"Could not create object: " << folder_path <<
" (Boost cast error)");
146 }
catch (rapidxml::parse_error err) {
147 ROS_ERROR_STREAM(
"Could not create object: " << folder_path <<
" (XML-parser error)");
HalconCpp::HSurfaceModel surface_model_
std::vector< ObjectViewDescriptor > model_view_descriptors_
HalconCpp::HSurfaceModel getSurfaceModel() const
Eigen::Vector3d parseStringVector(std::string input_string, std::string delim)
Parses the given string and returns the 3D-double-vector described by it.
int getRotationModelType() const
bool isValid() const
Checks whether this object can be used for recognition.
static const int ROTATION_MODEL_SPHERE
Eigen::Vector3d orientation_
std::vector< ObjectViewDescriptor > getModelViewDescriptors() const
int getInstanceCount() const
std::string getMesh() const
double getScore3D() const
double getDiameter() const
void setUsePoseVal(bool usePoseVal)
bool getUsePoseVal() const
std::string getName() const
Eigen::Vector2i parseStringVector2i(std::string input_string, std::string delim)
Parses the given string and returns the 2D-int-vector described by it.
#define ROS_ERROR_STREAM(args)
static const int ROTATION_MODEL_NONE
ObjectDescriptor(std::string folder_path, std::string xml_path, int instance_count, bool use_pose_val)
The constructor of this class.
Eigen::Vector3d getOrientation() const