Skip to content

Commit 9c63e7a

Browse files
neka-natneka-nat
committed
clang format
1 parent a59ba53 commit 9c63e7a

File tree

94 files changed

+6843
-3766
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

94 files changed

+6843
-3766
lines changed

‎.clang-format

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
BasedOnStyle: Google
2+
IndentWidth: 4
3+
ColumnLimit: 80
4+
UseTab: Never
5+
Language: Cpp
6+
Standard: Cpp11
7+
ContinuationIndentWidth: 8
8+
AccessModifierOffset: -4
9+
BinPackParameters: false
10+
SortIncludes: true

‎README.md

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -87,6 +87,10 @@ python benchmarks.py
8787

8888
![speedup](https://raw.githubusercontent.com/neka-nat/cupoch/master/docs/_static/speedup.png)
8989

90+
### Visual odometry with intel realsense D435
91+
92+
![vo](https://raw.githubusercontent.com/neka-nat/cupoch/master/docs/_static/vo_gpu.gif)
93+
9094
## References
9195

9296
* CUDA repository forked from Open3D, https://github.com/theNded/Open3D

‎docs/_static/vo_gpu.gif

3.87 MB
Loading

‎examples/python/realsense/realsense_rgbd_odometry.py

Lines changed: 14 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -67,16 +67,20 @@ def get_intrinsic_matrix(frame):
6767
align = rs.align(align_to)
6868

6969
fig = plt.figure()
70-
ax = p3.Axes3D(fig)
70+
ax1 = fig.add_subplot(1, 2, 1, projection='3d')
71+
ax2 = fig.add_subplot(2, 2, 2)
72+
ax3 = fig.add_subplot(2, 2, 4)
7173

7274
# Streaming loop
7375
prev_rgbd_image = None
7476
option = x3d.odometry.OdometryOption()
7577
cur_trans = np.identity(4)
7678
path = []
7779
path.append(cur_trans[:3, 3].tolist())
78-
line = ax.plot(*list(zip(*path)), 'r-')[0]
79-
pos = ax.plot(*list(zip(*path)), 'yo')[0]
80+
line = ax1.plot(*list(zip(*path)), 'r-')[0]
81+
pos = ax1.plot(*list(zip(*path)), 'yo')[0]
82+
depth_im = ax2.imshow(np.zeros((480, 640), dtype=np.uint8), cmap="gray", vmin=0, vmax=255)
83+
color_im = ax3.imshow(np.zeros((480, 640, 3), dtype=np.uint8))
8084
try:
8185
def update_odom(frame):
8286
global prev_rgbd_image, cur_trans
@@ -99,8 +103,8 @@ def update_odom(frame):
99103
if not aligned_depth_frame or not color_frame:
100104
return
101105

102-
depth_image = x3d.geometry.Image(
103-
np.array(aligned_depth_frame.get_data()))
106+
depth_temp = np.array(aligned_depth_frame.get_data())
107+
depth_image = x3d.geometry.Image(depth_temp)
104108
color_temp = np.asarray(color_frame.get_data())
105109
color_image = x3d.geometry.Image(color_temp)
106110

@@ -124,6 +128,11 @@ def update_odom(frame):
124128
line.set_3d_properties(data[2])
125129
pos.set_data([cur_trans[0, 3]], [cur_trans[1, 3]])
126130
pos.set_3d_properties(cur_trans[2, 3])
131+
depth_offset = depth_temp.min()
132+
depth_scale = depth_temp.max() - depth_offset
133+
depth_temp = np.clip((depth_temp - depth_offset) / depth_scale, 0.0, 1.0)
134+
depth_im.set_array((255.0 * depth_temp).astype(np.uint8))
135+
color_im.set_array(color_temp)
127136

128137
anim = animation.FuncAnimation(fig, update_odom, interval=10)
129138
plt.show()

‎src/cupoch/camera/pinhole_camera_intrinsic.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#include "cupoch/camera/pinhole_camera_intrinsic.h"
22

33
#include <json/json.h>
4+
45
#include <Eigen/Dense>
56

67
#include "cupoch/utility/console.h"

‎src/cupoch/camera/pinhole_camera_intrinsic.h

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,9 @@
11
#pragma once
22

3-
#include <Eigen/Core>
43
#include <thrust/pair.h>
54

5+
#include <Eigen/Core>
6+
67
#include "cupoch/utility/ijson_convertible.h"
78

89
namespace cupoch {
@@ -66,13 +67,15 @@ class PinholeCameraIntrinsic : public utility::IJsonConvertible {
6667

6768
/// Returns the focal length in a tuple of X-axis and Y-axis focal lengths.
6869
thrust::pair<float, float> GetFocalLength() const {
69-
return thrust::make_pair(intrinsic_matrix_(0, 0), intrinsic_matrix_(1, 1));
70+
return thrust::make_pair(intrinsic_matrix_(0, 0),
71+
intrinsic_matrix_(1, 1));
7072
}
7173

7274
/// Returns the principle point in a tuple of X-axis and Y-axis principle
7375
/// point.
7476
thrust::pair<float, float> GetPrincipalPoint() const {
75-
return thrust::make_pair(intrinsic_matrix_(0, 2), intrinsic_matrix_(1, 2));
77+
return thrust::make_pair(intrinsic_matrix_(0, 2),
78+
intrinsic_matrix_(1, 2));
7679
}
7780

7881
/// Returns the skew.

‎src/cupoch/geometry/boundingvolume.cu

Lines changed: 69 additions & 64 deletions
Original file line numberDiff line numberDiff line change
@@ -1,69 +1,69 @@
1+
#include <Eigen/Eigenvalues>
2+
#include <numeric>
3+
14
#include "cupoch/geometry/boundingvolume.h"
25
#include "cupoch/geometry/pointcloud.h"
36
#include "cupoch/geometry/trianglemesh.h"
4-
#include "cupoch/utility/platform.h"
57
#include "cupoch/utility/console.h"
6-
7-
#include <numeric>
8-
9-
#include <Eigen/Eigenvalues>
8+
#include "cupoch/utility/platform.h"
109

1110
using namespace cupoch;
1211
using namespace cupoch::geometry;
1312

1413
namespace {
1514

1615
struct check_within_oriented_bounding_box_functor {
17-
check_within_oriented_bounding_box_functor(const Eigen::Vector3f* points,
18-
const std::array<Eigen::Vector3f, 8>& box_points)
19-
: points_(points), box_points_(box_points) {};
20-
const Eigen::Vector3f* points_;
16+
check_within_oriented_bounding_box_functor(
17+
const Eigen::Vector3f *points,
18+
const std::array<Eigen::Vector3f, 8> &box_points)
19+
: points_(points), box_points_(box_points){};
20+
const Eigen::Vector3f *points_;
2121
const std::array<Eigen::Vector3f, 8> box_points_;
22-
__device__
23-
float test_plane(const Eigen::Vector3f& a, const Eigen::Vector3f& b,
24-
const Eigen::Vector3f c, const Eigen::Vector3f& x) const {
22+
__device__ float test_plane(const Eigen::Vector3f &a,
23+
const Eigen::Vector3f &b,
24+
const Eigen::Vector3f c,
25+
const Eigen::Vector3f &x) const {
2526
Eigen::Matrix3f design;
2627
design << (b - a), (c - a), (x - a);
2728
return design.determinant();
2829
};
29-
__device__
30-
bool operator()(size_t idx) const {
31-
const Eigen::Vector3f& point = points_[idx];
32-
return (test_plane(box_points_[0], box_points_[1], box_points_[3], point) <=
33-
0 &&
34-
test_plane(box_points_[0], box_points_[5], box_points_[3], point) >=
35-
0 &&
36-
test_plane(box_points_[2], box_points_[5], box_points_[7], point) <=
37-
0 &&
38-
test_plane(box_points_[1], box_points_[4], box_points_[7], point) >=
39-
0 &&
40-
test_plane(box_points_[3], box_points_[4], box_points_[5], point) <=
41-
0 &&
42-
test_plane(box_points_[0], box_points_[1], box_points_[7], point) >=
43-
0);
30+
__device__ bool operator()(size_t idx) const {
31+
const Eigen::Vector3f &point = points_[idx];
32+
return (test_plane(box_points_[0], box_points_[1], box_points_[3],
33+
point) <= 0 &&
34+
test_plane(box_points_[0], box_points_[5], box_points_[3],
35+
point) >= 0 &&
36+
test_plane(box_points_[2], box_points_[5], box_points_[7],
37+
point) <= 0 &&
38+
test_plane(box_points_[1], box_points_[4], box_points_[7],
39+
point) >= 0 &&
40+
test_plane(box_points_[3], box_points_[4], box_points_[5],
41+
point) <= 0 &&
42+
test_plane(box_points_[0], box_points_[1], box_points_[7],
43+
point) >= 0);
4444
}
4545
};
4646

4747
struct check_within_axis_aligned_bounding_box_functor {
48-
check_within_axis_aligned_bounding_box_functor(const Eigen::Vector3f* points,
49-
const Eigen::Vector3f& min_bound,
50-
const Eigen::Vector3f& max_bound)
51-
: points_(points), min_bound_(min_bound), max_bound_(max_bound) {};
52-
const Eigen::Vector3f* points_;
48+
check_within_axis_aligned_bounding_box_functor(
49+
const Eigen::Vector3f *points,
50+
const Eigen::Vector3f &min_bound,
51+
const Eigen::Vector3f &max_bound)
52+
: points_(points), min_bound_(min_bound), max_bound_(max_bound){};
53+
const Eigen::Vector3f *points_;
5354
const Eigen::Vector3f min_bound_;
5455
const Eigen::Vector3f max_bound_;
55-
__device__
56-
bool operator() (size_t idx) const {
57-
const Eigen::Vector3f& point = points_[idx];
56+
__device__ bool operator()(size_t idx) const {
57+
const Eigen::Vector3f &point = points_[idx];
5858
return (point(0) >= min_bound_(0) && point(0) <= max_bound_(0) &&
5959
point(1) >= min_bound_(1) && point(1) <= max_bound_(1) &&
6060
point(2) >= min_bound_(2) && point(2) <= max_bound_(2));
6161
}
6262
};
6363

64-
}
64+
} // namespace
6565

66-
OrientedBoundingBox& OrientedBoundingBox::Clear() {
66+
OrientedBoundingBox &OrientedBoundingBox::Clear() {
6767
center_.setZero();
6868
extent_.setZero();
6969
R_ = Eigen::Matrix3f::Identity();
@@ -93,16 +93,16 @@ OrientedBoundingBox OrientedBoundingBox::GetOrientedBoundingBox() const {
9393
return *this;
9494
}
9595

96-
OrientedBoundingBox& OrientedBoundingBox::Transform(
97-
const Eigen::Matrix4f& transformation) {
96+
OrientedBoundingBox &OrientedBoundingBox::Transform(
97+
const Eigen::Matrix4f &transformation) {
9898
utility::LogError(
9999
"A general transform of an OrientedBoundingBox is not implemented. "
100100
"Call Translate, Scale, and Rotate.");
101101
return *this;
102102
}
103103

104-
OrientedBoundingBox& OrientedBoundingBox::Translate(
105-
const Eigen::Vector3f& translation, bool relative) {
104+
OrientedBoundingBox &OrientedBoundingBox::Translate(
105+
const Eigen::Vector3f &translation, bool relative) {
106106
if (relative) {
107107
center_ += translation;
108108
} else {
@@ -111,7 +111,7 @@ OrientedBoundingBox& OrientedBoundingBox::Translate(
111111
return *this;
112112
}
113113

114-
OrientedBoundingBox& OrientedBoundingBox::Scale(const float scale,
114+
OrientedBoundingBox &OrientedBoundingBox::Scale(const float scale,
115115
bool center) {
116116
if (center) {
117117
extent_ *= scale;
@@ -122,7 +122,7 @@ OrientedBoundingBox& OrientedBoundingBox::Scale(const float scale,
122122
return *this;
123123
}
124124

125-
OrientedBoundingBox& OrientedBoundingBox::Rotate(const Eigen::Matrix3f& R,
125+
OrientedBoundingBox &OrientedBoundingBox::Rotate(const Eigen::Matrix3f &R,
126126
bool center) {
127127
if (center) {
128128
R_ *= R;
@@ -153,27 +153,30 @@ std::array<Eigen::Vector3f, 8> OrientedBoundingBox::GetBoxPoints() const {
153153
return points;
154154
}
155155

156-
utility::device_vector<size_t> OrientedBoundingBox::GetPointIndicesWithinBoundingBox(
157-
const utility::device_vector<Eigen::Vector3f>& points) const {
156+
utility::device_vector<size_t>
157+
OrientedBoundingBox::GetPointIndicesWithinBoundingBox(
158+
const utility::device_vector<Eigen::Vector3f> &points) const {
158159
auto box_points = GetBoxPoints();
159-
check_within_oriented_bounding_box_functor func(thrust::raw_pointer_cast(points.data()), box_points);
160+
check_within_oriented_bounding_box_functor func(
161+
thrust::raw_pointer_cast(points.data()), box_points);
160162
utility::device_vector<size_t> indices(points.size());
161-
auto end = thrust::copy_if(thrust::make_counting_iterator<size_t>(0), thrust::make_counting_iterator(points.size()),
163+
auto end = thrust::copy_if(thrust::make_counting_iterator<size_t>(0),
164+
thrust::make_counting_iterator(points.size()),
162165
indices.begin(), func);
163166
indices.resize(thrust::distance(indices.begin(), end));
164167
return indices;
165168
}
166169

167170
OrientedBoundingBox OrientedBoundingBox::CreateFromAxisAlignedBoundingBox(
168-
const AxisAlignedBoundingBox& aabox) {
171+
const AxisAlignedBoundingBox &aabox) {
169172
OrientedBoundingBox obox;
170173
obox.center_ = aabox.GetCenter();
171174
obox.extent_ = aabox.GetExtent();
172175
obox.R_ = Eigen::Matrix3f::Identity();
173176
return obox;
174177
}
175178

176-
AxisAlignedBoundingBox& AxisAlignedBoundingBox::Clear() {
179+
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Clear() {
177180
min_bound_.setZero();
178181
max_bound_.setZero();
179182
return *this;
@@ -201,16 +204,16 @@ OrientedBoundingBox AxisAlignedBoundingBox::GetOrientedBoundingBox() const {
201204
return OrientedBoundingBox::CreateFromAxisAlignedBoundingBox(*this);
202205
}
203206

204-
AxisAlignedBoundingBox& AxisAlignedBoundingBox::Transform(
205-
const Eigen::Matrix4f& transformation) {
207+
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Transform(
208+
const Eigen::Matrix4f &transformation) {
206209
utility::LogError(
207210
"A general transform of a AxisAlignedBoundingBox would not be axis "
208211
"aligned anymore, convert it to a OrientedBoundingBox first");
209212
return *this;
210213
}
211214

212-
AxisAlignedBoundingBox& AxisAlignedBoundingBox::Translate(
213-
const Eigen::Vector3f& translation, bool relative) {
215+
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Translate(
216+
const Eigen::Vector3f &translation, bool relative) {
214217
if (relative) {
215218
min_bound_ += translation;
216219
max_bound_ += translation;
@@ -222,7 +225,7 @@ AxisAlignedBoundingBox& AxisAlignedBoundingBox::Translate(
222225
return *this;
223226
}
224227

225-
AxisAlignedBoundingBox& AxisAlignedBoundingBox::Scale(const float scale,
228+
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Scale(const float scale,
226229
bool center) {
227230
if (center) {
228231
Eigen::Vector3f center = GetCenter();
@@ -235,8 +238,8 @@ AxisAlignedBoundingBox& AxisAlignedBoundingBox::Scale(const float scale,
235238
return *this;
236239
}
237240

238-
AxisAlignedBoundingBox& AxisAlignedBoundingBox::Rotate(
239-
const Eigen::Matrix3f& rotation, bool center) {
241+
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Rotate(
242+
const Eigen::Matrix3f &rotation, bool center) {
240243
utility::LogError(
241244
"A rotation of a AxisAlignedBoundingBox would not be axis aligned "
242245
"anymore, convert it to an OrientedBoundingBox first");
@@ -249,8 +252,8 @@ std::string AxisAlignedBoundingBox::GetPrintInfo() const {
249252
max_bound_(0), max_bound_(1), max_bound_(2));
250253
}
251254

252-
AxisAlignedBoundingBox& AxisAlignedBoundingBox::operator+=(
253-
const AxisAlignedBoundingBox& other) {
255+
AxisAlignedBoundingBox &AxisAlignedBoundingBox::operator+=(
256+
const AxisAlignedBoundingBox &other) {
254257
if (IsEmpty()) {
255258
min_bound_ = other.min_bound_;
256259
max_bound_ = other.max_bound_;
@@ -262,7 +265,7 @@ AxisAlignedBoundingBox& AxisAlignedBoundingBox::operator+=(
262265
}
263266

264267
AxisAlignedBoundingBox AxisAlignedBoundingBox::CreateFromPoints(
265-
const utility::device_vector<Eigen::Vector3f>& points) {
268+
const utility::device_vector<Eigen::Vector3f> &points) {
266269
AxisAlignedBoundingBox box;
267270
if (points.empty()) {
268271
box.min_bound_ = Eigen::Vector3f(0.0, 0.0, 0.0);
@@ -290,12 +293,14 @@ std::array<Eigen::Vector3f, 8> AxisAlignedBoundingBox::GetBoxPoints() const {
290293
return points;
291294
}
292295

293-
utility::device_vector<size_t> AxisAlignedBoundingBox::GetPointIndicesWithinBoundingBox(
294-
const utility::device_vector<Eigen::Vector3f>& points) const {
296+
utility::device_vector<size_t>
297+
AxisAlignedBoundingBox::GetPointIndicesWithinBoundingBox(
298+
const utility::device_vector<Eigen::Vector3f> &points) const {
295299
utility::device_vector<size_t> indices(points.size());
296-
check_within_axis_aligned_bounding_box_functor func(thrust::raw_pointer_cast(points.data()),
297-
min_bound_, max_bound_);
298-
auto end = thrust::copy_if(thrust::make_counting_iterator<size_t>(0), thrust::make_counting_iterator(points.size()),
300+
check_within_axis_aligned_bounding_box_functor func(
301+
thrust::raw_pointer_cast(points.data()), min_bound_, max_bound_);
302+
auto end = thrust::copy_if(thrust::make_counting_iterator<size_t>(0),
303+
thrust::make_counting_iterator(points.size()),
299304
indices.begin(), func);
300305
indices.resize(thrust::distance(indices.begin(), end));
301306
return indices;

0 commit comments

Comments
 (0)