| 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/8✓ 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.
✗ Branch 9 not taken.
✗ Branch 10 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/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
2 | return OcTreePtr_t(new OcTree(octree)); |
| 211 | 1 | } | |
| 212 | } // namespace coal | ||
| 213 |