GCC Code Coverage Report


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