39 #include <boost/uuid/random_generator.hpp>
40 #include <boost/uuid/uuid.hpp>
41 #include <boost/uuid/uuid_io.hpp>
43 #include <geometry_msgs/PointStamped.h>
44 #include <geometry_msgs/Vector3.h>
45 #include <visualization_msgs/MarkerArray.h>
54 #include <mesh_msgs/MeshGeometryStamped.h>
58 #include <visualization_msgs/Marker.h>
67 , private_nh(
"~/mesh_map/")
70 , layer_loader(
"mesh_map",
"mesh_map::AbstractLayer")
99 new dynamic_reconfigure::Server<mesh_map::MeshMapConfig>(
private_nh));
114 mesh_io_ptr = std::shared_ptr<lvr2::AttributeMeshIOBase>(
116 auto mesh_client_ptr = std::static_pointer_cast<mesh_client::MeshClient>(
mesh_io_ptr);
128 mesh_io_ptr = std::shared_ptr<lvr2::AttributeMeshIOBase>(hdf_5_mesh_io);
151 <<
mesh_ptr->numFaces() <<
" faces and "
152 <<
mesh_ptr->numEdges() <<
" edges.");
170 boost::uuids::random_generator gen;
171 boost::uuids::uuid uuid = gen();
172 uuid_str = boost::uuids::to_string(uuid);
176 if (face_normals_opt)
183 ROS_INFO_STREAM(
"No face normals found in the given map file, computing them...");
198 if (vertex_normals_opt)
205 ROS_INFO_STREAM(
"No vertex normals found in the given map file, computing them...");
222 if (edge_distances_opt)
272 ROS_WARN_STREAM(
"No layer plugins configured! - Use the param \"layers\" "
273 "in the namespace \""
275 <<
"\". \"layers\" must be must be a list of "
276 "tuples with a name and a type.");
282 for (
int i = 0; i < plugin_param_list.
size(); i++)
286 std::string name = elem[
"name"];
287 std::string type = elem[
"type"];
293 ROS_ERROR_STREAM(
"The plugin \"" << name <<
"\" has already been loaded! Names must be unique!");
308 std::pair<std::string, typename mesh_map::AbstractLayer::Ptr> elem(name, plugin_ptr);
314 << type <<
"\" has been loaded successfully under the name \"" << name <<
"\".");
318 ROS_ERROR_STREAM(
"Could not load the layer plugin with the name \"" << name <<
"\" and the type \"" << type
326 "has to be a list of structs "
327 <<
"with fields \"name\" and \"type\"!");
337 std::lock_guard<std::mutex> lock(
layer_mtx);
346 auto layer_iter =
layers.begin();
347 for (; layer_iter !=
layers.end(); layer_iter++)
350 lethals.insert(layer_iter->second->lethals().begin(), layer_iter->second->lethals().end());
352 if (layer_iter->first == layer_name)
357 layer_iter->second->defaultValue(), layer_iter->first,
360 if (layer_iter !=
layers.end())
365 for (; layer_iter !=
layers.end(); layer_iter++)
370 lethals.insert(layer_iter->second->lethals().begin(), layer_iter->second->lethals().end());
373 layer_iter->second->defaultValue(), layer_iter->first,
389 std::shared_ptr<mesh_map::MeshMap>
map(
this);
393 auto& layer_plugin =
layer.second;
394 const auto& layer_name =
layer.first;
396 auto callback = [
this](
const std::string& layer_name) {
layerChanged(layer_name); };
400 ROS_ERROR_STREAM(
"Could not initialize the layer plugin with the name \"" << layer_name <<
"\"!");
404 std::set<lvr2::VertexHandle> empty;
405 layer_plugin->updateLethal(
lethals, empty);
406 if (!layer_plugin->readLayer())
408 layer_plugin->computeLayer();
411 lethal_indices[layer_name].insert(layer_plugin->lethals().begin(), layer_plugin->lethals().end());
412 lethals.insert(layer_plugin->lethals().begin(), layer_plugin->lethals().end());
421 float combined_min = std::numeric_limits<float>::max();
422 float combined_max = std::numeric_limits<float>::min();
429 const auto& costs =
layer.second->costs();
434 const float norm_factor = factor /
norm;
436 <<
" factor: " << factor <<
" norm factor: " << norm_factor);
440 for (
auto vH :
mesh_ptr->vertices())
442 const float cost = costs.containsKey(vH) ? costs[vH] :
default_value;
443 if (std::isnan(cost))
446 if (std::isfinite(cost))
448 combined_max = std::max(combined_max,
vertex_costs[vH]);
449 combined_min = std::min(combined_min,
vertex_costs[vH]);
456 const float combined_norm = combined_max - combined_min;
460 vertex_costs[vH] = std::numeric_limits<float>::infinity();
471 std::array<lvr2::VertexHandle, 2> eH_vHs =
mesh_ptr->getVerticesOfEdge(eH);
476 if (
config.layer_factor != 0)
487 float vertex_factor =
config.layer_factor * cost_diff;
488 if (std::isnan(vertex_factor))
490 <<
" vertex_factor:" << vertex_factor <<
" cost_diff:" << cost_diff);
500 ROS_INFO(
"Successfully combined costs!");
506 std::vector<std::vector<lvr2::VertexHandle>> contours;
508 for (
auto contour : contours)
510 lethals.insert(contour.begin(), contour.end());
519 std::vector<std::vector<lvr2::VertexHandle>> tmp_contours;
521 array<lvr2::OptionalFaceHandle, 2> facepair;
523 for (
auto eHStart :
mesh_ptr->edges())
530 facepair =
mesh_ptr->getFacesOfEdge(eHStart);
533 if ((!facepair[0] || !facepair[1]) && !usedEdges[eHStart])
536 vector<lvr2::VertexHandle> contour;
538 array<lvr2::VertexHandle, 2> vertexPair =
mesh_ptr->getVerticesOfEdge(eHStart);
540 vector<lvr2::EdgeHandle> curEdges;
543 bool vertex_flag =
false;
549 usedEdges.
insert(eHTemp,
true);
550 local_usedEdges.
insert(eHTemp,
true);
552 vertexPair =
mesh_ptr->getVerticesOfEdge(eHTemp);
554 if (vH != vertexPair[0])
558 else if (vH != vertexPair[1])
564 usedVertices.
insert(vH,
true);
566 contour.push_back(vH);
567 mesh_ptr->getEdgesOfVertex(vH, curEdges);
570 for (
auto eHT : curEdges)
572 if (!usedEdges[eHT] && !local_usedEdges[eHT])
574 facepair =
mesh_ptr->getFacesOfEdge(eHT);
575 if (!facepair[0] || !facepair[1])
585 if (contour.size() > min_contour_size)
587 contours.push_back(contour);
602 const std::array<lvr2::VertexHandle, 3>& vertices,
603 const std::array<float, 3>& barycentric_coords)
612 if (a) vec += a.get() * barycentric_coords[0];
613 if (b) vec += b.get() * barycentric_coords[1];
614 if (c) vec += c.get() * barycentric_coords[2];
615 if (std::isfinite(vec.
x) && std::isfinite(vec.
y) && std::isfinite(vec.
z))
622 ROS_ERROR_THROTTLE(0.3,
"vector map does not contain any of the corresponding vectors");
628 const std::array<float, 3>& barycentric_coords)
634 const std::array<lvr2::VertexHandle, 3>& vertices,
635 const std::array<float, 3>& barycentric_coords)
637 const auto& a = costs.
get(vertices[0]);
638 const auto& b = costs.
get(vertices[1]);
639 const auto& c = costs.
get(vertices[2]);
643 std::array<float, 3> costs = { a.
get(), b.get(), c.get() };
646 return std::numeric_limits<float>::quiet_NaN();
651 visualization_msgs::Marker marker;
652 marker.header.frame_id =
mapFrame();
656 marker.type = visualization_msgs::Marker::SPHERE;
657 marker.action = visualization_msgs::Marker::ADD;
658 geometry_msgs::Vector3 scale;
662 marker.scale = scale;
664 geometry_msgs::Pose
p;
665 p.position.x = pos.
x;
666 p.position.y = pos.
y;
667 p.position.z = pos.
z;
669 marker.color =
color;
674 const std::string& name)
676 const auto& vertices =
mesh_ptr->getVerticesOfFace(face_handle);
677 visualization_msgs::Marker marker;
678 marker.header.frame_id =
mapFrame();
682 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
683 marker.action = visualization_msgs::Marker::ADD;
684 geometry_msgs::Vector3 scale;
688 marker.scale = scale;
690 for (
auto vertex : vertices)
692 auto& pos =
mesh_ptr->getVertexPosition(vertex);
693 geometry_msgs::Point
p;
697 marker.points.push_back(
p);
698 marker.colors.push_back(
color);
705 const bool publish_face_vectors)
722 auto opt_vec_map =
layer->vectorMap();
726 const auto& vecs = opt_vec_map.get();
729 auto opt_val = vertex_vectors.
get(vH);
730 vertex_vectors.
insert(vH, opt_val ? opt_val.get() + vecs[vH] : vecs[vH]);
731 for (
auto fH :
mesh_ptr->getFacesOfVertex(vH))
732 vector_field_faces[fH]++;
735 for (
auto fH : vector_field_faces)
737 if (vector_field_faces[fH] != 3)
740 const auto& vertices =
mesh_ptr->getVertexPositionsOfFace(fH);
741 const auto& vertex_handles =
mesh_ptr->getVerticesOfFace(fH);
743 std::array<float, 3> barycentric_coords;
747 auto opt_val = face_vectors.
get(fH);
748 auto vec_at =
layer->vectorAt(vertex_handles, barycentric_coords);
751 face_vectors.
insert(fH, opt_val ? opt_val.get() + vec_at : vec_at);
761 const std::function<
float(
float)>& cost_function,
const bool publish_face_vectors)
767 visualization_msgs::Marker vector_field;
769 geometry_msgs::Pose pose;
770 pose.position.x = pose.position.y = pose.position.z = 0;
771 pose.orientation.x = pose.orientation.y = pose.orientation.z = 0;
772 pose.orientation.w = 1;
773 vector_field.pose = pose;
775 vector_field.type = visualization_msgs::Marker::LINE_LIST;
776 vector_field.header.frame_id =
mapFrame();
778 vector_field.ns = name;
779 vector_field.scale.x = 0.01;
780 vector_field.color.a = 1;
783 vector_field.colors.reserve(2 *
vector_map.numValues());
784 vector_field.points.reserve(2 *
vector_map.numValues());
786 unsigned int cnt = 0;
787 unsigned int faces = 0;
790 std::set<lvr2::FaceHandle> complete_faces;
795 const float len2 = dir_vec.length2();
796 if (len2 == 0 || !std::isfinite(len2))
802 auto u =
mesh.getVertexPosition(vH);
803 auto v = u + dir_vec * 0.1;
808 if (!std::isfinite(u.x) || !std::isfinite(u.y) || !std::isfinite(u.z) || !std::isfinite(v.x) ||
809 !std::isfinite(v.y) || !std::isfinite(v.z))
813 vector_field.points.push_back(
toPoint(u));
814 vector_field.points.push_back(
toPoint(v));
816 const float value = cost_function ? cost_function(
values[vH]) :
values[vH];
818 vector_field.colors.push_back(
color);
819 vector_field.colors.push_back(
color);
827 for (
auto fH :
mesh.getFacesOfVertex(vH))
829 if (++vector_field_faces[fH] == 3)
832 complete_faces.insert(fH);
842 size_t invalid_cnt = 0;
855 if (publish_face_vectors)
857 vector_field.points.reserve(faces + cnt);
859 for (
auto fH : complete_faces)
861 const auto& vertices =
mesh.getVertexPositionsOfFace(fH);
862 const auto& vertex_handles =
mesh.getVerticesOfFace(fH);
864 std::array<float, 3> barycentric_coords;
872 const float& value = cost_function ? cost_function(cost) : cost;
879 auto v = u + dir_opt.get() * 0.1;
881 if (!std::isfinite(u.x) || !std::isfinite(u.y) || !std::isfinite(u.z) || !std::isfinite(v.x) ||
882 !std::isfinite(v.y) || !std::isfinite(v.z))
889 vector_field.points.push_back(
toPoint(u));
890 vector_field.points.push_back(
toPoint(v));
893 vector_field.colors.push_back(
color);
894 vector_field.colors.push_back(
color);
908 ROS_INFO_STREAM(
"Published vector field \"" << name <<
"\" with " << cnt <<
" elements.");
913 const auto& vertices =
mesh_ptr->getVerticesOfFace(face);
915 mesh_ptr->getVertexPosition(vertices[2]), dist, 0.0001);
918 boost::optional<std::tuple<lvr2::FaceHandle, std::array<Vector, 3>, std::array<float, 3>>>
921 const float& max_radius,
const float& max_dist)
923 std::list<lvr2::FaceHandle> possible_faces;
924 possible_faces.push_back(face);
925 std::list<lvr2::FaceHandle>::iterator face_iter = possible_faces.begin();
928 const auto& start_vertices =
mesh_ptr->getVertexPositionsOfFace(face);
929 for (
auto vertex : start_vertices)
935 float vertex_center_max = 0;
936 for (
auto vertex : start_vertices)
938 vertex_center_max = std::max(vertex_center_max, vertex.distance(center));
941 float ext_radius = max_radius + vertex_center_max;
942 float max_radius_sq = ext_radius * ext_radius;
945 in_list_map.
insert(face,
true);
947 std::array<float, 3> bary_coords;
949 while (possible_faces.end() != face_iter)
951 const auto& vertices =
mesh_ptr->getVertexPositionsOfFace(*face_iter);
955 return std::make_tuple(*face_iter, vertices, bary_coords);
959 const auto& vertices =
mesh_ptr->getVerticesOfFace(*face_iter);
960 for (
auto vertex : vertices)
966 const auto& nn_faces =
mesh_ptr->getFacesOfVertex(vertex);
967 for (
auto nn_face : nn_faces)
971 possible_faces.push_back(nn_face);
972 in_list_map.
insert(nn_face,
true);
991 std::array<float, 3> bary_coords;
1000 auto search_res = *search_res_opt;
1001 face = std::get<0>(search_res);
1002 std::array<Vector, 3> vertices = std::get<1>(search_res);
1003 bary_coords = std::get<2>(search_res);
1016 std::array<lvr2::VertexHandle, 3> handels =
mesh_ptr->getVerticesOfFace(face);
1020 dir +=
layer.second->vectorAt(handels, bary_coords);
1023 pos += dir * step_size;
1033 return std::get<0>(*search_result);
1037 boost::optional<std::tuple<lvr2::FaceHandle, std::array<mesh_map::Vector , 3>,
1039 Vector& position,
const float& max_dist)
1043 auto vH = vH_opt.unwrap();
1044 float min_triangle_position_distance = std::numeric_limits<float>::max();
1045 std::array<Vector, 3> vertices;
1046 std::array<float, 3> bary_coords;
1048 for(
auto fH :
mesh_ptr->getFacesOfVertex(vH))
1050 const auto& tmp_vertices =
mesh_ptr->getVertexPositionsOfFace(fH);
1052 std::array<float, 3> tmp_bary_coords;
1054 && std::fabs(dist) < max_dist)
1056 return std::make_tuple(fH, tmp_vertices, tmp_bary_coords);
1059 float triangle_dist = 0;
1060 triangle_dist += (vertices[0] - position).length2();
1061 triangle_dist += (vertices[1] - position).length2();
1062 triangle_dist += (vertices[2] - position).length2();
1063 if(triangle_dist < min_triangle_position_distance)
1065 min_triangle_position_distance = triangle_dist;
1067 vertices = tmp_vertices;
1068 bary_coords = tmp_bary_coords;
1073 return std::make_tuple(opt_fH.
unwrap(), vertices, bary_coords);
1084 float querry_point[3] = {pos.
x, pos.
y, pos.
z};
1087 size_t num_results =
kd_tree_ptr->knnSearch(&querry_point[0], 1, &ret_index, &out_dist_sqr);
1093 geometry_msgs::Point
p;
1103 std::array<float, 3>& barycentric_coords,
float& dist)
1105 const auto& face =
mesh_ptr->getVertexPositionsOfFace(triangle);
1116 const auto& face =
mesh_ptr->getVertexPositionsOfFace(triangle);
1121 const Vector& v2,
float& t,
float& u,
float& v,
Vector& p)
1129 float denom = N.
dot(N);
1134 float NdotRayDirection = N.
dot(dir);
1135 if (fabs(NdotRayDirection) <
kEpsilon)
1139 float d = N.
dot(v0);
1142 t = (N.
dot(orig) +
d) / NdotRayDirection;
1156 C = edge0.
cross(vp0);
1163 C = edge1.
cross(vp1);
1164 if ((u = N.
dot(C)) < 0)
1170 C = edge2.
cross(vp2);
1171 if ((v = N.
dot(C)) < 0)
1206 VertexColorMapOpt vertex_colors_opt = this->
mesh_io_ptr->getDenseAttributeMap<VertexColorMap>(
"vertex_colors");
1207 if (vertex_colors_opt)
1209 const VertexColorMap
colors = vertex_colors_opt.get();
1210 mesh_msgs::MeshVertexColorsStamped msg;
1214 msg.mesh_vertex_colors.vertex_colors.reserve(
colors.numValues());
1217 std_msgs::ColorRGBA color_rgba;
1218 const auto& color_array =
colors[vH];
1220 color_rgba.r = color_array[0] / 255.0;
1221 color_rgba.g = color_array[1] / 255.0;
1222 color_rgba.b = color_array[2] / 255.0;
1223 msg.mesh_vertex_colors.vertex_colors.push_back(color_rgba);
1241 if (cfg.cost_limit !=
config.cost_limit)