Skip to content

Commit

Permalink
clang format
Browse files Browse the repository at this point in the history
  • Loading branch information
@neka-nat
neka-nat committed Mar 1, 2020
1 parent a59ba53 commit 9c63e7a
Show file tree
Hide file tree
Showing 94 changed files with 6,843 additions and 3,766 deletions.
10 changes: 10 additions & 0 deletions .clang-format
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
BasedOnStyle: Google
IndentWidth: 4
ColumnLimit: 80
UseTab: Never
Language: Cpp
Standard: Cpp11
ContinuationIndentWidth: 8
AccessModifierOffset: -4
BinPackParameters: false
SortIncludes: true
4 changes: 4 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,10 @@ python benchmarks.py

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

### Visual odometry with intel realsense D435

![vo](https://raw.githubusercontent.com/neka-nat/cupoch/master/docs/_static/vo_gpu.gif)

## References

* CUDA repository forked from Open3D, https://github.com/theNded/Open3D
Binary file added docs/_static/vo_gpu.gif
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
19 changes: 14 additions & 5 deletions examples/python/realsense/realsense_rgbd_odometry.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,16 +67,20 @@ def get_intrinsic_matrix(frame):
align = rs.align(align_to)

fig = plt.figure()
ax = p3.Axes3D(fig)
ax1 = fig.add_subplot(1, 2, 1, projection='3d')
ax2 = fig.add_subplot(2, 2, 2)
ax3 = fig.add_subplot(2, 2, 4)

# Streaming loop
prev_rgbd_image = None
option = x3d.odometry.OdometryOption()
cur_trans = np.identity(4)
path = []
path.append(cur_trans[:3, 3].tolist())
line = ax.plot(*list(zip(*path)), 'r-')[0]
pos = ax.plot(*list(zip(*path)), 'yo')[0]
line = ax1.plot(*list(zip(*path)), 'r-')[0]
pos = ax1.plot(*list(zip(*path)), 'yo')[0]
depth_im = ax2.imshow(np.zeros((480, 640), dtype=np.uint8), cmap="gray", vmin=0, vmax=255)
color_im = ax3.imshow(np.zeros((480, 640, 3), dtype=np.uint8))
try:
def update_odom(frame):
global prev_rgbd_image, cur_trans
Expand All @@ -99,8 +103,8 @@ def update_odom(frame):
if not aligned_depth_frame or not color_frame:
return

depth_image = x3d.geometry.Image(
np.array(aligned_depth_frame.get_data()))
depth_temp = np.array(aligned_depth_frame.get_data())
depth_image = x3d.geometry.Image(depth_temp)
color_temp = np.asarray(color_frame.get_data())
color_image = x3d.geometry.Image(color_temp)

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

anim = animation.FuncAnimation(fig, update_odom, interval=10)
plt.show()
Expand Down
1 change: 1 addition & 0 deletions src/cupoch/camera/pinhole_camera_intrinsic.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "cupoch/camera/pinhole_camera_intrinsic.h"

#include <json/json.h>

#include <Eigen/Dense>

#include "cupoch/utility/console.h"
Expand Down
9 changes: 6 additions & 3 deletions src/cupoch/camera/pinhole_camera_intrinsic.h
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,9 @@
#pragma once

#include <Eigen/Core>
#include <thrust/pair.h>

#include <Eigen/Core>

#include "cupoch/utility/ijson_convertible.h"

namespace cupoch {
Expand Down Expand Up @@ -66,13 +67,15 @@ class PinholeCameraIntrinsic : public utility::IJsonConvertible {

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

/// Returns the principle point in a tuple of X-axis and Y-axis principle
/// point.
thrust::pair<float, float> GetPrincipalPoint() const {
return thrust::make_pair(intrinsic_matrix_(0, 2), intrinsic_matrix_(1, 2));
return thrust::make_pair(intrinsic_matrix_(0, 2),
intrinsic_matrix_(1, 2));
}

/// Returns the skew.
Expand Down
133 changes: 69 additions & 64 deletions src/cupoch/geometry/boundingvolume.cu
View file
Original file line number Diff line number Diff line change
@@ -1,69 +1,69 @@
#include <Eigen/Eigenvalues>
#include <numeric>

#include "cupoch/geometry/boundingvolume.h"
#include "cupoch/geometry/pointcloud.h"
#include "cupoch/geometry/trianglemesh.h"
#include "cupoch/utility/platform.h"
#include "cupoch/utility/console.h"

#include <numeric>

#include <Eigen/Eigenvalues>
#include "cupoch/utility/platform.h"

using namespace cupoch;
using namespace cupoch::geometry;

namespace {

struct check_within_oriented_bounding_box_functor {
check_within_oriented_bounding_box_functor(const Eigen::Vector3f* points,
const std::array<Eigen::Vector3f, 8>& box_points)
: points_(points), box_points_(box_points) {};
const Eigen::Vector3f* points_;
check_within_oriented_bounding_box_functor(
const Eigen::Vector3f *points,
const std::array<Eigen::Vector3f, 8> &box_points)
: points_(points), box_points_(box_points){};
const Eigen::Vector3f *points_;
const std::array<Eigen::Vector3f, 8> box_points_;
__device__
float test_plane(const Eigen::Vector3f& a, const Eigen::Vector3f& b,
const Eigen::Vector3f c, const Eigen::Vector3f& x) const {
__device__ float test_plane(const Eigen::Vector3f &a,
const Eigen::Vector3f &b,
const Eigen::Vector3f c,
const Eigen::Vector3f &x) const {
Eigen::Matrix3f design;
design << (b - a), (c - a), (x - a);
return design.determinant();
};
__device__
bool operator()(size_t idx) const {
const Eigen::Vector3f& point = points_[idx];
return (test_plane(box_points_[0], box_points_[1], box_points_[3], point) <=
0 &&
test_plane(box_points_[0], box_points_[5], box_points_[3], point) >=
0 &&
test_plane(box_points_[2], box_points_[5], box_points_[7], point) <=
0 &&
test_plane(box_points_[1], box_points_[4], box_points_[7], point) >=
0 &&
test_plane(box_points_[3], box_points_[4], box_points_[5], point) <=
0 &&
test_plane(box_points_[0], box_points_[1], box_points_[7], point) >=
0);
__device__ bool operator()(size_t idx) const {
const Eigen::Vector3f &point = points_[idx];
return (test_plane(box_points_[0], box_points_[1], box_points_[3],
point) <= 0 &&
test_plane(box_points_[0], box_points_[5], box_points_[3],
point) >= 0 &&
test_plane(box_points_[2], box_points_[5], box_points_[7],
point) <= 0 &&
test_plane(box_points_[1], box_points_[4], box_points_[7],
point) >= 0 &&
test_plane(box_points_[3], box_points_[4], box_points_[5],
point) <= 0 &&
test_plane(box_points_[0], box_points_[1], box_points_[7],
point) >= 0);
}
};

struct check_within_axis_aligned_bounding_box_functor {
check_within_axis_aligned_bounding_box_functor(const Eigen::Vector3f* points,
const Eigen::Vector3f& min_bound,
const Eigen::Vector3f& max_bound)
: points_(points), min_bound_(min_bound), max_bound_(max_bound) {};
const Eigen::Vector3f* points_;
check_within_axis_aligned_bounding_box_functor(
const Eigen::Vector3f *points,
const Eigen::Vector3f &min_bound,
const Eigen::Vector3f &max_bound)
: points_(points), min_bound_(min_bound), max_bound_(max_bound){};
const Eigen::Vector3f *points_;
const Eigen::Vector3f min_bound_;
const Eigen::Vector3f max_bound_;
__device__
bool operator() (size_t idx) const {
const Eigen::Vector3f& point = points_[idx];
__device__ bool operator()(size_t idx) const {
const Eigen::Vector3f &point = points_[idx];
return (point(0) >= min_bound_(0) && point(0) <= max_bound_(0) &&
point(1) >= min_bound_(1) && point(1) <= max_bound_(1) &&
point(2) >= min_bound_(2) && point(2) <= max_bound_(2));
}
};

}
} // namespace

OrientedBoundingBox& OrientedBoundingBox::Clear() {
OrientedBoundingBox &OrientedBoundingBox::Clear() {
center_.setZero();
extent_.setZero();
R_ = Eigen::Matrix3f::Identity();
Expand Down Expand Up @@ -93,16 +93,16 @@ OrientedBoundingBox OrientedBoundingBox::GetOrientedBoundingBox() const {
return *this;
}

OrientedBoundingBox& OrientedBoundingBox::Transform(
const Eigen::Matrix4f& transformation) {
OrientedBoundingBox &OrientedBoundingBox::Transform(
const Eigen::Matrix4f &transformation) {
utility::LogError(
"A general transform of an OrientedBoundingBox is not implemented. "
"Call Translate, Scale, and Rotate.");
return *this;
}

OrientedBoundingBox& OrientedBoundingBox::Translate(
const Eigen::Vector3f& translation, bool relative) {
OrientedBoundingBox &OrientedBoundingBox::Translate(
const Eigen::Vector3f &translation, bool relative) {
if (relative) {
center_ += translation;
} else {
Expand All @@ -111,7 +111,7 @@ OrientedBoundingBox& OrientedBoundingBox::Translate(
return *this;
}

OrientedBoundingBox& OrientedBoundingBox::Scale(const float scale,
OrientedBoundingBox &OrientedBoundingBox::Scale(const float scale,
bool center) {
if (center) {
extent_ *= scale;
Expand All @@ -122,7 +122,7 @@ OrientedBoundingBox& OrientedBoundingBox::Scale(const float scale,
return *this;
}

OrientedBoundingBox& OrientedBoundingBox::Rotate(const Eigen::Matrix3f& R,
OrientedBoundingBox &OrientedBoundingBox::Rotate(const Eigen::Matrix3f &R,
bool center) {
if (center) {
R_ *= R;
Expand Down Expand Up @@ -153,27 +153,30 @@ std::array<Eigen::Vector3f, 8> OrientedBoundingBox::GetBoxPoints() const {
return points;
}

utility::device_vector<size_t> OrientedBoundingBox::GetPointIndicesWithinBoundingBox(
const utility::device_vector<Eigen::Vector3f>& points) const {
utility::device_vector<size_t>
OrientedBoundingBox::GetPointIndicesWithinBoundingBox(
const utility::device_vector<Eigen::Vector3f> &points) const {
auto box_points = GetBoxPoints();
check_within_oriented_bounding_box_functor func(thrust::raw_pointer_cast(points.data()), box_points);
check_within_oriented_bounding_box_functor func(
thrust::raw_pointer_cast(points.data()), box_points);
utility::device_vector<size_t> indices(points.size());
auto end = thrust::copy_if(thrust::make_counting_iterator<size_t>(0), thrust::make_counting_iterator(points.size()),
auto end = thrust::copy_if(thrust::make_counting_iterator<size_t>(0),
thrust::make_counting_iterator(points.size()),
indices.begin(), func);
indices.resize(thrust::distance(indices.begin(), end));
return indices;
}

OrientedBoundingBox OrientedBoundingBox::CreateFromAxisAlignedBoundingBox(
const AxisAlignedBoundingBox& aabox) {
const AxisAlignedBoundingBox &aabox) {
OrientedBoundingBox obox;
obox.center_ = aabox.GetCenter();
obox.extent_ = aabox.GetExtent();
obox.R_ = Eigen::Matrix3f::Identity();
return obox;
}

AxisAlignedBoundingBox& AxisAlignedBoundingBox::Clear() {
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Clear() {
min_bound_.setZero();
max_bound_.setZero();
return *this;
Expand Down Expand Up @@ -201,16 +204,16 @@ OrientedBoundingBox AxisAlignedBoundingBox::GetOrientedBoundingBox() const {
return OrientedBoundingBox::CreateFromAxisAlignedBoundingBox(*this);
}

AxisAlignedBoundingBox& AxisAlignedBoundingBox::Transform(
const Eigen::Matrix4f& transformation) {
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Transform(
const Eigen::Matrix4f &transformation) {
utility::LogError(
"A general transform of a AxisAlignedBoundingBox would not be axis "
"aligned anymore, convert it to a OrientedBoundingBox first");
return *this;
}

AxisAlignedBoundingBox& AxisAlignedBoundingBox::Translate(
const Eigen::Vector3f& translation, bool relative) {
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Translate(
const Eigen::Vector3f &translation, bool relative) {
if (relative) {
min_bound_ += translation;
max_bound_ += translation;
Expand All @@ -222,7 +225,7 @@ AxisAlignedBoundingBox& AxisAlignedBoundingBox::Translate(
return *this;
}

AxisAlignedBoundingBox& AxisAlignedBoundingBox::Scale(const float scale,
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Scale(const float scale,
bool center) {
if (center) {
Eigen::Vector3f center = GetCenter();
Expand All @@ -235,8 +238,8 @@ AxisAlignedBoundingBox& AxisAlignedBoundingBox::Scale(const float scale,
return *this;
}

AxisAlignedBoundingBox& AxisAlignedBoundingBox::Rotate(
const Eigen::Matrix3f& rotation, bool center) {
AxisAlignedBoundingBox &AxisAlignedBoundingBox::Rotate(
const Eigen::Matrix3f &rotation, bool center) {
utility::LogError(
"A rotation of a AxisAlignedBoundingBox would not be axis aligned "
"anymore, convert it to an OrientedBoundingBox first");
Expand All @@ -249,8 +252,8 @@ std::string AxisAlignedBoundingBox::GetPrintInfo() const {
max_bound_(0), max_bound_(1), max_bound_(2));
}

AxisAlignedBoundingBox& AxisAlignedBoundingBox::operator+=(
const AxisAlignedBoundingBox& other) {
AxisAlignedBoundingBox &AxisAlignedBoundingBox::operator+=(
const AxisAlignedBoundingBox &other) {
if (IsEmpty()) {
min_bound_ = other.min_bound_;
max_bound_ = other.max_bound_;
Expand All @@ -262,7 +265,7 @@ AxisAlignedBoundingBox& AxisAlignedBoundingBox::operator+=(
}

AxisAlignedBoundingBox AxisAlignedBoundingBox::CreateFromPoints(
const utility::device_vector<Eigen::Vector3f>& points) {
const utility::device_vector<Eigen::Vector3f> &points) {
AxisAlignedBoundingBox box;
if (points.empty()) {
box.min_bound_ = Eigen::Vector3f(0.0, 0.0, 0.0);
Expand Down Expand Up @@ -290,12 +293,14 @@ std::array<Eigen::Vector3f, 8> AxisAlignedBoundingBox::GetBoxPoints() const {
return points;
}

utility::device_vector<size_t> AxisAlignedBoundingBox::GetPointIndicesWithinBoundingBox(
const utility::device_vector<Eigen::Vector3f>& points) const {
utility::device_vector<size_t>
AxisAlignedBoundingBox::GetPointIndicesWithinBoundingBox(
const utility::device_vector<Eigen::Vector3f> &points) const {
utility::device_vector<size_t> indices(points.size());
check_within_axis_aligned_bounding_box_functor func(thrust::raw_pointer_cast(points.data()),
min_bound_, max_bound_);
auto end = thrust::copy_if(thrust::make_counting_iterator<size_t>(0), thrust::make_counting_iterator(points.size()),
check_within_axis_aligned_bounding_box_functor func(
thrust::raw_pointer_cast(points.data()), min_bound_, max_bound_);
auto end = thrust::copy_if(thrust::make_counting_iterator<size_t>(0),
thrust::make_counting_iterator(points.size()),
indices.begin(), func);
indices.resize(thrust::distance(indices.begin(), end));
return indices;
Expand Down
Loading

0 comments on commit 9c63e7a

Please sign in to comment.