|
16 | 16 | // Hugues THOMAS - 10/02/2017 |
17 | 17 | // |
18 | 18 |
|
| 19 | +#pragma once |
19 | 20 |
|
20 | | -# pragma once |
21 | | - |
| 21 | +#include <algorithm> |
22 | 22 | #include <cmath> |
23 | | -#include <vector> |
24 | | -#include <unordered_map> |
| 23 | +#include <iomanip> |
| 24 | +#include <iostream> |
25 | 25 | #include <map> |
26 | | -#include <algorithm> |
27 | 26 | #include <numeric> |
28 | | -#include <iostream> |
29 | | -#include <iomanip> |
| 27 | +#include <unordered_map> |
| 28 | +#include <vector> |
30 | 29 |
|
31 | 30 | #include <time.h> |
32 | 31 |
|
33 | | - |
34 | | -template<typename scalar_t> |
35 | | -struct PointCloud |
36 | | -{ |
37 | | - struct PointXYZ { |
38 | | - scalar_t x,y,z; |
39 | | - }; |
40 | | - |
41 | | - std::vector<PointXYZ> pts; |
42 | | - |
43 | | - void set(std::vector<scalar_t> new_pts){ |
44 | | - |
45 | | - // pts = std::vector<PointXYZ>((PointXYZ*)new_pts, (PointXYZ*)new_pts+new_pts.size()/3); |
46 | | - std::vector<PointXYZ> temp(new_pts.size()/3); |
47 | | - for(unsigned int i=0; i < new_pts.size(); i++){ |
48 | | - if(i%3 == 0){ |
49 | | - |
50 | | - PointXYZ point; |
51 | | - point.x = new_pts[i]; |
52 | | - point.y = new_pts[i+1]; |
53 | | - point.z = new_pts[i+2]; |
54 | | - temp[i/3] = point; |
55 | | - } |
56 | | - } |
57 | | - pts = temp; |
58 | | - } |
59 | | - void set_batch(std::vector<scalar_t> new_pts, int begin, int size){ |
60 | | - |
61 | | - std::vector<PointXYZ> temp(size); |
62 | | - for(int i=0; i < size; i++){ |
63 | | - PointXYZ point; |
64 | | - point.x = new_pts[3*(begin+i)]; |
65 | | - point.y = new_pts[3*(begin+i) + 1]; |
66 | | - point.z = new_pts[3*(begin+i) + 2]; |
67 | | - temp[i] = point; |
68 | | - |
69 | | - } |
70 | | - pts = temp; |
71 | | - } |
72 | | - |
73 | | - // Must return the number of data points |
74 | | - inline size_t kdtree_get_point_count() const { return pts.size(); } |
75 | | - |
76 | | - // Returns the dim'th component of the idx'th point in the class: |
77 | | - // Since this is inlined and the "dim" argument is typically an immediate value, the |
78 | | - // "if/else's" are actually solved at compile time. |
79 | | - inline scalar_t kdtree_get_pt(const size_t idx, const size_t dim) const |
80 | | - { |
81 | | - if (dim == 0) return pts[idx].x; |
82 | | - else if (dim == 1) return pts[idx].y; |
83 | | - else return pts[idx].z; |
84 | | - } |
85 | | - |
86 | | - // Optional bounding-box computation: return false to default to a standard bbox computation loop. |
87 | | - // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. |
88 | | - // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) |
89 | | - template <class BBOX> |
90 | | - bool kdtree_get_bbox(BBOX& /* bb */) const { return false; } |
91 | | - |
92 | | - |
| 32 | +template <typename scalar_t> |
| 33 | +struct PointCloud { |
| 34 | + struct PointXYZ { |
| 35 | + scalar_t x, y, z; |
| 36 | + }; |
| 37 | + |
| 38 | + std::vector<PointXYZ> pts; |
| 39 | + |
| 40 | + void set(std::vector<scalar_t> new_pts) { |
| 41 | + // pts = std::vector<PointXYZ>((PointXYZ*)new_pts, |
| 42 | + // (PointXYZ*)new_pts+new_pts.size()/3); |
| 43 | + std::vector<PointXYZ> temp(new_pts.size() / 3); |
| 44 | + for (unsigned int i = 0; i < new_pts.size(); i++) { |
| 45 | + if (i % 3 == 0) { |
| 46 | + PointXYZ point; |
| 47 | + point.x = new_pts[i]; |
| 48 | + point.y = new_pts[i + 1]; |
| 49 | + point.z = new_pts[i + 2]; |
| 50 | + temp[i / 3] = point; |
| 51 | + } |
| 52 | + } |
| 53 | + pts = temp; |
| 54 | + } |
| 55 | + void set_batch(std::vector<scalar_t> new_pts, int begin, int size) { |
| 56 | + std::vector<PointXYZ> temp(size); |
| 57 | + for (int i = 0; i < size; i++) { |
| 58 | + PointXYZ point; |
| 59 | + point.x = new_pts[3 * (begin + i)]; |
| 60 | + point.y = new_pts[3 * (begin + i) + 1]; |
| 61 | + point.z = new_pts[3 * (begin + i) + 2]; |
| 62 | + temp[i] = point; |
| 63 | + } |
| 64 | + pts = temp; |
| 65 | + } |
| 66 | + |
| 67 | + // Must return the number of data points |
| 68 | + inline size_t kdtree_get_point_count() const { return pts.size(); } |
| 69 | + |
| 70 | + // Returns the dim'th component of the idx'th point in the class: |
| 71 | + // Since this is inlined and the "dim" argument is typically an immediate |
| 72 | + // value, the |
| 73 | + // "if/else's" are actually solved at compile time. |
| 74 | + inline scalar_t kdtree_get_pt(const size_t idx, const size_t dim) const { |
| 75 | + if (dim == 0) |
| 76 | + return pts[idx].x; |
| 77 | + else if (dim == 1) |
| 78 | + return pts[idx].y; |
| 79 | + else |
| 80 | + return pts[idx].z; |
| 81 | + } |
| 82 | + |
| 83 | + // Optional bounding-box computation: return false to default to a standard |
| 84 | + // bbox computation loop. |
| 85 | + // Return true if the BBOX was already computed by the class and returned in |
| 86 | + // "bb" so it can be avoided to redo it again. Look at bb.size() to find out |
| 87 | + // the expected dimensionality (e.g. 2 or 3 for point clouds) |
| 88 | + template <class BBOX> |
| 89 | + bool kdtree_get_bbox(BBOX& /* bb */) const { |
| 90 | + return false; |
| 91 | + } |
93 | 92 | }; |
0 commit comments