mesh_map.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2020, Sebastian Pütz
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  *
11  * 2. Redistributions in binary form must reproduce the above
12  * copyright notice, this list of conditions and the following
13  * disclaimer in the documentation and/or other materials provided
14  * with the distribution.
15  *
16  * 3. Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived
18  * from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * authors:
34  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
35  *
36  */
37 #include <XmlRpcException.h>
38 #include <algorithm>
39 #include <boost/uuid/random_generator.hpp>
40 #include <boost/uuid/uuid.hpp>
41 #include <boost/uuid/uuid_io.hpp>
42 #include <functional>
43 #include <geometry_msgs/PointStamped.h>
44 #include <geometry_msgs/Vector3.h>
45 #include <visualization_msgs/MarkerArray.h>
47 
48 #include <lvr2/geometry/Normal.hpp>
51 #include <lvr2/io/hdf5/MeshIO.hpp>
52 #include <mesh_map/mesh_map.h>
53 #include <mesh_map/util.h>
54 #include <mesh_msgs/MeshGeometryStamped.h>
56 #include <mutex>
57 #include <ros/ros.h>
58 #include <visualization_msgs/Marker.h>
59 
60 namespace mesh_map
61 {
64 
67  , private_nh("~/mesh_map/")
68  , first_config(true)
69  , map_loaded(false)
70  , layer_loader("mesh_map", "mesh_map::AbstractLayer")
71  , mesh_ptr(new lvr2::HalfEdgeMesh<Vector>())
72 {
73  private_nh.param<std::string>("server_url", srv_url, "");
74  private_nh.param<std::string>("server_username", srv_username, "");
75  private_nh.param<std::string>("server_password", srv_password, "");
76  private_nh.param<std::string>("mesh_layer", mesh_layer, "mesh0");
77  private_nh.param<float>("min_roughness", min_roughness, 0);
78  private_nh.param<float>("max_roughness", max_roughness, 0);
79  private_nh.param<float>("min_height_diff", min_height_diff, 0);
80  private_nh.param<float>("max_height_diff", max_height_diff, 0);
81  private_nh.param<float>("bb_min_x", bb_min_x, 0);
82  private_nh.param<float>("bb_min_y", bb_min_y, 0);
83  private_nh.param<float>("bb_min_z", bb_min_z, 0);
84  private_nh.param<float>("bb_max_x", bb_max_x, 0);
85  private_nh.param<float>("bb_max_y", bb_max_y, 0);
86  private_nh.param<float>("bb_max_z", bb_max_z, 0);
87 
88  private_nh.param<std::string>("mesh_file", mesh_file, "");
89  private_nh.param<std::string>("mesh_part", mesh_part, "");
90  private_nh.param<std::string>("global_frame", global_frame, "map");
91  ROS_INFO_STREAM("mesh file is set to: " << mesh_file);
92 
93  marker_pub = private_nh.advertise<visualization_msgs::Marker>("marker", 100, true);
94  mesh_geometry_pub = private_nh.advertise<mesh_msgs::MeshGeometryStamped>("mesh", 1, true);
95  vertex_costs_pub = private_nh.advertise<mesh_msgs::MeshVertexCostsStamped>("vertex_costs", 1, false);
96  vertex_colors_pub = private_nh.advertise<mesh_msgs::MeshVertexColorsStamped>("vertex_colors", 1, true);
97  vector_field_pub = private_nh.advertise<visualization_msgs::Marker>("vector_field", 1, true);
99  new dynamic_reconfigure::Server<mesh_map::MeshMapConfig>(private_nh));
100 
101  config_callback = boost::bind(&MeshMap::reconfigureCallback, this, _1, _2);
103 }
104 
106 {
107  ROS_INFO_STREAM("server url: " << srv_url);
108  bool server = false;
109 
110  if (!srv_url.empty())
111  {
112  server = true;
113 
114  mesh_io_ptr = std::shared_ptr<lvr2::AttributeMeshIOBase>(
116  auto mesh_client_ptr = std::static_pointer_cast<mesh_client::MeshClient>(mesh_io_ptr);
117 
118  mesh_client_ptr->setBoundingBox(bb_min_x, bb_min_y, bb_min_z, bb_max_x, bb_max_y, bb_max_z);
119  mesh_client_ptr->addFilter("roughness", min_roughness, max_roughness);
120  mesh_client_ptr->addFilter("height_diff", min_height_diff, max_height_diff);
121  }
122  else if (!mesh_file.empty() && !mesh_part.empty())
123  {
124  ROS_INFO_STREAM("Load \"" << mesh_part << "\" from file \"" << mesh_file << "\"...");
125  HDF5MeshIO* hdf_5_mesh_io = new HDF5MeshIO();
126  hdf_5_mesh_io->open(mesh_file);
127  hdf_5_mesh_io->setMeshName(mesh_part);
128  mesh_io_ptr = std::shared_ptr<lvr2::AttributeMeshIOBase>(hdf_5_mesh_io);
129  }
130  else
131  {
132  ROS_ERROR_STREAM("Could not open file or server connection!");
133  return false;
134  }
135 
136  if (server)
137  {
138  ROS_INFO_STREAM("Start reading the mesh from the server '" << srv_url);
139  }
140  else
141  {
142  ROS_INFO_STREAM("Start reading the mesh part '" << mesh_part << "' from the map file '" << mesh_file << "'...");
143  }
144 
145  auto mesh_opt = mesh_io_ptr->getMesh();
146 
147  if (mesh_opt)
148  {
149  *mesh_ptr = mesh_opt.get();
150  ROS_INFO_STREAM("The mesh has been loaded successfully with " << mesh_ptr->numVertices() << " vertices and "
151  << mesh_ptr->numFaces() << " faces and "
152  << mesh_ptr->numEdges() << " edges.");
153 
154  adaptor_ptr = std::make_unique<NanoFlannMeshAdaptor>(*mesh_ptr);
155  kd_tree_ptr = std::make_unique<KDTree>(3,*adaptor_ptr, nanoflann::KDTreeSingleIndexAdaptorParams(10));
156  kd_tree_ptr->buildIndex();
157  ROS_INFO_STREAM("The k-d tree has been build successfully!");
158  }
159  else
160  {
161  ROS_ERROR_STREAM("Could not load the mesh '" << mesh_part << "' from the map file '" << mesh_file << "' ");
162  return false;
163  }
164 
165  vertex_costs = lvr2::DenseVertexMap<float>(mesh_ptr->nextVertexIndex(), 0);
166  edge_weights = lvr2::DenseEdgeMap<float>(mesh_ptr->nextEdgeIndex(), 0);
167  invalid = lvr2::DenseVertexMap<bool>(mesh_ptr->nextVertexIndex(), false);
168 
169  // TODO read and write uuid
170  boost::uuids::random_generator gen;
171  boost::uuids::uuid uuid = gen();
172  uuid_str = boost::uuids::to_string(uuid);
173 
174  auto face_normals_opt = mesh_io_ptr->getDenseAttributeMap<lvr2::DenseFaceMap<Normal>>("face_normals");
175 
176  if (face_normals_opt)
177  {
178  face_normals = face_normals_opt.get();
179  ROS_INFO_STREAM("Found " << face_normals.numValues() << " face normals in map file.");
180  }
181  else
182  {
183  ROS_INFO_STREAM("No face normals found in the given map file, computing them...");
185  ROS_INFO_STREAM("Computed " << face_normals.numValues() << " face normals.");
186  if (mesh_io_ptr->addDenseAttributeMap(face_normals, "face_normals"))
187  {
188  ROS_INFO_STREAM("Saved face normals to map file.");
189  }
190  else
191  {
192  ROS_ERROR_STREAM("Could not save face normals to map file!");
193  }
194  }
195 
196  auto vertex_normals_opt = mesh_io_ptr->getDenseAttributeMap<lvr2::DenseVertexMap<Normal>>("vertex_normals");
197 
198  if (vertex_normals_opt)
199  {
200  vertex_normals = vertex_normals_opt.get();
201  ROS_INFO_STREAM("Found " << vertex_normals.numValues() << " vertex normals in map file!");
202  }
203  else
204  {
205  ROS_INFO_STREAM("No vertex normals found in the given map file, computing them...");
207  if (mesh_io_ptr->addDenseAttributeMap(vertex_normals, "vertex_normals"))
208  {
209  ROS_INFO_STREAM("Saved vertex normals to map file.");
210  }
211  else
212  {
213  ROS_ERROR_STREAM("Could not save vertex normals to map file!");
214  }
215  }
216 
217  mesh_geometry_pub.publish(mesh_msgs_conversions::toMeshGeometryStamped<float>(*mesh_ptr, global_frame, uuid_str, vertex_normals));
218 
219  ROS_INFO_STREAM("Try to read edge distances from map file...");
220  auto edge_distances_opt = mesh_io_ptr->getAttributeMap<lvr2::DenseEdgeMap<float>>("edge_distances");
221 
222  if (edge_distances_opt)
223  {
224  ROS_INFO_STREAM("Vertex distances have been read successfully.");
225  edge_distances = edge_distances_opt.get();
226  }
227  else
228  {
229  ROS_INFO_STREAM("Computing edge distances...");
231  ROS_INFO_STREAM("Saving " << edge_distances.numValues() << " edge distances to map file...");
232 
233  if (mesh_io_ptr->addAttributeMap(edge_distances, "edge_distances"))
234  {
235  ROS_INFO_STREAM("Saved edge distances to map file.");
236  }
237  else
238  {
239  ROS_ERROR_STREAM("Could not save edge distances to map file!");
240  }
241  }
242 
243  ROS_INFO_STREAM("Load layer plugins...");
244  if (!loadLayerPlugins())
245  {
246  ROS_FATAL_STREAM("Could not load any layer plugin!");
247  return false;
248  }
249 
250  ROS_INFO_STREAM("Initialize layer plugins...");
251  if (!initLayerPlugins())
252  {
253  ROS_FATAL_STREAM("Could not initialize plugins!");
254  return false;
255  }
256 
257  sleep(1);
258 
262 
263  map_loaded = true;
264  return true;
265 }
266 
268 {
269  XmlRpc::XmlRpcValue plugin_param_list;
270  if (!private_nh.getParam("layers", plugin_param_list))
271  {
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.");
277  return false;
278  }
279 
280  try
281  {
282  for (int i = 0; i < plugin_param_list.size(); i++)
283  {
284  XmlRpc::XmlRpcValue elem = plugin_param_list[i];
285 
286  std::string name = elem["name"];
287  std::string type = elem["type"];
288 
289  typename AbstractLayer::Ptr plugin_ptr;
290 
291  if (layer_names.find(name) != layer_names.end())
292  {
293  ROS_ERROR_STREAM("The plugin \"" << name << "\" has already been loaded! Names must be unique!");
294  return false;
295  }
296 
297  try
298  {
299  plugin_ptr = layer_loader.createInstance(type);
300  }
302  {
303  ROS_ERROR_STREAM(e.what());
304  }
305 
306  if (plugin_ptr)
307  {
308  std::pair<std::string, typename mesh_map::AbstractLayer::Ptr> elem(name, plugin_ptr);
309 
310  layers.push_back(elem);
311  layer_names.insert(elem);
312 
313  ROS_INFO_STREAM("The layer plugin with the type \""
314  << type << "\" has been loaded successfully under the name \"" << name << "\".");
315  }
316  else
317  {
318  ROS_ERROR_STREAM("Could not load the layer plugin with the name \"" << name << "\" and the type \"" << type
319  << "\"!");
320  }
321  }
322  }
323  catch (XmlRpc::XmlRpcException& e)
324  {
325  ROS_ERROR_STREAM("Invalid parameter structure. The \"layers\" parameter "
326  "has to be a list of structs "
327  << "with fields \"name\" and \"type\"!");
329  return false;
330  }
331  // is there any layer plugin loaded for the map?
332  return !layers.empty();
333 }
334 
335 void MeshMap::layerChanged(const std::string& layer_name)
336 {
337  std::lock_guard<std::mutex> lock(layer_mtx);
338 
339  ROS_INFO_STREAM("Layer \"" << layer_name << "\" changed.");
340 
341  lethals.clear();
342 
343  ROS_INFO_STREAM("Combine underlining lethal sets...");
344 
345  // TODO pre-compute combined lethals upto a layer level
346  auto layer_iter = layers.begin();
347  for (; layer_iter != layers.end(); layer_iter++)
348  {
349  // TODO add lethal and removae lethal sets
350  lethals.insert(layer_iter->second->lethals().begin(), layer_iter->second->lethals().end());
351  // TODO merge with std::set_merge
352  if (layer_iter->first == layer_name)
353  break;
354  }
355 
356  vertex_costs_pub.publish(mesh_msgs_conversions::toVertexCostsStamped(layer_iter->second->costs(), mesh_ptr->numVertices(),
357  layer_iter->second->defaultValue(), layer_iter->first,
359 
360  if (layer_iter != layers.end())
361  layer_iter++;
362 
363  ROS_INFO_STREAM("Combine lethal sets...");
364 
365  for (; layer_iter != layers.end(); layer_iter++)
366  {
367  // TODO add lethal and remove lethal sets as param
368  layer_iter->second->updateLethal(lethals, lethals);
369 
370  lethals.insert(layer_iter->second->lethals().begin(), layer_iter->second->lethals().end());
371 
372  vertex_costs_pub.publish(mesh_msgs_conversions::toVertexCostsStamped(layer_iter->second->costs(), mesh_ptr->numVertices(),
373  layer_iter->second->defaultValue(), layer_iter->first,
375  }
376 
377  ROS_INFO_STREAM("Found " << lethals.size() << " lethal vertices");
378  ROS_INFO_STREAM("Combine layer costs...");
379 
381  // TODO new lethals old lethals -> renew potential field! around this areas
382 }
383 
385 {
386  lethals.clear();
387  lethal_indices.clear();
388 
389  std::shared_ptr<mesh_map::MeshMap> map(this);
390 
391  for (auto& layer : layers)
392  {
393  auto& layer_plugin = layer.second;
394  const auto& layer_name = layer.first;
395 
396  auto callback = [this](const std::string& layer_name) { layerChanged(layer_name); };
397 
398  if (!layer_plugin->initialize(layer_name, callback, map, mesh_ptr, mesh_io_ptr))
399  {
400  ROS_ERROR_STREAM("Could not initialize the layer plugin with the name \"" << layer_name << "\"!");
401  return false;
402  }
403 
404  std::set<lvr2::VertexHandle> empty;
405  layer_plugin->updateLethal(lethals, empty);
406  if (!layer_plugin->readLayer())
407  {
408  layer_plugin->computeLayer();
409  }
410 
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());
413  }
414  return true;
415 }
416 
418 {
419  ROS_INFO_STREAM("Combining costs...");
420 
421  float combined_min = std::numeric_limits<float>::max();
422  float combined_max = std::numeric_limits<float>::min();
423 
424  vertex_costs = lvr2::DenseVertexMap<float>(mesh_ptr->nextVertexIndex(), 0);
425 
426  bool hasNaN = false;
427  for (auto layer : layers)
428  {
429  const auto& costs = layer.second->costs();
430  float min, max;
431  mesh_map::getMinMax(costs, min, max);
432  const float norm = max - min;
433  const float factor = private_nh.param<float>(layer.first + "/factor", 1.0);
434  const float norm_factor = factor / norm;
435  ROS_INFO_STREAM("Layer \"" << layer.first << "\" max value: " << max << " min value: " << min << " norm: " << norm
436  << " factor: " << factor << " norm factor: " << norm_factor);
437 
438  const float default_value = layer.second->defaultValue();
439  hasNaN = false;
440  for (auto vH : mesh_ptr->vertices())
441  {
442  const float cost = costs.containsKey(vH) ? costs[vH] : default_value;
443  if (std::isnan(cost))
444  hasNaN = true;
445  vertex_costs[vH] += factor * cost;
446  if (std::isfinite(cost))
447  {
448  combined_max = std::max(combined_max, vertex_costs[vH]);
449  combined_min = std::min(combined_min, vertex_costs[vH]);
450  }
451  }
452  if (hasNaN)
453  ROS_ERROR_STREAM("Layer \"" << layer.first << "\" contains NaN values!");
454  }
455 
456  const float combined_norm = combined_max - combined_min;
457 
458  for (auto vH : lethals)
459  {
460  vertex_costs[vH] = std::numeric_limits<float>::infinity();
461  }
462 
464 
465  hasNaN = false;
466 
467  ROS_INFO_STREAM("Layer weighting factor is: " << config.layer_factor);
468  for (auto eH : mesh_ptr->edges())
469  {
470  // Get both Vertices of the current Edge
471  std::array<lvr2::VertexHandle, 2> eH_vHs = mesh_ptr->getVerticesOfEdge(eH);
472  const lvr2::VertexHandle& vH1 = eH_vHs[0];
473  const lvr2::VertexHandle& vH2 = eH_vHs[1];
474  // Get the Riskiness for the current Edge (the maximum value from both
475  // Vertices)
476  if (config.layer_factor != 0)
477  {
478  if (std::isinf(vertex_costs[vH1]) || std::isinf(vertex_costs[vH2]))
479  {
480  edge_weights[eH] = edge_distances[eH];
481  // edge_weights[eH] = std::numeric_limits<float>::infinity();
482  }
483  else
484  {
485  float cost_diff = std::fabs(vertex_costs[vH1] - vertex_costs[vH2]);
486 
487  float vertex_factor = config.layer_factor * cost_diff;
488  if (std::isnan(vertex_factor))
489  ROS_INFO_STREAM("NaN: v1:" << vertex_costs[vH1] << " v2:" << vertex_costs[vH2]
490  << " vertex_factor:" << vertex_factor << " cost_diff:" << cost_diff);
491  edge_weights[eH] = edge_distances[eH] * (1 + vertex_factor);
492  }
493  }
494  else
495  {
496  edge_weights[eH] = edge_distances[eH];
497  }
498  }
499 
500  ROS_INFO("Successfully combined costs!");
501 }
502 
503 void MeshMap::findLethalByContours(const int& min_contour_size, std::set<lvr2::VertexHandle>& lethals)
504 {
505  int size = lethals.size();
506  std::vector<std::vector<lvr2::VertexHandle>> contours;
507  findContours(contours, min_contour_size);
508  for (auto contour : contours)
509  {
510  lethals.insert(contour.begin(), contour.end());
511  }
512  ROS_INFO_STREAM("Found " << lethals.size() - size << " lethal vertices as contour vertices");
513 }
514 
515 void MeshMap::findContours(std::vector<std::vector<lvr2::VertexHandle>>& contours, int min_contour_size)
516 {
517  ROS_INFO_STREAM("Find contours...");
518 
519  std::vector<std::vector<lvr2::VertexHandle>> tmp_contours;
520 
521  array<lvr2::OptionalFaceHandle, 2> facepair;
522  lvr2::SparseEdgeMap<bool> usedEdges(false);
523  for (auto eHStart : mesh_ptr->edges())
524  {
525  lvr2::SparseVertexMap<bool> usedVertices(false);
526  lvr2::SparseEdgeMap<bool> local_usedEdges(false);
527  int count = 0;
528 
529  // Look for border Edges
530  facepair = mesh_ptr->getFacesOfEdge(eHStart);
531 
532  // If border Edge found
533  if ((!facepair[0] || !facepair[1]) && !usedEdges[eHStart])
534  {
535  std:
536  vector<lvr2::VertexHandle> contour;
537  // Set vector which links to the following Edge
538  array<lvr2::VertexHandle, 2> vertexPair = mesh_ptr->getVerticesOfEdge(eHStart);
539  lvr2::VertexHandle vH = vertexPair[1];
540  vector<lvr2::EdgeHandle> curEdges;
541  lvr2::EdgeHandle eHTemp = eHStart;
542  bool moving = true;
543  bool vertex_flag = false;
544 
545  // While the conotur did not come full circle
546  while (moving)
547  {
548  moving = false;
549  usedEdges.insert(eHTemp, true);
550  local_usedEdges.insert(eHTemp, true);
551  // Set vector which links to the following Edge
552  vertexPair = mesh_ptr->getVerticesOfEdge(eHTemp);
553  // Eliminate the possibility to choose the previous Vertex
554  if (vH != vertexPair[0])
555  {
556  vH = vertexPair[0];
557  }
558  else if (vH != vertexPair[1])
559  {
560  vH = vertexPair[1];
561  }
562 
563  // Add the current Vertex to the contour
564  usedVertices.insert(vH, true);
565  count++;
566  contour.push_back(vH);
567  mesh_ptr->getEdgesOfVertex(vH, curEdges);
568 
569  // Look for other edge of vertex that is a border Edge
570  for (auto eHT : curEdges)
571  {
572  if (!usedEdges[eHT] && !local_usedEdges[eHT])
573  {
574  facepair = mesh_ptr->getFacesOfEdge(eHT);
575  if (!facepair[0] || !facepair[1])
576  {
577  eHTemp = eHT;
578  moving = true;
579  continue;
580  }
581  }
582  }
583  }
584  // Add contour to list of contours
585  if (contour.size() > min_contour_size)
586  {
587  contours.push_back(contour);
588  }
589  }
590  }
591 
592  ROS_INFO_STREAM("Found " << contours.size() << " contours.");
593 }
594 
596 {
597  this->vector_map = vector_map;
598 }
599 
600 boost::optional<Vector> MeshMap::directionAtPosition(
601  const lvr2::VertexMap<lvr2::BaseVector<float>>& vector_map,
602  const std::array<lvr2::VertexHandle, 3>& vertices,
603  const std::array<float, 3>& barycentric_coords)
604 {
605  const auto& a = vector_map.get(vertices[0]);
606  const auto& b = vector_map.get(vertices[1]);
607  const auto& c = vector_map.get(vertices[2]);
608 
609  if (a || b || c)
610  {
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))
616  return vec;
617  else
618  ROS_ERROR_THROTTLE(0.3, "vector map contains invalid vectors!");
619  }
620  else
621  {
622  ROS_ERROR_THROTTLE(0.3, "vector map does not contain any of the corresponding vectors");
623  }
624  return boost::none;
625 }
626 
627 float MeshMap::costAtPosition(const std::array<lvr2::VertexHandle, 3>& vertices,
628  const std::array<float, 3>& barycentric_coords)
629 {
630  return costAtPosition(vertex_costs, vertices, barycentric_coords);
631 }
632 
634  const std::array<lvr2::VertexHandle, 3>& vertices,
635  const std::array<float, 3>& barycentric_coords)
636 {
637  const auto& a = costs.get(vertices[0]);
638  const auto& b = costs.get(vertices[1]);
639  const auto& c = costs.get(vertices[2]);
640 
641  if (a && b && c)
642  {
643  std::array<float, 3> costs = { a.get(), b.get(), c.get() };
644  return mesh_map::linearCombineBarycentricCoords(costs, barycentric_coords);
645  }
646  return std::numeric_limits<float>::quiet_NaN();
647 }
648 
649 void MeshMap::publishDebugPoint(const Vector pos, const std_msgs::ColorRGBA& color, const std::string& name)
650 {
651  visualization_msgs::Marker marker;
652  marker.header.frame_id = mapFrame();
653  marker.header.stamp = ros::Time();
654  marker.ns = name;
655  marker.id = 0;
656  marker.type = visualization_msgs::Marker::SPHERE;
657  marker.action = visualization_msgs::Marker::ADD;
658  geometry_msgs::Vector3 scale;
659  scale.x = 0.05;
660  scale.y = 0.05;
661  scale.z = 0.05;
662  marker.scale = scale;
663 
664  geometry_msgs::Pose p;
665  p.position.x = pos.x;
666  p.position.y = pos.y;
667  p.position.z = pos.z;
668  marker.pose = p;
669  marker.color = color;
670  marker_pub.publish(marker);
671 }
672 
673 void MeshMap::publishDebugFace(const lvr2::FaceHandle& face_handle, const std_msgs::ColorRGBA& color,
674  const std::string& name)
675 {
676  const auto& vertices = mesh_ptr->getVerticesOfFace(face_handle);
677  visualization_msgs::Marker marker;
678  marker.header.frame_id = mapFrame();
679  marker.header.stamp = ros::Time();
680  marker.ns = name;
681  marker.id = 0;
682  marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
683  marker.action = visualization_msgs::Marker::ADD;
684  geometry_msgs::Vector3 scale;
685  scale.x = 1.0;
686  scale.y = 1.0;
687  scale.z = 1.0;
688  marker.scale = scale;
689 
690  for (auto vertex : vertices)
691  {
692  auto& pos = mesh_ptr->getVertexPosition(vertex);
693  geometry_msgs::Point p;
694  p.x = pos.x;
695  p.y = pos.y;
696  p.z = pos.z;
697  marker.points.push_back(p);
698  marker.colors.push_back(color);
699  }
700  marker_pub.publish(marker);
701 }
702 
703 void MeshMap::publishVectorField(const std::string& name,
705  const bool publish_face_vectors)
706 {
707  publishVectorField(name, vector_map, vertex_costs, {}, publish_face_vectors);
708 }
709 
711 {
712  lvr2::DenseVertexMap<Vector> vertex_vectors;
713  lvr2::DenseFaceMap<Vector> face_vectors;
714 
715  vertex_vectors.reserve(mesh_ptr->nextVertexIndex());
716  face_vectors.reserve(mesh_ptr->nextFaceIndex());
717 
718  for (auto layer_iter : layer_names)
719  {
720  lvr2::DenseFaceMap<uint8_t> vector_field_faces(mesh_ptr->nextFaceIndex(), 0);
721  AbstractLayer::Ptr layer = layer_iter.second;
722  auto opt_vec_map = layer->vectorMap();
723  if (!opt_vec_map)
724  continue;
725 
726  const auto& vecs = opt_vec_map.get();
727  for (auto vH : vecs)
728  {
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]++;
733  }
734 
735  for (auto fH : vector_field_faces)
736  {
737  if (vector_field_faces[fH] != 3)
738  continue;
739 
740  const auto& vertices = mesh_ptr->getVertexPositionsOfFace(fH);
741  const auto& vertex_handles = mesh_ptr->getVerticesOfFace(fH);
742  mesh_map::Vector center = (vertices[0] + vertices[1] + vertices[2]) / 3;
743  std::array<float, 3> barycentric_coords;
744  float dist;
745  if (mesh_map::projectedBarycentricCoords(center, vertices, barycentric_coords, dist))
746  {
747  auto opt_val = face_vectors.get(fH);
748  auto vec_at = layer->vectorAt(vertex_handles, barycentric_coords);
749  if (vec_at != Vector())
750  {
751  face_vectors.insert(fH, opt_val ? opt_val.get() + vec_at : vec_at);
752  }
753  }
754  }
755  }
756 }
757 
758 void MeshMap::publishVectorField(const std::string& name,
760  const lvr2::DenseVertexMap<float>& values,
761  const std::function<float(float)>& cost_function, const bool publish_face_vectors)
762 {
763  const auto& mesh = this->mesh();
764  const auto& vertex_costs = vertexCosts();
765  const auto& face_normals = faceNormals();
766 
767  visualization_msgs::Marker vector_field;
768 
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;
774 
775  vector_field.type = visualization_msgs::Marker::LINE_LIST;
776  vector_field.header.frame_id = mapFrame();
777  vector_field.header.stamp = ros::Time::now();
778  vector_field.ns = name;
779  vector_field.scale.x = 0.01;
780  vector_field.color.a = 1;
781  vector_field.id = 0;
782 
783  vector_field.colors.reserve(2 * vector_map.numValues());
784  vector_field.points.reserve(2 * vector_map.numValues());
785 
786  unsigned int cnt = 0;
787  unsigned int faces = 0;
788 
789  lvr2::DenseFaceMap<uint8_t> vector_field_faces(mesh.numFaces(), 0);
790  std::set<lvr2::FaceHandle> complete_faces;
791 
792  for (auto vH : vector_map)
793  {
794  const auto& dir_vec = vector_map[vH];
795  const float len2 = dir_vec.length2();
796  if (len2 == 0 || !std::isfinite(len2))
797  {
798  ROS_DEBUG_STREAM_THROTTLE(0.3, "Found invalid direction vector in vector field \"" << name << "\". Ignoring it!");
799  continue;
800  }
801 
802  auto u = mesh.getVertexPosition(vH);
803  auto v = u + dir_vec * 0.1;
804 
805  u.z = u.z + 0.01;
806  v.z = v.z + 0.01;
807 
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))
810  {
811  continue;
812  }
813  vector_field.points.push_back(toPoint(u));
814  vector_field.points.push_back(toPoint(v));
815 
816  const float value = cost_function ? cost_function(values[vH]) : values[vH];
817  std_msgs::ColorRGBA color = getRainbowColor(value);
818  vector_field.colors.push_back(color);
819  vector_field.colors.push_back(color);
820 
821  cnt++;
822  // vector.header.seq = cnt;
823  // vector.id = cnt++;
824  // vector_field.markers.push_back(vector);
825  try
826  {
827  for (auto fH : mesh.getFacesOfVertex(vH))
828  {
829  if (++vector_field_faces[fH] == 3)
830  {
831  faces++;
832  complete_faces.insert(fH);
833  }
834  }
835  }
836  catch (lvr2::PanicException exception)
837  {
838  invalid.insert(vH, true);
839  }
840  }
841 
842  size_t invalid_cnt = 0;
843  for (auto vH : invalid)
844  {
845  if (invalid[vH])
846  invalid_cnt++;
847  }
848 
849  if (invalid_cnt > 0)
850  {
851  ROS_WARN_STREAM("Found " << invalid_cnt << " non manifold vertices!");
852  }
853  ROS_INFO_STREAM("Found " << faces << " complete vector faces!");
854 
855  if (publish_face_vectors)
856  {
857  vector_field.points.reserve(faces + cnt);
858 
859  for (auto fH : complete_faces)
860  {
861  const auto& vertices = mesh.getVertexPositionsOfFace(fH);
862  const auto& vertex_handles = mesh.getVerticesOfFace(fH);
863  mesh_map::Vector center = (vertices[0] + vertices[1] + vertices[2]) / 3;
864  std::array<float, 3> barycentric_coords;
865  float dist;
866  if (mesh_map::projectedBarycentricCoords(center, vertices, barycentric_coords, dist))
867  {
868  boost::optional<mesh_map::Vector> dir_opt = directionAtPosition(vector_map, vertex_handles, barycentric_coords);
869  if (dir_opt)
870  {
871  const float& cost = costAtPosition(values, vertex_handles, barycentric_coords);
872  const float& value = cost_function ? cost_function(cost) : cost;
873 
874  // vector.color = getRainbowColor(value);
875  // vector.pose = mesh_map::calculatePoseFromDirection(
876  // center, dir_opt.get(), face_normals[fH]);
877 
878  auto u = center;
879  auto v = u + dir_opt.get() * 0.1;
880 
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))
883  {
884  continue;
885  }
886 
887  // vector_field.header.seq = cnt;
888  // vector_field.id = cnt++;
889  vector_field.points.push_back(toPoint(u));
890  vector_field.points.push_back(toPoint(v));
891 
892  std_msgs::ColorRGBA color = getRainbowColor(value);
893  vector_field.colors.push_back(color);
894  vector_field.colors.push_back(color);
895  }
896  else
897  {
898  ROS_ERROR_STREAM_THROTTLE(0.3, "Could not compute the direction!");
899  }
900  }
901  else
902  {
903  ROS_ERROR_STREAM_THROTTLE(0.3, "Could not compute the barycentric coords!");
904  }
905  }
906  }
907  vector_field_pub.publish(vector_field);
908  ROS_INFO_STREAM("Published vector field \"" << name << "\" with " << cnt << " elements.");
909 }
910 
911 bool MeshMap::inTriangle(const Vector& pos, const lvr2::FaceHandle& face, const float& dist)
912 {
913  const auto& vertices = mesh_ptr->getVerticesOfFace(face);
914  return mesh_map::inTriangle(pos, mesh_ptr->getVertexPosition(vertices[0]), mesh_ptr->getVertexPosition(vertices[1]),
915  mesh_ptr->getVertexPosition(vertices[2]), dist, 0.0001);
916 }
917 
918 boost::optional<std::tuple<lvr2::FaceHandle, std::array<Vector, 3>, std::array<float, 3>>>
920  const Vector& pos, const lvr2::FaceHandle& face,
921  const float& max_radius, const float& max_dist)
922 {
923  std::list<lvr2::FaceHandle> possible_faces;
924  possible_faces.push_back(face);
925  std::list<lvr2::FaceHandle>::iterator face_iter = possible_faces.begin();
926 
927  Vector center(0, 0, 0);
928  const auto& start_vertices = mesh_ptr->getVertexPositionsOfFace(face);
929  for (auto vertex : start_vertices)
930  {
931  center += vertex;
932  }
933  center /= 3;
934 
935  float vertex_center_max = 0;
936  for (auto vertex : start_vertices)
937  {
938  vertex_center_max = std::max(vertex_center_max, vertex.distance(center));
939  }
940 
941  float ext_radius = max_radius + vertex_center_max;
942  float max_radius_sq = ext_radius * ext_radius;
943 
944  lvr2::SparseFaceMap<bool> in_list_map;
945  in_list_map.insert(face, true);
946 
947  std::array<float, 3> bary_coords;
948 
949  while (possible_faces.end() != face_iter)
950  {
951  const auto& vertices = mesh_ptr->getVertexPositionsOfFace(*face_iter);
952  float dist;
953  if (mesh_map::projectedBarycentricCoords(pos, vertices, bary_coords, dist) && std::fabs(dist) < max_dist)
954  {
955  return std::make_tuple(*face_iter, vertices, bary_coords);
956  }
957  else
958  {
959  const auto& vertices = mesh_ptr->getVerticesOfFace(*face_iter);
960  for (auto vertex : vertices)
961  {
962  if (center.distance2(mesh_ptr->getVertexPosition(vertex)) < max_radius_sq)
963  {
964  try
965  {
966  const auto& nn_faces = mesh_ptr->getFacesOfVertex(vertex);
967  for (auto nn_face : nn_faces)
968  {
969  if (!in_list_map.containsKey(nn_face))
970  {
971  possible_faces.push_back(nn_face);
972  in_list_map.insert(nn_face, true);
973  }
974  }
975  }
976  catch (lvr2::PanicException exception)
977  {
978  // TODO handle case properly
979  }
980  }
981  }
982  ++face_iter;
983  }
984  }
985 
986  return boost::none;
987 }
988 
989 bool MeshMap::meshAhead(mesh_map::Vector& pos, lvr2::FaceHandle& face, const float& step_size)
990 {
991  std::array<float, 3> bary_coords;
992  float dist;
993  // get barycentric coordinates of the current face or the next neighbour face
994  if (mesh_map::projectedBarycentricCoords(pos, mesh_ptr->getVertexPositionsOfFace(face), bary_coords, dist))
995  {
996 
997  }
998  else if (auto search_res_opt = searchNeighbourFaces(pos, face, step_size, 0.4))
999  {
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);
1004 
1005  // project position onto surface
1006  pos = mesh_map::linearCombineBarycentricCoords(vertices, bary_coords);
1007  }
1008  else
1009  {
1010  return false;
1011  }
1012  const auto& opt_dir = directionAtPosition(vector_map, mesh_ptr->getVerticesOfFace(face), bary_coords);
1013  if (opt_dir)
1014  {
1015  Vector dir = opt_dir.get().normalized();
1016  std::array<lvr2::VertexHandle, 3> handels = mesh_ptr->getVerticesOfFace(face);
1017  // iter over all layer vector fields
1018  for (auto layer : layers)
1019  {
1020  dir += layer.second->vectorAt(handels, bary_coords);
1021  }
1022  dir.normalize();
1023  pos += dir * step_size;
1024  return true;
1025  }
1026  return false;
1027 }
1028 
1030 {
1031  auto search_result = searchContainingFace(position, max_dist);
1032  if(search_result)
1033  return std::get<0>(*search_result);
1034  return lvr2::OptionalFaceHandle();
1035 }
1036 
1037 boost::optional<std::tuple<lvr2::FaceHandle, std::array<mesh_map::Vector , 3>,
1038  std::array<float, 3>>> MeshMap::searchContainingFace(
1039  Vector& position, const float& max_dist)
1040 {
1041  if(auto vH_opt = getNearestVertexHandle(position))
1042  {
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;
1047  lvr2::OptionalFaceHandle opt_fH;
1048  for(auto fH : mesh_ptr->getFacesOfVertex(vH))
1049  {
1050  const auto& tmp_vertices = mesh_ptr->getVertexPositionsOfFace(fH);
1051  float dist = 0;
1052  std::array<float, 3> tmp_bary_coords;
1053  if (mesh_map::projectedBarycentricCoords(position, vertices, tmp_bary_coords, dist)
1054  && std::fabs(dist) < max_dist)
1055  {
1056  return std::make_tuple(fH, tmp_vertices, tmp_bary_coords);
1057  }
1058 
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)
1064  {
1065  min_triangle_position_distance = triangle_dist;
1066  opt_fH = fH;
1067  vertices = tmp_vertices;
1068  bary_coords = tmp_bary_coords;
1069  }
1070  }
1071  if(opt_fH)
1072  {
1073  return std::make_tuple(opt_fH.unwrap(), vertices, bary_coords);
1074  }
1075  ROS_ERROR_STREAM("No containing face found!");
1076  return boost::none;
1077  }
1078  ROS_FATAL_STREAM("Could not find the nearest vertex");
1079  return boost::none;
1080 }
1081 
1083 {
1084  float querry_point[3] = {pos.x, pos.y, pos.z};
1085  size_t ret_index;
1086  float out_dist_sqr;
1087  size_t num_results = kd_tree_ptr->knnSearch(&querry_point[0], 1, &ret_index, &out_dist_sqr);
1088  return num_results == 0 ? lvr2::OptionalVertexHandle() : lvr2::VertexHandle(ret_index);
1089 }
1090 
1091 inline const geometry_msgs::Point MeshMap::toPoint(const Vector& vec)
1092 {
1093  geometry_msgs::Point p;
1094  p.x = vec.x;
1095  p.y = vec.y;
1096  p.z = vec.z;
1097  return p;
1098 }
1099 
1100 constexpr float kEpsilon = 1e-8;
1101 
1103  std::array<float, 3>& barycentric_coords, float& dist)
1104 {
1105  const auto& face = mesh_ptr->getVertexPositionsOfFace(triangle);
1106  return mesh_map::projectedBarycentricCoords(p, face, barycentric_coords, dist);
1107 }
1108 
1109 mesh_map::AbstractLayer::Ptr MeshMap::layer(const std::string& layer_name)
1110 {
1111  return layer_names[layer_name];
1112 }
1113 
1114 bool MeshMap::barycentricCoords(const Vector& p, const lvr2::FaceHandle& triangle, float& u, float& v, float& w)
1115 {
1116  const auto& face = mesh_ptr->getVertexPositionsOfFace(triangle);
1117  return mesh_map::barycentricCoords(p, face[0], face[1], face[2], u, v, w);
1118 }
1119 
1120 bool MeshMap::rayTriangleIntersect(const Vector& orig, const Vector& dir, const Vector& v0, const Vector& v1,
1121  const Vector& v2, float& t, float& u, float& v, Vector& p)
1122 {
1123  // compute plane's normal
1124  Vector v0v1 = v1 - v0;
1125  Vector v0v2 = v2 - v0;
1126 
1127  // no need to normalize
1128  Vector N = v0v1.cross(v0v2); // N
1129  float denom = N.dot(N);
1130 
1131  // Step 1: finding P
1132 
1133  // check if ray and plane are parallel ?
1134  float NdotRayDirection = N.dot(dir);
1135  if (fabs(NdotRayDirection) < kEpsilon) // almost 0
1136  return false; // they are parallel so they don't intersect !
1137 
1138  // compute d parameter using equation 2
1139  float d = N.dot(v0);
1140 
1141  // compute t (equation 3)
1142  t = (N.dot(orig) + d) / NdotRayDirection;
1143 
1144  // check if the triangle is in behind the ray
1145  // if (t < 0) return false; // the triangle is behind
1146 
1147  // compute the intersection point using equation 1
1148  p = orig + dir * t;
1149 
1150  // Step 2: inside-outside test
1151  Vector C; // vector perpendicular to triangle's plane
1152 
1153  // edge 0
1154  Vector edge0 = v1 - v0;
1155  Vector vp0 = p - v0;
1156  C = edge0.cross(vp0);
1157  if (N.dot(C) < 0)
1158  return false; // P is on the right side
1159 
1160  // edge 1
1161  Vector edge1 = v2 - v1;
1162  Vector vp1 = p - v1;
1163  C = edge1.cross(vp1);
1164  if ((u = N.dot(C)) < 0)
1165  return false; // P is on the right side
1166 
1167  // edge 2
1168  Vector edge2 = v0 - v2;
1169  Vector vp2 = p - v2;
1170  C = edge2.cross(vp2);
1171  if ((v = N.dot(C)) < 0)
1172  return false; // P is on the right side;
1173 
1174  u /= denom;
1175  v /= denom;
1176 
1177  return true; // this ray hits the triangle
1178 }
1179 
1181 {
1182  return true; // TODO implement
1183 }
1184 
1186 {
1187  for (auto& layer : layers)
1188  {
1190  layer.second->defaultValue(), layer.first, global_frame,
1191  uuid_str));
1192  }
1194 }
1195 
1196 void MeshMap::publishVertexCosts(const lvr2::VertexMap<float>& costs, const std::string& name)
1197 {
1200 }
1201 
1203 {
1204  using VertexColorMapOpt = lvr2::DenseVertexMapOptional<std::array<uint8_t, 3>>;
1205  using VertexColorMap = lvr2::DenseVertexMap<std::array<uint8_t, 3>>;
1206  VertexColorMapOpt vertex_colors_opt = this->mesh_io_ptr->getDenseAttributeMap<VertexColorMap>("vertex_colors");
1207  if (vertex_colors_opt)
1208  {
1209  const VertexColorMap colors = vertex_colors_opt.get();
1210  mesh_msgs::MeshVertexColorsStamped msg;
1211  msg.header.frame_id = mapFrame();
1212  msg.header.stamp = ros::Time::now();
1213  msg.uuid = uuid_str;
1214  msg.mesh_vertex_colors.vertex_colors.reserve(colors.numValues());
1215  for (auto vH : colors)
1216  {
1217  std_msgs::ColorRGBA color_rgba;
1218  const auto& color_array = colors[vH];
1219  color_rgba.a = 1;
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);
1224  }
1225  this->vertex_colors_pub.publish(msg);
1226  }
1227 }
1228 
1229 void MeshMap::reconfigureCallback(mesh_map::MeshMapConfig& cfg, uint32_t level)
1230 {
1231  ROS_INFO_STREAM("Dynamic reconfigure callback...");
1232  if (first_config)
1233  {
1234  config = cfg;
1235  first_config = false;
1236  return;
1237  }
1238 
1239  if (!first_config && map_loaded)
1240  {
1241  if (cfg.cost_limit != config.cost_limit)
1242  {
1244  }
1245 
1246  config = cfg;
1247  }
1248 }
1249 
1250 const std::string MeshMap::getGlobalFrameID()
1251 {
1252  return global_frame;
1253 }
1254 
1255 } /* namespace mesh_map */
mesh_map::MeshMap::publishDebugFace
void publishDebugFace(const lvr2::FaceHandle &face_handle, const std_msgs::ColorRGBA &color, const std::string &name)
Publishes a triangle as marker. Used for debug purposes.
Definition: mesh_map.cpp:673
XmlRpc::XmlRpcValue::size
int size() const
mesh_map::MeshMap::loadLayerPlugins
bool loadLayerPlugins()
Loads all configures layer plugins.
Definition: mesh_map.cpp:267
ROS_ERROR_THROTTLE
#define ROS_ERROR_THROTTLE(period,...)
mesh_map::MeshMap::directionAtPosition
boost::optional< Vector > directionAtPosition(const lvr2::VertexMap< lvr2::BaseVector< float >> &vector_map, const std::array< lvr2::VertexHandle, 3 > &vertices, const std::array< float, 3 > &barycentric_coords)
Definition: mesh_map.cpp:600
ROS_ERROR_STREAM_THROTTLE
#define ROS_ERROR_STREAM_THROTTLE(period, args)
mesh_map::MeshMap::lethals
std::set< lvr2::VertexHandle > lethals
all impassable vertices
Definition: mesh_map.h:417
mesh_map::MeshMap::mesh
const lvr2::HalfEdgeMesh< Vector > & mesh()
Returns the stored mesh.
Definition: mesh_map.h:268
min
int min(int a, int b)
lvr2::Hdf5IO< lvr2::hdf5features::ArrayIO, lvr2::hdf5features::ChannelIO, lvr2::hdf5features::VariantChannelIO, lvr2::hdf5features::MeshIO >
mesh_map::MeshMap::vertex_costs_pub
ros::Publisher vertex_costs_pub
publisher for vertex costs
Definition: mesh_map.h:466
lvr2::PanicException
lvr2::BaseVector::normalized
Normal< CoordT > normalized() const
lvr2::BaseVector< float >
lvr2::HashMap
mesh_map::MeshMap::vertexCosts
const lvr2::DenseVertexMap< float > & vertexCosts()
Returns the stored combined costs.
Definition: mesh_map.h:276
mesh_map::MeshMap::config
MeshMapConfig config
current mesh map configuration
Definition: mesh_map.h:493
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
mesh_map::MeshMap::mapFrame
const std::string & mapFrame()
Returns the map frame / coordinate system id.
Definition: mesh_map.h:284
boost::shared_ptr
mesh_map::MeshMap::combineVertexCosts
void combineVertexCosts()
A method which combines all layer costs with the respective weightings.
Definition: mesh_map.cpp:417
mesh_map::MeshMap::max_roughness
float max_roughness
Definition: mesh_map.h:434
mesh_map::MeshMap::invalid
lvr2::DenseVertexMap< bool > invalid
Definition: mesh_map.h:401
XmlRpc::XmlRpcException::getMessage
const std::string & getMessage() const
mesh_map
Definition: abstract_layer.h:47
lvr2::FaceHandle
mesh_map::MeshMap::faceNormals
const lvr2::DenseFaceMap< Normal > & faceNormals()
Returns the mesh's triangle normals.
Definition: mesh_map.h:292
mesh_map::MeshMap::mesh_part
std::string mesh_part
Definition: mesh_map.h:445
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ROS_FATAL_STREAM
#define ROS_FATAL_STREAM(args)
ros.h
mesh_map::MeshMap::rayTriangleIntersect
bool rayTriangleIntersect(const Vector &orig, const Vector &dir, const Vector &v0, const Vector &v1, const Vector &v2, float &t, float &u, float &v, Vector &p)
Definition: mesh_map.cpp:1120
mesh_map::MeshMap::face_normals
lvr2::DenseFaceMap< Normal > face_normals
triangle normals
Definition: mesh_map.h:460
lvr2::AttributeMap
mesh_map::MeshMap::mesh_layer
std::string mesh_layer
Definition: mesh_map.h:431
NormalAlgorithms.hpp
mesh_map::MeshMap::edge_weights
lvr2::DenseEdgeMap< float > edge_weights
edge weights
Definition: mesh_map.h:457
lvr2::Hdf5IO
mesh_map::linearCombineBarycentricCoords
T linearCombineBarycentricCoords(const std::array< T, 3 > &vertex_properties, const std::array< float, 3 > &barycentric_coords)
Computes a linear combination of vertex properties and the barycentric coordinates.
Definition: util.h:169
lvr2::OptionalVertexHandle
mesh_map::MeshMap::srv_username
std::string srv_username
login username to connect to the server
Definition: mesh_map.h:426
lvr2::HashMap::containsKey
bool containsKey(HandleT key) const final
mesh_map::MeshMap::edge_distances
lvr2::DenseEdgeMap< float > edge_distances
vertex distance for each edge
Definition: mesh_map.h:454
mesh_map::MeshMap::min_height_diff
float min_height_diff
Definition: mesh_map.h:435
lvr2::DenseVertexMapOptional
boost::optional< DenseVertexMap< ValueT > > DenseVertexMapOptional
mesh_map::color
std_msgs::ColorRGBA color(const float &r, const float &g, const float &b, const float &a=1.0)
Function to build std_msgs color instances.
Definition: util.cpp:149
mesh_map::MeshMap::bb_max_x
float bb_max_x
Definition: mesh_map.h:440
mesh_map::MeshMap::resetLayers
bool resetLayers()
Resets all layers.
Definition: mesh_map.cpp:1180
mesh_map::MeshMap::layer_mtx
std::mutex layer_mtx
layer mutex to handle simultaneous layer changes
Definition: mesh_map.h:505
mesh_map::MeshMap::mesh_geometry_pub
ros::Publisher mesh_geometry_pub
publisher for the mesh geometry
Definition: mesh_map.h:472
mesh_map::MeshMap::marker_pub
ros::Publisher marker_pub
publisher for the debug markers
Definition: mesh_map.h:475
mesh_map::MeshMap::lethal_indices
std::map< std::string, std::set< lvr2::VertexHandle > > lethal_indices
each layer maps to a set of impassable indices
Definition: mesh_map.h:414
lvr2::BaseVector::distance2
CoordT distance2(const BaseVector &other) const
p
SharedPointer p
mesh_map::MeshMap::bb_min_y
float bb_min_y
Definition: mesh_map.h:438
mesh_map::MeshMap::layers
std::vector< std::pair< std::string, mesh_map::AbstractLayer::Ptr > > layers
vector of name and layer instances
Definition: mesh_map.h:411
mesh_map::MeshMap::adaptor_ptr
std::unique_ptr< NanoFlannMeshAdaptor > adaptor_ptr
k-d tree nano flann mesh adaptor to access mesh data
Definition: mesh_map.h:513
mesh_map::MeshMap::toPoint
const geometry_msgs::Point toPoint(const Vector &vec)
Converts a vector to a ROS geometry_msgs/Point message.
Definition: mesh_map.cpp:1091
lvr2::BaseVector::x
CoordT x
lvr2::BaseVector::cross
BaseVector< CoordT > cross(const BaseVector &other) const
mesh_map::MeshMap::searchNeighbourFaces
boost::optional< std::tuple< lvr2::FaceHandle, std::array< Vector, 3 >, std::array< float, 3 > > > searchNeighbourFaces(const Vector &pos, const lvr2::FaceHandle &face, const float &max_radius, const float &max_dist)
Definition: mesh_map.cpp:919
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
BaseOptionalHandle< Index, FaceHandle >::unwrap
FaceHandle unwrap() const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
mesh_msgs_conversions::toVertexCostsStamped
const mesh_msgs::MeshVertexCostsStamped toVertexCostsStamped(const lvr2::DenseVertexMap< float > &costs, const std::string &name, const std::string &frame_id, const std::string &uuid, const ros::Time &stamp=ros::Time::now())
mesh_map::MeshMap::getContainingFace
lvr2::OptionalFaceHandle getContainingFace(Vector &position, const float &max_dist)
Searches for a triangle which contains the given position with respect to the maximum distance.
Definition: mesh_map.cpp:1029
mesh_client.h
mesh_map::MeshMap::vertex_costs
lvr2::DenseVertexMap< float > vertex_costs
combined layer costs
Definition: mesh_map.h:448
mesh_map::Vector
lvr2::BaseVector< float > Vector
use vectors with datatype folat
Definition: abstract_layer.h:49
mesh_map::MeshMap::publishVectorField
void publishVectorField(const std::string &name, const lvr2::DenseVertexMap< lvr2::BaseVector< float >> &vector_map, const bool publish_face_vectors=false)
Publishes a vector field as visualisation_msgs/Marker.
Definition: mesh_map.cpp:703
lvr2::EdgeHandle
mesh_map::MeshMap::kd_tree_ptr
std::unique_ptr< KDTree > kd_tree_ptr
k-d tree to query mesh vertices in logarithmic time
Definition: mesh_map.h:516
lvr2::VectorMap
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
mesh_map::MeshMap::reconfigure_server_ptr
boost::shared_ptr< dynamic_reconfigure::Server< mesh_map::MeshMapConfig > > reconfigure_server_ptr
shared pointer to dynamic reconfigure server
Definition: mesh_map.h:481
mesh_map::MeshMap::uuid_str
std::string uuid_str
uuid for the load mesh map
Definition: mesh_map.h:502
mesh_map::MeshMap::getNearestVertexHandle
lvr2::OptionalVertexHandle getNearestVertexHandle(const mesh_map::Vector &pos)
Return and optional vertex handle of to the closest vertex to the given position.
Definition: mesh_map.cpp:1082
nanoflann::KDTreeSingleIndexAdaptorParams
Definition: nanoflann.hpp:547
mesh_map::MeshMap::vector_map
lvr2::DenseVertexMap< mesh_map::Vector > vector_map
stored vector map to share between planner and controller
Definition: mesh_map.h:451
mesh_map::MeshMap::bb_max_y
float bb_max_y
Definition: mesh_map.h:441
map
OutMapT< typename InMapT::HandleType, std::result_of_t< MapF(typename InMapT::ValueType)> > map(const InMapT &mapIn, MapF func)
mesh_map::MeshMap::MeshMap
MeshMap(tf2_ros::Buffer &tf)
Definition: mesh_map.cpp:65
mesh_map::MeshMap::srv_password
std::string srv_password
login password to connect to the server
Definition: mesh_map.h:429
mesh_map::MeshMap::mesh_file
std::string mesh_file
Definition: mesh_map.h:444
mesh_map::MeshMap::initLayerPlugins
bool initLayerPlugins()
Initialized all loaded layer plugins.
Definition: mesh_map.cpp:384
mesh_map::MeshMap::mesh_ptr
std::shared_ptr< lvr2::HalfEdgeMesh< Vector > > mesh_ptr
Definition: mesh_map.h:399
lvr2::OptionalFaceHandle
mesh_map::MeshMap::inTriangle
bool inTriangle(const Vector &pos, const lvr2::FaceHandle &face, const float &dist)
return true if the given position lies inside the triangle with respect to the given maximum distance...
Definition: mesh_map.cpp:911
mesh_map::MeshMap::first_config
bool first_config
first reconfigure call
Definition: mesh_map.h:487
Normal.hpp
lvr2::BaseVector::y
CoordT y
lvr2::VertexHandle
lvr2::hdf5features::VariantChannelIO
mesh_map::projectedBarycentricCoords
bool projectedBarycentricCoords(const Vector &p, const std::array< Vector, 3 > &vertices, std::array< float, 3 > &barycentric_coords, float &dist)
Computes projected barycentric coordinates and whether the query point lies inside or outside the giv...
Definition: util.cpp:120
mesh_map::inTriangle
bool inTriangle(const Vector &p, const Vector &v0, const Vector &v1, const Vector &v2, const float &max_dist, const float &epsilon)
Computes whether a points lies inside or outside a triangle with respect to a maximum distance while ...
Definition: util.cpp:100
mesh_map::MeshMap::private_nh
ros::NodeHandle private_nh
private node handle within the mesh map namespace
Definition: mesh_map.h:496
lvr2::calcVertexDistances
DenseEdgeMap< float > calcVertexDistances(const BaseMesh< BaseVecT > &mesh)
mesh_map::MeshMap::layer
mesh_map::AbstractLayer::Ptr layer(const std::string &layer_name)
returns a shared pointer to the specified layer
Definition: mesh_map.cpp:1109
lvr2::VectorMap::get
boost::optional< const ValueT & > get(HandleT key) const final
values
values
d
d
ROS_DEBUG_STREAM_THROTTLE
#define ROS_DEBUG_STREAM_THROTTLE(period, args)
tf2_ros::Buffer
mesh_map::MeshMap::min_roughness
float min_roughness
Definition: mesh_map.h:433
mesh_map::MeshMap::barycentricCoords
bool barycentricCoords(const Vector &p, const lvr2::FaceHandle &triangle, float &u, float &v, float &w)
Definition: mesh_map.cpp:1114
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
mesh_map::MeshMap::projectedBarycentricCoords
bool projectedBarycentricCoords(const Vector &p, const lvr2::FaceHandle &triangle, std::array< float, 3 > &barycentric_coords, float &dist)
Computes the projected barycentric coordinates, it implements Heidrich's method See https://www....
Definition: mesh_map.cpp:1102
mesh_map::getMinMax
void getMinMax(const lvr2::VertexMap< float > &map, float &min, float &max)
Function to compute the minimum and maximum of a cost layer.
Definition: util.cpp:47
colors
colors
mesh_map::MeshMap::mesh_io_ptr
std::shared_ptr< lvr2::AttributeMeshIOBase > mesh_io_ptr
Definition: mesh_map.h:398
mesh_map::MeshMap::publishCombinedVectorField
void publishCombinedVectorField()
Publishes the vector field as visualisation_msgs/Marker.
Definition: mesh_map.cpp:710
GeometryAlgorithms.hpp
XmlRpc::XmlRpcException
mesh_map::kEpsilon
constexpr float kEpsilon
Definition: mesh_map.cpp:1100
mesh_map::MeshMap::bb_min_x
float bb_min_x
Definition: mesh_map.h:437
lvr2::Hdf5IO::open
void open(std::string filename)
mesh_map::MeshMap::map_loaded
bool map_loaded
indicates whether the map has been loaded
Definition: mesh_map.h:490
mesh_map::MeshMap::publishCostLayers
void publishCostLayers()
Publishes all layer costs as mesh_msgs/VertexCosts.
Definition: mesh_map.cpp:1185
lvr2::BaseVector::z
CoordT z
mesh_map::MeshMap::vertex_normals
lvr2::DenseVertexMap< Normal > vertex_normals
vertex normals
Definition: mesh_map.h:463
pluginlib::ClassLoader::createInstance
boost::shared_ptr< T > createInstance(const std::string &lookup_name)
ros::Time
mesh_map::MeshMap::layer_names
std::map< std::string, mesh_map::AbstractLayer::Ptr > layer_names
mapping from name to layer instance
Definition: mesh_map.h:408
mesh_map::MeshMap::getGlobalFrameID
const std::string getGlobalFrameID()
Returns the global frame / coordinate system id string.
Definition: mesh_map.cpp:1250
mesh_map.h
mesh_map::MeshMap::searchContainingFace
boost::optional< std::tuple< lvr2::FaceHandle, std::array< mesh_map::Vector, 3 >, std::array< float, 3 > > > searchContainingFace(Vector &position, const float &max_dist)
Searches for a triangle which contains the given position with respect to the maximum distance.
Definition: mesh_map.cpp:1038
mesh_map::MeshMap::costAtPosition
float costAtPosition(const lvr2::DenseVertexMap< float > &costs, const std::array< lvr2::VertexHandle, 3 > &vertices, const std::array< float, 3 > &barycentric_coords)
Definition: mesh_map.cpp:633
tf_buffer
tf2_ros::Buffer * tf_buffer
std
mesh_map::MeshMap::vector_field_pub
ros::Publisher vector_field_pub
publisher for the stored vector field
Definition: mesh_map.h:478
lvr2
mesh_map::MeshMap::setVectorMap
void setVectorMap(lvr2::DenseVertexMap< mesh_map::Vector > &vector_map)
Stores the given vector map.
Definition: mesh_map.cpp:595
mesh_map::MeshMap::layerChanged
void layerChanged(const std::string &layer_name)
Callback function which is called from inside a layer plugin if cost values change.
Definition: mesh_map.cpp:335
mesh_map::MeshMap::max_height_diff
float max_height_diff
Definition: mesh_map.h:436
mesh_map::MeshMap::config_callback
dynamic_reconfigure::Server< mesh_map::MeshMapConfig >::CallbackType config_callback
dynamic reconfigure callback function binding
Definition: mesh_map.h:484
default_value
def default_value(type_)
lvr2::VectorMap::insert
boost::optional< ValueT > insert(HandleT key, const ValueT &value) final
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
mesh_map::MeshMap::bb_max_z
float bb_max_z
Definition: mesh_map.h:442
lvr2::BaseVector::dot
CoordT dot(const BaseVector &other) const
pluginlib::LibraryLoadException
MeshIO.hpp
norm
__kf_device__ float norm(const float3 &v)
conversions.h
lvr2::calcVertexNormals
DenseVertexMap< Normal< typename BaseVecT::CoordType > > calcVertexNormals(const BaseMesh< BaseVecT > &mesh, const FaceMap< Normal< typename BaseVecT::CoordType >> &normals)
XmlRpcException.h
mesh_map::getRainbowColor
std_msgs::ColorRGBA getRainbowColor(const float value)
map value to color on color rainbow
Definition: util.cpp:201
lvr2::HashMap::insert
boost::optional< ValueT > insert(HandleT key, const ValueT &value) final
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
mesh_map::MeshMap::findLethalByContours
void findLethalByContours(const int &min_contour_size, std::set< lvr2::VertexHandle > &lethals)
Compute all contours and returns the corresponding vertices to use these as lethal vertices.
Definition: mesh_map.cpp:503
mesh_map::MeshMap::publishDebugPoint
void publishDebugPoint(const Vector pos, const std_msgs::ColorRGBA &color, const std::string &name)
Publishes a position as marker. Used for debug purposes.
Definition: mesh_map.cpp:649
mesh_map::MeshMap::bb_min_z
float bb_min_z
Definition: mesh_map.h:439
mesh_map::MeshMap::layer_loader
pluginlib::ClassLoader< mesh_map::AbstractLayer > layer_loader
plugin class loader for for the layer plugins
Definition: mesh_map.h:405
mesh_map::MeshMap::readMap
bool readMap()
Reads in the mesh geometry, normals and cost values and publishes all as mesh_msgs.
Definition: mesh_map.cpp:105
lvr2::VectorMap::reserve
void reserve(size_t newCap)
ROS_INFO
#define ROS_INFO(...)
mesh_map::MeshMap::vertex_colors_pub
ros::Publisher vertex_colors_pub
publisher for vertex colors
Definition: mesh_map.h:469
mesh_map::barycentricCoords
bool barycentricCoords(const Vector &p, const Vector &v0, const Vector &v1, const Vector &v2, float &u, float &v, float &w)
Computes the barycentric coordinates u, v,q of a query point p onto the triangle v0,...
Definition: util.cpp:159
util.h
lvr2::hdf5features::ArrayIO
mesh_map::MeshMap::publishVertexColors
void publishVertexColors()
Definition: mesh_map.cpp:1202
lvr2::calcFaceNormals
DenseFaceMap< Normal< typename BaseVecT::CoordType > > calcFaceNormals(const BaseMesh< BaseVecT > &mesh)
mesh_map::MeshMap::reconfigureCallback
void reconfigureCallback(mesh_map::MeshMapConfig &config, uint32_t level)
reconfigure callback function which is called if a dynamic reconfiguration were triggered.
Definition: mesh_map.cpp:1229
lvr2::BaseVector::normalize
void normalize()
t
geometry_msgs::TransformStamped t
lvr2::hdf5features::ChannelIO
mesh_map::MeshMap::srv_url
std::string srv_url
server url
Definition: mesh_map.h:423
mesh_map::HDF5MeshIO
lvr2::Hdf5IO< lvr2::hdf5features::ArrayIO, lvr2::hdf5features::ChannelIO, lvr2::hdf5features::VariantChannelIO, lvr2::hdf5features::MeshIO > HDF5MeshIO
Definition: mesh_map.cpp:63
mesh_map::MeshMap::meshAhead
bool meshAhead(Vector &vec, lvr2::FaceHandle &face, const float &step_width)
Definition: mesh_map.cpp:989
XmlRpc::XmlRpcValue
lvr2::hdf5features::MeshIO
mesh_map::MeshMap::findContours
void findContours(std::vector< std::vector< lvr2::VertexHandle >> &contours, int min_contour_size)
Computes contours.
Definition: mesh_map.cpp:515
mesh_client::MeshClient
ros::Time::now
static Time now()
mesh_map::MeshMap::publishVertexCosts
void publishVertexCosts(const lvr2::VertexMap< float > &costs, const std::string &name)
Publishes the given vertex map as mesh_msgs/VertexCosts, e.g. to visualize these.
Definition: mesh_map.cpp:1196
mesh_map::MeshMap::global_frame
std::string global_frame
global frame / coordinate system id
Definition: mesh_map.h:420


mesh_map
Author(s): Sebastian Pütz
autogenerated on Thu Jan 25 2024 03:42:45