load_mesh.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #include <cstdio>
36 #include <cmath>
37 #include <algorithm>
38 #include <set>
41 #include <ros/assert.h>
42 #include <tinyxml.h>
43 #if defined(ASSIMP_UNIFIED_HEADER_NAMES)
44 #include <assimp/Importer.hpp>
45 #include <assimp/scene.h>
46 #include <assimp/postprocess.h>
47 #include <assimp/IOStream.hpp>
48 #include <assimp/IOSystem.hpp>
49 #else
50 #include <assimp/assimp.hpp>
51 #include <assimp/aiScene.h>
52 #include <assimp/aiPostProcess.h>
53 #include <assimp/IOStream.h>
54 #include <assimp/IOSystem.h>
55 #endif
56 
57 
58 // \author Ioan Sucan ; based on stl_to_mesh
59 
60 namespace shapes
61 {
62 
63  // Assimp wrappers
64  class ResourceIOStream : public Assimp::IOStream
65  {
66  public:
68  : res_(res)
69  , pos_(res.data.get())
70  {}
71 
73  {}
74 
75  size_t Read(void* buffer, size_t size, size_t count)
76  {
77  size_t to_read = size * count;
78  if (pos_ + to_read > res_.data.get() + res_.size)
79  {
80  to_read = res_.size - (pos_ - res_.data.get());
81  }
82 
83  memcpy(buffer, pos_, to_read);
84  pos_ += to_read;
85 
86  return to_read;
87  }
88 
89  size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; }
90 
91  aiReturn Seek( size_t offset, aiOrigin origin)
92  {
93  uint8_t* new_pos = 0;
94  switch (origin)
95  {
96  case aiOrigin_SET:
97  new_pos = res_.data.get() + offset;
98  break;
99  case aiOrigin_CUR:
100  new_pos = pos_ + offset; // TODO is this right? can offset really not be negative
101  break;
102  case aiOrigin_END:
103  new_pos = res_.data.get() + res_.size - offset; // TODO is this right?
104  break;
105  default:
106  ROS_BREAK();
107  }
108 
109  if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
110  {
111  return aiReturn_FAILURE;
112  }
113 
114  pos_ = new_pos;
115  return aiReturn_SUCCESS;
116  }
117 
118  size_t Tell() const
119  {
120  return pos_ - res_.data.get();
121  }
122 
123  size_t FileSize() const
124  {
125  return res_.size;
126  }
127 
128  void Flush() {}
129 
130  private:
132  uint8_t* pos_;
133  };
134 
135 
136  class ResourceIOSystem : public Assimp::IOSystem
137  {
138  public:
140  {
141  }
142 
144  {
145  }
146 
147  // Check whether a specific file exists
148  bool Exists(const char* file) const
149  {
150  // Ugly -- two retrievals where there should be one (Exists + Open)
151  // resource_retriever needs a way of checking for existence
152  // TODO: cache this
154  try
155  {
156  res = retriever_.get(file);
157  }
159  {
160  return false;
161  }
162 
163  return true;
164  }
165 
166  // Get the path delimiter character we'd like to see
167  char getOsSeparator() const
168  {
169  return '/';
170  }
171 
172  // ... and finally a method to open a custom stream
173  Assimp::IOStream* Open(const char* file, const char* mode = "rb")
174  {
175  // Ugly -- two retrievals where there should be one (Exists + Open)
176  // resource_retriever needs a way of checking for existence
178  try
179  {
180  res = retriever_.get(file);
181  }
183  {
184  return 0;
185  }
186 
187  return new ResourceIOStream(res);
188  }
189 
190  void Close(Assimp::IOStream* stream);
191 
192  private:
194  };
195 
196  void ResourceIOSystem::Close(Assimp::IOStream* stream)
197  {
198  delete stream;
199  }
200 
201  float getMeshUnitRescale(const std::string& resource_path)
202  {
203  static std::map<std::string, float> rescale_cache;
204 
205  // Try to read unit to meter conversion ratio from mesh. Only valid in Collada XML formats.
206  TiXmlDocument xmlDoc;
207  float unit_scale(1.0);
210  try
211  {
212  res = retriever.get(resource_path);
213  }
215  {
216  ROS_ERROR("%s", e.what());
217  return unit_scale;
218  }
219 
220  if (res.size == 0)
221  {
222  return unit_scale;
223  }
224 
225 
226  // Use the resource retriever to get the data.
227  const char * data = reinterpret_cast<const char * > (res.data.get());
228  xmlDoc.Parse(data);
229 
230  // Find the appropriate element if it exists
231  if(!xmlDoc.Error())
232  {
233  TiXmlElement * colladaXml = xmlDoc.FirstChildElement("COLLADA");
234  if(colladaXml)
235  {
236  TiXmlElement *assetXml = colladaXml->FirstChildElement("asset");
237  if(assetXml)
238  {
239  TiXmlElement *unitXml = assetXml->FirstChildElement("unit");
240  if (unitXml && unitXml->Attribute("meter"))
241  {
242  // Failing to convert leaves unit_scale as the default.
243  if(unitXml->QueryFloatAttribute("meter", &unit_scale) != 0)
244  ROS_WARN_STREAM("getMeshUnitRescale::Failed to convert unit element meter attribute to determine scaling. unit element: "
245  << *unitXml);
246  }
247  }
248  }
249  }
250  return unit_scale;
251  }
252 
253  std::vector<tf::Vector3> getVerticesFromAssimpNode(const aiScene* scene, const aiNode* node, const float scale)
254  {
255  std::vector<tf::Vector3> vertices;
256  if (!node)
257  {
258  return vertices;
259  }
260 
261  aiMatrix4x4 transform = node->mTransformation;
262  aiNode *pnode = node->mParent;
263  while (pnode)
264  {
265  // Don't convert to y-up orientation, which is what the root node in
266  // Assimp does
267  if (pnode->mParent != NULL)
268  transform = pnode->mTransformation * transform;
269  pnode = pnode->mParent;
270  }
271 
272  aiMatrix3x3 rotation(transform);
273  aiMatrix3x3 inverse_transpose_rotation(rotation);
274  inverse_transpose_rotation.Inverse();
275  inverse_transpose_rotation.Transpose();
276 
277  for (uint32_t i = 0; i < node->mNumMeshes; i++)
278  {
279  aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
280  // Add the vertices
281  for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
282  {
283  aiVector3D p = input_mesh->mVertices[j];
284  p *= transform;
285  p *= scale;
286  tf::Vector3 v(p.x, p.y, p.z);
287  vertices.push_back(v);
288  }
289  }
290 
291  for (uint32_t i=0; i < node->mNumChildren; ++i)
292  {
293  std::vector<tf::Vector3> sub_vertices = getVerticesFromAssimpNode(scene,node->mChildren[i], scale);
294  // Add vertices
295  for (size_t j = 0; j < sub_vertices.size(); j++) {
296  vertices.push_back(sub_vertices[j]);
297  }
298  }
299  return vertices;
300  }
301 
302  shapes::Mesh* meshFromAssimpScene(const std::string& name, const aiScene* scene)
303  {
304  if (!scene->HasMeshes())
305  {
306  ROS_ERROR("No meshes found in file [%s]", name.c_str());
307  return NULL;
308  }
309 
310  float scale = getMeshUnitRescale(name);
311 
312  std::vector<tf::Vector3> vertices = getVerticesFromAssimpNode(scene, scene->mRootNode, scale);
313 
314  return createMeshFromVertices(vertices);
315  }
316 
317  namespace detail
318  {
319  struct myVertex
320  {
322  unsigned int index;
323  };
324 
326  {
327  bool operator()(const myVertex &p1, const myVertex &p2) const
328  {
329  const tf::Vector3 &v1 = p1.point;
330  const tf::Vector3 &v2 = p2.point;
331  if (v1.getX() < v2.getX())
332  return true;
333  if (v1.getX() > v2.getX())
334  return false;
335  if (v1.getY() < v2.getY())
336  return true;
337  if (v1.getY() > v2.getY())
338  return false;
339  if (v1.getZ() < v2.getZ())
340  return true;
341  return false;
342  }
343  };
344 
346  {
347  bool operator()(const myVertex &p1, const myVertex &p2) const
348  {
349  return p1.index < p2.index;
350  }
351  };
352  }
353 
354  shapes::Mesh* createMeshFromVertices(const std::vector<tf::Vector3> &vertices, const std::vector<unsigned int> &triangles)
355  {
356  unsigned int nt = triangles.size() / 3;
357  shapes::Mesh *mesh = new shapes::Mesh(vertices.size(), nt);
358  for (unsigned int i = 0 ; i < vertices.size() ; ++i)
359  {
360  mesh->vertices[3 * i ] = vertices[i].getX();
361  mesh->vertices[3 * i + 1] = vertices[i].getY();
362  mesh->vertices[3 * i + 2] = vertices[i].getZ();
363  }
364 
365  std::copy(triangles.begin(), triangles.end(), mesh->triangles);
366 
367  // compute normals
368  for (unsigned int i = 0 ; i < nt ; ++i)
369  {
370  tf::Vector3 s1 = vertices[triangles[i * 3 ]] - vertices[triangles[i * 3 + 1]];
371  tf::Vector3 s2 = vertices[triangles[i * 3 + 1]] - vertices[triangles[i * 3 + 2]];
372  tf::Vector3 normal = s1.cross(s2);
373  normal.normalize();
374  mesh->normals[3 * i ] = normal.getX();
375  mesh->normals[3 * i + 1] = normal.getY();
376  mesh->normals[3 * i + 2] = normal.getZ();
377  }
378  return mesh;
379  }
380 
381  shapes::Mesh* createMeshFromVertices(const std::vector<tf::Vector3> &source)
382  {
383  if (source.size() < 3)
384  return NULL;
385 
386  std::set<detail::myVertex, detail::ltVertexValue> vertices;
387  std::vector<unsigned int> triangles;
388 
389  for (unsigned int i = 0 ; i < source.size() / 3 ; ++i)
390  {
391  // check if we have new vertices
392  detail::myVertex vt;
393 
394  vt.point = source[3 * i];
395  std::set<detail::myVertex, detail::ltVertexValue>::iterator p1 = vertices.find(vt);
396  if (p1 == vertices.end())
397  {
398  vt.index = vertices.size();
399  vertices.insert(vt);
400  }
401  else
402  vt.index = p1->index;
403  triangles.push_back(vt.index);
404 
405  vt.point = source[3 * i + 1];
406  std::set<detail::myVertex, detail::ltVertexValue>::iterator p2 = vertices.find(vt);
407  if (p2 == vertices.end())
408  {
409  vt.index = vertices.size();
410  vertices.insert(vt);
411  }
412  else
413  vt.index = p2->index;
414  triangles.push_back(vt.index);
415 
416  vt.point = source[3 * i + 2];
417  std::set<detail::myVertex, detail::ltVertexValue>::iterator p3 = vertices.find(vt);
418  if (p3 == vertices.end())
419  {
420  vt.index = vertices.size();
421  vertices.insert(vt);
422  }
423  else
424  vt.index = p3->index;
425 
426  triangles.push_back(vt.index);
427  }
428 
429  // sort our vertices
430  std::vector<detail::myVertex> vt;
431  vt.insert(vt.begin(), vertices.begin(), vertices.end());
432  std::sort(vt.begin(), vt.end(), detail::ltVertexIndex());
433 
434  // copy the data to a mesh structure
435  unsigned int nt = triangles.size() / 3;
436 
437  shapes::Mesh *mesh = new shapes::Mesh(vt.size(), nt);
438  for (unsigned int i = 0 ; i < vt.size() ; ++i)
439  {
440  mesh->vertices[3 * i ] = vt[i].point.getX();
441  mesh->vertices[3 * i + 1] = vt[i].point.getY();
442  mesh->vertices[3 * i + 2] = vt[i].point.getZ();
443  }
444 
445  std::copy(triangles.begin(), triangles.end(), mesh->triangles);
446 
447  // compute normals
448  for (unsigned int i = 0 ; i < nt ; ++i)
449  {
450  tf::Vector3 s1 = vt[triangles[i * 3 ]].point - vt[triangles[i * 3 + 1]].point;
451  tf::Vector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point;
452  tf::Vector3 normal = s1.cross(s2);
453  normal.normalize();
454  mesh->normals[3 * i ] = normal.getX();
455  mesh->normals[3 * i + 1] = normal.getY();
456  mesh->normals[3 * i + 2] = normal.getZ();
457  }
458 
459  return mesh;
460  }
461 
462  shapes::Mesh* createMeshFromBinaryStlData(const char *data, unsigned int size)
463  {
464  const char* pos = data;
465  pos += 80; // skip the 80 byte header
466 
467  unsigned int numTriangles = *(unsigned int*)pos;
468  pos += 4;
469 
470  // make sure we have read enough data
471  if ((long)(50 * numTriangles + 84) <= size)
472  {
473  std::vector<tf::Vector3> vertices;
474 
475  for (unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle)
476  {
477  // skip the normal
478  pos += 12;
479 
480  // read vertices
481  tf::Vector3 v1(0,0,0);
482  tf::Vector3 v2(0,0,0);
483  tf::Vector3 v3(0,0,0);
484 
485  v1.setX(*(float*)pos);
486  pos += 4;
487  v1.setY(*(float*)pos);
488  pos += 4;
489  v1.setZ(*(float*)pos);
490  pos += 4;
491 
492  v2.setX(*(float*)pos);
493  pos += 4;
494  v2.setY(*(float*)pos);
495  pos += 4;
496  v2.setZ(*(float*)pos);
497  pos += 4;
498 
499  v3.setX(*(float*)pos);
500  pos += 4;
501  v3.setY(*(float*)pos);
502  pos += 4;
503  v3.setZ(*(float*)pos);
504  pos += 4;
505 
506  // skip attribute
507  pos += 2;
508 
509  vertices.push_back(v1);
510  vertices.push_back(v2);
511  vertices.push_back(v3);
512  }
513 
514  return createMeshFromVertices(vertices);
515  }
516 
517  return NULL;
518  }
519 
520  shapes::Mesh* createMeshFromBinaryDAE(const char* filename)
521  {
522  std::string resource_path(filename);
523  Assimp::Importer importer;
524  importer.SetIOHandler(new ResourceIOSystem());
525  const aiScene* scene = importer.ReadFile(resource_path, aiProcess_SortByPType|aiProcess_GenNormals|aiProcess_Triangulate|aiProcess_GenUVCoords|aiProcess_FlipUVs);
526  if (!scene)
527  {
528  ROS_ERROR("Could not load resource [%s]: %s", resource_path.c_str(), importer.GetErrorString());
529  return NULL;
530  }
531  return meshFromAssimpScene(resource_path, scene);
532  }
533 
534  shapes::Mesh* createMeshFromBinaryStl(const char *filename)
535  {
536  FILE* input = fopen(filename, "r");
537  if (!input)
538  return NULL;
539 
540  fseek(input, 0, SEEK_END);
541  long fileSize = ftell(input);
542  fseek(input, 0, SEEK_SET);
543 
544  char* buffer = new char[fileSize];
545  size_t rd = fread(buffer, fileSize, 1, input);
546 
547  fclose(input);
548 
549  shapes::Mesh *result = NULL;
550 
551  if (rd == 1)
552  result = createMeshFromBinaryStlData(buffer, fileSize);
553 
554  delete[] buffer;
555 
556  return result;
557  }
558 
559 }
Mesh * createMeshFromBinaryStl(const char *filename)
Load a mesh from a binary STL file. Normals are recomputed and repeating vertices are identified...
Definition: load_mesh.cpp:534
Definition: shapes.h:48
std::vector< tf::Vector3 > getVerticesFromAssimpNode(const aiScene *scene, const aiNode *node, const float scale)
Definition: load_mesh.cpp:253
TFSIMD_FORCE_INLINE void setX(tfScalar x)
Mesh * createMeshFromVertices(const std::vector< tf::Vector3 > &vertices, const std::vector< unsigned int > &triangles)
Load a mesh from a set of vertices. Triangles are constructed using index values from the triangles v...
Definition: load_mesh.cpp:354
TFSIMD_FORCE_INLINE void setY(tfScalar y)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
Assimp::IOStream * Open(const char *file, const char *mode="rb")
Definition: load_mesh.cpp:173
TFSIMD_FORCE_INLINE const tfScalar & getY() const
char getOsSeparator() const
Definition: load_mesh.cpp:167
bool Exists(const char *file) const
Definition: load_mesh.cpp:148
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
unsigned int * triangles
the vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1...
Definition: shapes.h:196
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
def get(url)
Mesh * createMeshFromBinaryStlData(const char *data, unsigned int size)
Load a mesh from a binary STL stream. Normals are recomputed and repeating vertices are identified...
Definition: load_mesh.cpp:462
Mesh * createMeshFromBinaryDAE(const char *filename)
Load a mesh from a binary DAE file. Normals are recomputed and repeating vertices are identified...
Definition: load_mesh.cpp:520
boost::shared_array< uint8_t > data
void Close(Assimp::IOStream *stream)
Definition: load_mesh.cpp:196
size_t Read(void *buffer, size_t size, size_t count)
Definition: load_mesh.cpp:75
double * vertices
the position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
Definition: shapes.h:189
ResourceIOStream(const resource_retriever::MemoryResource &res)
Definition: load_mesh.cpp:67
bool operator()(const myVertex &p1, const myVertex &p2) const
Definition: load_mesh.cpp:347
#define ROS_WARN_STREAM(args)
shapes::Mesh * meshFromAssimpScene(const std::string &name, const aiScene *scene)
Definition: load_mesh.cpp:302
size_t Write(const void *buffer, size_t size, size_t count)
Definition: load_mesh.cpp:89
TFSIMD_FORCE_INLINE Vector3 & normalize()
MemoryResource get(const std::string &url)
float getMeshUnitRescale(const std::string &resource_path)
Definition: load_mesh.cpp:201
size_t FileSize() const
Definition: load_mesh.cpp:123
Definition of a mesh.
Definition: shapes.h:151
aiReturn Seek(size_t offset, aiOrigin origin)
Definition: load_mesh.cpp:91
TFSIMD_FORCE_INLINE const tfScalar & getX() const
resource_retriever::MemoryResource res_
Definition: load_mesh.cpp:131
size_t Tell() const
Definition: load_mesh.cpp:118
double * normals
the normal to each triangle unit vector represented as (x,y,z)
Definition: shapes.h:200
#define ROS_BREAK()
#define ROS_ERROR(...)
resource_retriever::Retriever retriever_
Definition: load_mesh.cpp:193
bool operator()(const myVertex &p1, const myVertex &p2) const
Definition: load_mesh.cpp:327


pr2_navigation_self_filter
Author(s): Eitan Marder-Eppstein
autogenerated on Mon Jun 10 2019 14:28:54