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)) { |
84 |
|
|
n.hasNeighboordMinusX(); |
85 |
|
|
} else if ((fabs(x - xo + s) < e) && (fabs(y - yo) < e) && |
86 |
|
|
(fabs(z - zo) < e)) { |
87 |
|
|
n.hasNeighboordPlusX(); |
88 |
|
|
} else if ((fabs(x - xo) < e) && (fabs(y - yo - s) < e) && |
89 |
|
|
(fabs(z - zo) < e)) { |
90 |
|
|
n.hasNeighboordMinusY(); |
91 |
|
|
} else if ((fabs(x - xo) < e) && (fabs(y - yo + s) < e) && |
92 |
|
|
(fabs(z - zo) < e)) { |
93 |
|
|
n.hasNeighboordPlusY(); |
94 |
|
|
} else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) && |
95 |
|
|
(fabs(z - zo - s) < e)) { |
96 |
|
|
n.hasNeighboordMinusZ(); |
97 |
|
|
} else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) && |
98 |
|
|
(fabs(z - zo + s) < e)) { |
99 |
|
|
n.hasNeighboordPlusZ(); |
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 |
|
|
|
185 |
|
|
OcTreePtr_t makeOctree( |
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 |