src/octree.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020, INRIA
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 Open Source Robotics Foundation 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 <hpp/fcl/octree.h>
36 
37 namespace hpp {
38 namespace fcl {
39 namespace internal {
40 struct Neighbors {
41  char value;
42  Neighbors() : value(0) {}
43  bool minusX() const { return value & 0x1; }
44  bool plusX() const { return value & 0x2; }
45  bool minusY() const { return value & 0x4; }
46  bool plusY() const { return value & 0x8; }
47  bool minusZ() const { return value & 0x10; }
48  bool plusZ() const { return value & 0x20; }
49  void hasNeighboordMinusX() { value |= 0x1; }
50  void hasNeighboordPlusX() { value |= 0x2; }
51  void hasNeighboordMinusY() { value |= 0x4; }
52  void hasNeighboordPlusY() { value |= 0x8; }
53  void hasNeighboordMinusZ() { value |= 0x10; }
54  void hasNeighboordPlusZ() { value |= 0x20; }
55 }; // struct neighbors
56 
57 void computeNeighbors(const std::vector<boost::array<FCL_REAL, 6> >& boxes,
58  std::vector<Neighbors>& neighbors) {
59  typedef std::vector<boost::array<FCL_REAL, 6> > VectorArray6;
60  FCL_REAL fixedSize = -1;
61  FCL_REAL e(1e-8);
62  for (std::size_t i = 0; i < boxes.size(); ++i) {
63  const boost::array<FCL_REAL, 6>& box(boxes[i]);
64  Neighbors& n(neighbors[i]);
65  FCL_REAL x(box[0]);
66  FCL_REAL y(box[1]);
67  FCL_REAL z(box[2]);
68  FCL_REAL s(box[3]);
69  if (fixedSize == -1)
70  fixedSize = s;
71  else
72  assert(s == fixedSize);
73 
74  for (VectorArray6::const_iterator it = boxes.begin(); it != boxes.end();
75  ++it) {
76  const boost::array<FCL_REAL, 6>& otherBox = *it;
77  FCL_REAL xo(otherBox[0]);
78  FCL_REAL yo(otherBox[1]);
79  FCL_REAL zo(otherBox[2]);
80  // if (fabs(x-xo) < e && fabs(y-yo) < e && fabs(z-zo) < e){
81  // continue;
82  // }
83  if ((fabs(x - xo - s) < e) && (fabs(y - yo) < e) && (fabs(z - zo) < e)) {
85  } else if ((fabs(x - xo + s) < e) && (fabs(y - yo) < e) &&
86  (fabs(z - zo) < e)) {
88  } else if ((fabs(x - xo) < e) && (fabs(y - yo - s) < e) &&
89  (fabs(z - zo) < e)) {
91  } else if ((fabs(x - xo) < e) && (fabs(y - yo + s) < e) &&
92  (fabs(z - zo) < e)) {
94  } else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) &&
95  (fabs(z - zo - s) < e)) {
97  } else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) &&
98  (fabs(z - zo + s) < e)) {
100  }
101  }
102  }
103 }
104 
105 } // namespace internal
106 
107 void OcTree::exportAsObjFile(const std::string& filename) const {
108  std::vector<boost::array<FCL_REAL, 6> > boxes(this->toBoxes());
109  std::vector<internal::Neighbors> neighbors(boxes.size());
110  internal::computeNeighbors(boxes, neighbors);
111  // compute list of vertices and faces
112 
113  typedef std::vector<Vec3f> VectorVec3f;
114  std::vector<Vec3f> vertices;
115 
116  typedef boost::array<std::size_t, 4> Array4;
117  typedef std::vector<Array4> VectorArray4;
118  std::vector<Array4> faces;
119 
120  for (std::size_t i = 0; i < boxes.size(); ++i) {
121  const boost::array<FCL_REAL, 6>& box(boxes[i]);
122  internal::Neighbors& n(neighbors[i]);
123 
124  FCL_REAL x(box[0]);
125  FCL_REAL y(box[1]);
126  FCL_REAL z(box[2]);
127  FCL_REAL size(box[3]);
128 
129  vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z - .5 * size));
130  vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z - .5 * size));
131  vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z - .5 * size));
132  vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z - .5 * size));
133  vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z + .5 * size));
134  vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z + .5 * size));
135  vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z + .5 * size));
136  vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z + .5 * size));
137 
138  // Add face only if box has no neighbor with the same face
139  if (!n.minusX()) {
140  Array4 a = {{8 * i + 1, 8 * i + 5, 8 * i + 7, 8 * i + 3}};
141  faces.push_back(a);
142  }
143  if (!n.plusX()) {
144  Array4 a = {{8 * i + 2, 8 * i + 4, 8 * i + 8, 8 * i + 6}};
145  faces.push_back(a);
146  }
147  if (!n.minusY()) {
148  Array4 a = {{8 * i + 1, 8 * i + 2, 8 * i + 6, 8 * i + 5}};
149  faces.push_back(a);
150  }
151  if (!n.plusY()) {
152  Array4 a = {{8 * i + 4, 8 * i + 3, 8 * i + 7, 8 * i + 8}};
153  faces.push_back(a);
154  }
155  if (!n.minusZ()) {
156  Array4 a = {{8 * i + 1, 8 * i + 2, 8 * i + 4, 8 * i + 3}};
157  faces.push_back(a);
158  }
159  if (!n.plusZ()) {
160  Array4 a = {{8 * i + 5, 8 * i + 6, 8 * i + 8, 8 * i + 7}};
161  faces.push_back(a);
162  }
163  }
164  // write obj in a file
165  std::ofstream os;
166  os.open(filename);
167  if (!os.is_open())
168  throw std::runtime_error(std::string("failed to open file \"") + filename +
169  std::string("\""));
170  // write vertices
171  os << "# list of vertices\n";
172  for (VectorVec3f::const_iterator it = vertices.begin(); it != vertices.end();
173  ++it) {
174  const Vec3f& v = *it;
175  os << "v " << v[0] << " " << v[1] << " " << v[2] << '\n';
176  }
177  os << "\n# list of faces\n";
178  for (VectorArray4::const_iterator it = faces.begin(); it != faces.end();
179  ++it) {
180  const Array4& f = *it;
181  os << "f " << f[0] << " " << f[1] << " " << f[2] << " " << f[3] << '\n';
182  }
183 }
184 
186  const Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3>& point_cloud,
187  const FCL_REAL resolution) {
188  typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3> InputType;
189  typedef InputType::ConstRowXpr RowType;
190 
191  shared_ptr<octomap::OcTree> octree(new octomap::OcTree(resolution));
192  for (Eigen::DenseIndex row_id = 0; row_id < point_cloud.rows(); ++row_id) {
193  RowType row = point_cloud.row(row_id);
194  octomap::point3d p(static_cast<float>(row[0]), static_cast<float>(row[1]),
195  static_cast<float>(row[2]));
196  octree->updateNode(p, true);
197  }
198  octree->updateInnerOccupancy();
199 
200  return OcTreePtr_t(new OcTree(octree));
201 }
202 } // namespace fcl
203 } // namespace hpp
hpp::fcl::internal::Neighbors::hasNeighboordMinusX
void hasNeighboordMinusX()
Definition: src/octree.cpp:49
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::internal::Neighbors::hasNeighboordPlusZ
void hasNeighboordPlusZ()
Definition: src/octree.cpp:54
hpp::fcl::OcTreePtr_t
shared_ptr< OcTree > OcTreePtr_t
Definition: include/hpp/fcl/fwd.hh:106
y
y
hpp::fcl::internal::Neighbors::minusZ
bool minusZ() const
Definition: src/octree.cpp:47
hpp::fcl::internal::Neighbors::minusY
bool minusY() const
Definition: src/octree.cpp:45
hpp::fcl::internal::Neighbors::plusZ
bool plusZ() const
Definition: src/octree.cpp:48
hpp::fcl::internal::Neighbors::Neighbors
Neighbors()
Definition: src/octree.cpp:42
octree
Definition: octree.py:1
hpp::fcl::internal::Neighbors::plusX
bool plusX() const
Definition: src/octree.cpp:44
hpp::fcl::internal::Neighbors
Definition: src/octree.cpp:40
a
list a
hpp::fcl::makeOctree
HPP_FCL_DLLAPI OcTreePtr_t makeOctree(const Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 3 > &point_cloud, const FCL_REAL resolution)
Build an OcTree from a point cloud and a given resolution.
Definition: src/octree.cpp:185
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::internal::Neighbors::hasNeighboordPlusX
void hasNeighboordPlusX()
Definition: src/octree.cpp:50
collision-bench.filename
filename
Definition: collision-bench.py:6
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::internal::Neighbors::hasNeighboordMinusZ
void hasNeighboordMinusZ()
Definition: src/octree.cpp:53
x
x
hpp::fcl::internal::Neighbors::hasNeighboordPlusY
void hasNeighboordPlusY()
Definition: src/octree.cpp:52
hpp::fcl::internal::computeNeighbors
void computeNeighbors(const std::vector< boost::array< FCL_REAL, 6 > > &boxes, std::vector< Neighbors > &neighbors)
Definition: src/octree.cpp:57
hpp::fcl::OcTree::toBoxes
std::vector< boost::array< FCL_REAL, 6 > > toBoxes() const
transform the octree into a bunch of boxes; uncertainty information is kept in the boxes....
Definition: octree.h:140
hpp::fcl::internal::Neighbors::minusX
bool minusX() const
Definition: src/octree.cpp:43
hpp::fcl::OcTree
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition: octree.h:53
hpp::fcl::OcTree::exportAsObjFile
void exportAsObjFile(const std::string &filename) const
Definition: src/octree.cpp:107
hpp::fcl::internal::Neighbors::hasNeighboordMinusY
void hasNeighboordMinusY()
Definition: src/octree.cpp:51
hpp::fcl::internal::Neighbors::plusY
bool plusY() const
Definition: src/octree.cpp:46
octree.h
hpp::fcl::internal::Neighbors::value
char value
Definition: src/octree.cpp:41
obb.v
list v
Definition: obb.py:48


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:14