Directory: | ./ |
---|---|
File: | src/octree.cpp |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 8 | 119 | 6.7% |
Branches: | 14 | 240 | 5.8% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Software License Agreement (BSD License) | ||
3 | * | ||
4 | * Copyright (c) 2020-2023, 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 "coal/octree.h" | ||
36 | |||
37 | #include <array> | ||
38 | |||
39 | namespace coal { | ||
40 | namespace internal { | ||
41 | struct Neighbors { | ||
42 | char value; | ||
43 | ✗ | Neighbors() : value(0) {} | |
44 | ✗ | bool minusX() const { return value & 0x1; } | |
45 | ✗ | bool plusX() const { return value & 0x2; } | |
46 | ✗ | bool minusY() const { return value & 0x4; } | |
47 | ✗ | bool plusY() const { return value & 0x8; } | |
48 | ✗ | bool minusZ() const { return value & 0x10; } | |
49 | ✗ | bool plusZ() const { return value & 0x20; } | |
50 | ✗ | void hasNeighboordMinusX() { value |= 0x1; } | |
51 | ✗ | void hasNeighboordPlusX() { value |= 0x2; } | |
52 | ✗ | void hasNeighboordMinusY() { value |= 0x4; } | |
53 | ✗ | void hasNeighboordPlusY() { value |= 0x8; } | |
54 | ✗ | void hasNeighboordMinusZ() { value |= 0x10; } | |
55 | ✗ | void hasNeighboordPlusZ() { value |= 0x20; } | |
56 | }; // struct neighbors | ||
57 | |||
58 | ✗ | void computeNeighbors(const std::vector<Vec6s>& boxes, | |
59 | std::vector<Neighbors>& neighbors) { | ||
60 | typedef std::vector<Vec6s> VectorVec6s; | ||
61 | ✗ | Scalar fixedSize = -1; | |
62 | ✗ | Scalar e = Scalar(1e-8); | |
63 | ✗ | for (std::size_t i = 0; i < boxes.size(); ++i) { | |
64 | ✗ | const Vec6s& box(boxes[i]); | |
65 | ✗ | Neighbors& n(neighbors[i]); | |
66 | ✗ | Scalar x(box[0]); | |
67 | ✗ | Scalar y(box[1]); | |
68 | ✗ | Scalar z(box[2]); | |
69 | ✗ | Scalar s(box[3]); | |
70 | ✗ | if (fixedSize == -1) | |
71 | ✗ | fixedSize = s; | |
72 | else | ||
73 | ✗ | assert(s == fixedSize); | |
74 | |||
75 | ✗ | for (VectorVec6s::const_iterator it = boxes.begin(); it != boxes.end(); | |
76 | ✗ | ++it) { | |
77 | ✗ | const Vec6s& otherBox = *it; | |
78 | ✗ | Scalar xo(otherBox[0]); | |
79 | ✗ | Scalar yo(otherBox[1]); | |
80 | ✗ | Scalar zo(otherBox[2]); | |
81 | // if (fabs(x-xo) < e && fabs(y-yo) < e && fabs(z-zo) < e){ | ||
82 | // continue; | ||
83 | // } | ||
84 | ✗ | if ((fabs(x - xo - s) < e) && (fabs(y - yo) < e) && (fabs(z - zo) < e)) { | |
85 | ✗ | n.hasNeighboordMinusX(); | |
86 | ✗ | } else if ((fabs(x - xo + s) < e) && (fabs(y - yo) < e) && | |
87 | ✗ | (fabs(z - zo) < e)) { | |
88 | ✗ | n.hasNeighboordPlusX(); | |
89 | ✗ | } else if ((fabs(x - xo) < e) && (fabs(y - yo - s) < e) && | |
90 | ✗ | (fabs(z - zo) < e)) { | |
91 | ✗ | n.hasNeighboordMinusY(); | |
92 | ✗ | } else if ((fabs(x - xo) < e) && (fabs(y - yo + s) < e) && | |
93 | ✗ | (fabs(z - zo) < e)) { | |
94 | ✗ | n.hasNeighboordPlusY(); | |
95 | ✗ | } else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) && | |
96 | ✗ | (fabs(z - zo - s) < e)) { | |
97 | ✗ | n.hasNeighboordMinusZ(); | |
98 | ✗ | } else if ((fabs(x - xo) < e) && (fabs(y - yo) < e) && | |
99 | ✗ | (fabs(z - zo + s) < e)) { | |
100 | ✗ | n.hasNeighboordPlusZ(); | |
101 | } | ||
102 | } | ||
103 | } | ||
104 | } | ||
105 | |||
106 | } // namespace internal | ||
107 | |||
108 | ✗ | void OcTree::exportAsObjFile(const std::string& filename) const { | |
109 | ✗ | std::vector<Vec6s> boxes(this->toBoxes()); | |
110 | ✗ | std::vector<internal::Neighbors> neighbors(boxes.size()); | |
111 | ✗ | internal::computeNeighbors(boxes, neighbors); | |
112 | // compute list of vertices and faces | ||
113 | |||
114 | typedef std::vector<Vec3s> VectorVec3s; | ||
115 | ✗ | std::vector<Vec3s> vertices; | |
116 | |||
117 | typedef std::array<std::size_t, 4> Array4; | ||
118 | typedef std::vector<Array4> VectorArray4; | ||
119 | ✗ | std::vector<Array4> faces; | |
120 | |||
121 | ✗ | for (std::size_t i = 0; i < boxes.size(); ++i) { | |
122 | ✗ | const Vec6s& box(boxes[i]); | |
123 | ✗ | internal::Neighbors& n(neighbors[i]); | |
124 | |||
125 | ✗ | Scalar x(box[0]); | |
126 | ✗ | Scalar y(box[1]); | |
127 | ✗ | Scalar z(box[2]); | |
128 | ✗ | Scalar size(box[3]); | |
129 | |||
130 | ✗ | const Scalar half = Scalar(0.5); | |
131 | ✗ | vertices.push_back( | |
132 | ✗ | Vec3s(x - half * size, y - half * size, z - half * size)); | |
133 | ✗ | vertices.push_back( | |
134 | ✗ | Vec3s(x + half * size, y - half * size, z - half * size)); | |
135 | ✗ | vertices.push_back( | |
136 | ✗ | Vec3s(x - half * size, y + half * size, z - half * size)); | |
137 | ✗ | vertices.push_back( | |
138 | ✗ | Vec3s(x + half * size, y + half * size, z - half * size)); | |
139 | ✗ | vertices.push_back( | |
140 | ✗ | Vec3s(x - half * size, y - half * size, z + half * size)); | |
141 | ✗ | vertices.push_back( | |
142 | ✗ | Vec3s(x + half * size, y - half * size, z + half * size)); | |
143 | ✗ | vertices.push_back( | |
144 | ✗ | Vec3s(x - half * size, y + half * size, z + half * size)); | |
145 | ✗ | vertices.push_back( | |
146 | ✗ | Vec3s(x + half * size, y + half * size, z + half * size)); | |
147 | |||
148 | // Add face only if box has no neighbor with the same face | ||
149 | ✗ | if (!n.minusX()) { | |
150 | ✗ | Array4 a = {{8 * i + 1, 8 * i + 5, 8 * i + 7, 8 * i + 3}}; | |
151 | ✗ | faces.push_back(a); | |
152 | } | ||
153 | ✗ | if (!n.plusX()) { | |
154 | ✗ | Array4 a = {{8 * i + 2, 8 * i + 4, 8 * i + 8, 8 * i + 6}}; | |
155 | ✗ | faces.push_back(a); | |
156 | } | ||
157 | ✗ | if (!n.minusY()) { | |
158 | ✗ | Array4 a = {{8 * i + 1, 8 * i + 2, 8 * i + 6, 8 * i + 5}}; | |
159 | ✗ | faces.push_back(a); | |
160 | } | ||
161 | ✗ | if (!n.plusY()) { | |
162 | ✗ | Array4 a = {{8 * i + 4, 8 * i + 3, 8 * i + 7, 8 * i + 8}}; | |
163 | ✗ | faces.push_back(a); | |
164 | } | ||
165 | ✗ | if (!n.minusZ()) { | |
166 | ✗ | Array4 a = {{8 * i + 1, 8 * i + 2, 8 * i + 4, 8 * i + 3}}; | |
167 | ✗ | faces.push_back(a); | |
168 | } | ||
169 | ✗ | if (!n.plusZ()) { | |
170 | ✗ | Array4 a = {{8 * i + 5, 8 * i + 6, 8 * i + 8, 8 * i + 7}}; | |
171 | ✗ | faces.push_back(a); | |
172 | } | ||
173 | } | ||
174 | // write obj in a file | ||
175 | ✗ | std::ofstream os; | |
176 | ✗ | os.open(filename); | |
177 | ✗ | if (!os.is_open()) | |
178 | ✗ | COAL_THROW_PRETTY( | |
179 | (std::string("failed to open file \"") + filename + std::string("\"")) | ||
180 | .c_str(), | ||
181 | std::runtime_error); | ||
182 | // write vertices | ||
183 | ✗ | os << "# list of vertices\n"; | |
184 | ✗ | for (VectorVec3s::const_iterator it = vertices.begin(); it != vertices.end(); | |
185 | ✗ | ++it) { | |
186 | ✗ | const Vec3s& v = *it; | |
187 | ✗ | os << "v " << v[0] << " " << v[1] << " " << v[2] << '\n'; | |
188 | } | ||
189 | ✗ | os << "\n# list of faces\n"; | |
190 | ✗ | for (VectorArray4::const_iterator it = faces.begin(); it != faces.end(); | |
191 | ✗ | ++it) { | |
192 | ✗ | const Array4& f = *it; | |
193 | ✗ | os << "f " << f[0] << " " << f[1] << " " << f[2] << " " << f[3] << '\n'; | |
194 | } | ||
195 | } | ||
196 | |||
197 | 1 | OcTreePtr_t makeOctree( | |
198 | const Eigen::Matrix<Scalar, Eigen::Dynamic, 3>& point_cloud, | ||
199 | const Scalar resolution) { | ||
200 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 3> InputType; | ||
201 | typedef InputType::ConstRowXpr RowType; | ||
202 | |||
203 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | shared_ptr<octomap::OcTree> octree(new octomap::OcTree(resolution)); |
204 |
2/2✓ Branch 1 taken 1000 times.
✓ Branch 2 taken 1 times.
|
1001 | for (Eigen::DenseIndex row_id = 0; row_id < point_cloud.rows(); ++row_id) { |
205 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | RowType row = point_cloud.row(row_id); |
206 |
4/8✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
|
1000 | octree->updateNode(row[0], row[1], row[2], true, true); |
207 | } | ||
208 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | octree->updateInnerOccupancy(); |
209 | |||
210 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | return OcTreePtr_t(new OcTree(octree)); |
211 | 1 | } | |
212 | } // namespace coal | ||
213 |