diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..678d1f6b --- /dev/null +++ b/.clang-format @@ -0,0 +1,10 @@ +BasedOnStyle: Google +IndentWidth: 4 +ColumnLimit: 80 +UseTab: Never +Language: Cpp +Standard: Cpp11 +ContinuationIndentWidth: 8 +AccessModifierOffset: -4 +BinPackParameters: false +SortIncludes: true diff --git a/README.md b/README.md index 8f54a658..86602284 100644 --- a/README.md +++ b/README.md @@ -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 \ No newline at end of file diff --git a/docs/_static/vo_gpu.gif b/docs/_static/vo_gpu.gif new file mode 100644 index 00000000..17370130 Binary files /dev/null and b/docs/_static/vo_gpu.gif differ diff --git a/examples/python/realsense/realsense_rgbd_odometry.py b/examples/python/realsense/realsense_rgbd_odometry.py index d7aa08b8..2e148e07 100644 --- a/examples/python/realsense/realsense_rgbd_odometry.py +++ b/examples/python/realsense/realsense_rgbd_odometry.py @@ -67,7 +67,9 @@ 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 @@ -75,8 +77,10 @@ def get_intrinsic_matrix(frame): 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 @@ -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) @@ -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() diff --git a/src/cupoch/camera/pinhole_camera_intrinsic.cpp b/src/cupoch/camera/pinhole_camera_intrinsic.cpp index 9ce383ff..13770bae 100644 --- a/src/cupoch/camera/pinhole_camera_intrinsic.cpp +++ b/src/cupoch/camera/pinhole_camera_intrinsic.cpp @@ -1,6 +1,7 @@ #include "cupoch/camera/pinhole_camera_intrinsic.h" #include + #include #include "cupoch/utility/console.h" diff --git a/src/cupoch/camera/pinhole_camera_intrinsic.h b/src/cupoch/camera/pinhole_camera_intrinsic.h index bb22d176..d3d79726 100644 --- a/src/cupoch/camera/pinhole_camera_intrinsic.h +++ b/src/cupoch/camera/pinhole_camera_intrinsic.h @@ -1,8 +1,9 @@ #pragma once -#include #include +#include + #include "cupoch/utility/ijson_convertible.h" namespace cupoch { @@ -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 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 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. diff --git a/src/cupoch/geometry/boundingvolume.cu b/src/cupoch/geometry/boundingvolume.cu index 8c445561..32c93a55 100644 --- a/src/cupoch/geometry/boundingvolume.cu +++ b/src/cupoch/geometry/boundingvolume.cu @@ -1,12 +1,11 @@ +#include +#include + #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 - -#include +#include "cupoch/utility/platform.h" using namespace cupoch; using namespace cupoch::geometry; @@ -14,56 +13,57 @@ 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& 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 &box_points) + : points_(points), box_points_(box_points){}; + const Eigen::Vector3f *points_; const std::array 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(); @@ -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 { @@ -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; @@ -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; @@ -153,19 +153,22 @@ std::array OrientedBoundingBox::GetBoxPoints() const { return points; } -utility::device_vector OrientedBoundingBox::GetPointIndicesWithinBoundingBox( - const utility::device_vector& points) const { +utility::device_vector +OrientedBoundingBox::GetPointIndicesWithinBoundingBox( + const utility::device_vector &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 indices(points.size()); - auto end = thrust::copy_if(thrust::make_counting_iterator(0), thrust::make_counting_iterator(points.size()), + auto end = thrust::copy_if(thrust::make_counting_iterator(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(); @@ -173,7 +176,7 @@ OrientedBoundingBox OrientedBoundingBox::CreateFromAxisAlignedBoundingBox( return obox; } -AxisAlignedBoundingBox& AxisAlignedBoundingBox::Clear() { +AxisAlignedBoundingBox &AxisAlignedBoundingBox::Clear() { min_bound_.setZero(); max_bound_.setZero(); return *this; @@ -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; @@ -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(); @@ -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"); @@ -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_; @@ -262,7 +265,7 @@ AxisAlignedBoundingBox& AxisAlignedBoundingBox::operator+=( } AxisAlignedBoundingBox AxisAlignedBoundingBox::CreateFromPoints( - const utility::device_vector& points) { + const utility::device_vector &points) { AxisAlignedBoundingBox box; if (points.empty()) { box.min_bound_ = Eigen::Vector3f(0.0, 0.0, 0.0); @@ -290,12 +293,14 @@ std::array AxisAlignedBoundingBox::GetBoxPoints() const { return points; } -utility::device_vector AxisAlignedBoundingBox::GetPointIndicesWithinBoundingBox( - const utility::device_vector& points) const { +utility::device_vector +AxisAlignedBoundingBox::GetPointIndicesWithinBoundingBox( + const utility::device_vector &points) const { utility::device_vector 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(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(0), + thrust::make_counting_iterator(points.size()), indices.begin(), func); indices.resize(thrust::distance(indices.begin(), end)); return indices; diff --git a/src/cupoch/geometry/boundingvolume.h b/src/cupoch/geometry/boundingvolume.h index d35bb09d..859ddd8d 100644 --- a/src/cupoch/geometry/boundingvolume.h +++ b/src/cupoch/geometry/boundingvolume.h @@ -34,31 +34,30 @@ class OrientedBoundingBox : public Geometry3D { /// \param R The rotation matrix specifying the orientation of the /// bounding box with the original frame of reference. /// \param extent The extent of the bounding box. - OrientedBoundingBox(const Eigen::Vector3f& center, - const Eigen::Matrix3f& R, - const Eigen::Vector3f& extent) + OrientedBoundingBox(const Eigen::Vector3f ¢er, + const Eigen::Matrix3f &R, + const Eigen::Vector3f &extent) : Geometry3D(Geometry::GeometryType::OrientedBoundingBox), center_(center), R_(R), extent_(extent) {} - __host__ __device__ - ~OrientedBoundingBox() {} + __host__ __device__ ~OrientedBoundingBox() {} public: - OrientedBoundingBox& Clear() override; + OrientedBoundingBox &Clear() override; bool IsEmpty() const override; virtual Eigen::Vector3f GetMinBound() const override; virtual Eigen::Vector3f GetMaxBound() const override; virtual Eigen::Vector3f GetCenter() const override; virtual AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override; virtual OrientedBoundingBox GetOrientedBoundingBox() const; - virtual OrientedBoundingBox& Transform( - const Eigen::Matrix4f& transformation) override; - virtual OrientedBoundingBox& Translate(const Eigen::Vector3f& translation, + virtual OrientedBoundingBox &Transform( + const Eigen::Matrix4f &transformation) override; + virtual OrientedBoundingBox &Translate(const Eigen::Vector3f &translation, bool relative = true) override; - virtual OrientedBoundingBox& Scale(const float scale, + virtual OrientedBoundingBox &Scale(const float scale, bool center = true) override; - virtual OrientedBoundingBox& Rotate(const Eigen::Matrix3f& R, + virtual OrientedBoundingBox &Rotate(const Eigen::Matrix3f &R, bool center = true) override; /// Returns the volume of the bounding box. @@ -68,14 +67,14 @@ class OrientedBoundingBox : public Geometry3D { /// Return indices to points that are within the bounding box. utility::device_vector GetPointIndicesWithinBoundingBox( - const utility::device_vector& points) const; + const utility::device_vector &points) const; /// Returns an oriented bounding box from the AxisAlignedBoundingBox. /// /// \param aabox AxisAlignedBoundingBox object from which /// OrientedBoundingBox is created. static OrientedBoundingBox CreateFromAxisAlignedBoundingBox( - const AxisAlignedBoundingBox& aabox); + const AxisAlignedBoundingBox &aabox); public: /// The center point of the bounding box. @@ -110,33 +109,32 @@ class AxisAlignedBoundingBox : public Geometry3D { /// /// \param min_bound Lower bounds of the bounding box for all axes. /// \param max_bound Upper bounds of the bounding box for all axes. - AxisAlignedBoundingBox(const Eigen::Vector3f& min_bound, - const Eigen::Vector3f& max_bound) + AxisAlignedBoundingBox(const Eigen::Vector3f &min_bound, + const Eigen::Vector3f &max_bound) : Geometry3D(Geometry::GeometryType::AxisAlignedBoundingBox), min_bound_(min_bound), max_bound_(max_bound), color_(0, 0, 0) {} - __host__ __device__ - ~AxisAlignedBoundingBox() {} + __host__ __device__ ~AxisAlignedBoundingBox() {} public: - AxisAlignedBoundingBox& Clear() override; + AxisAlignedBoundingBox &Clear() override; bool IsEmpty() const override; virtual Eigen::Vector3f GetMinBound() const override; virtual Eigen::Vector3f GetMaxBound() const override; virtual Eigen::Vector3f GetCenter() const override; virtual AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override; virtual OrientedBoundingBox GetOrientedBoundingBox() const; - virtual AxisAlignedBoundingBox& Transform( - const Eigen::Matrix4f& transformation) override; - virtual AxisAlignedBoundingBox& Translate( - const Eigen::Vector3f& translation, bool relative = true) override; - virtual AxisAlignedBoundingBox& Scale(const float scale, + virtual AxisAlignedBoundingBox &Transform( + const Eigen::Matrix4f &transformation) override; + virtual AxisAlignedBoundingBox &Translate( + const Eigen::Vector3f &translation, bool relative = true) override; + virtual AxisAlignedBoundingBox &Scale(const float scale, bool center = true) override; - virtual AxisAlignedBoundingBox& Rotate(const Eigen::Matrix3f& R, + virtual AxisAlignedBoundingBox &Rotate(const Eigen::Matrix3f &R, bool center = true) override; - AxisAlignedBoundingBox& operator+=(const AxisAlignedBoundingBox& other); + AxisAlignedBoundingBox &operator+=(const AxisAlignedBoundingBox &other); /// Get the extent/length of the bounding box in x, y, and z dimension. Eigen::Vector3f GetExtent() const { return (max_bound_ - min_bound_); } @@ -148,18 +146,15 @@ class AxisAlignedBoundingBox : public Geometry3D { /// extents. float GetMaxExtent() const { return (max_bound_ - min_bound_).maxCoeff(); } - __host__ __device__ - float GetXPercentage(float x) const { + __host__ __device__ float GetXPercentage(float x) const { return (x - min_bound_(0)) / (max_bound_(0) - min_bound_(0)); } - __host__ __device__ - float GetYPercentage(float y) const { + __host__ __device__ float GetYPercentage(float y) const { return (y - min_bound_(1)) / (max_bound_(1) - min_bound_(1)); } - __host__ __device__ - float GetZPercentage(float z) const { + __host__ __device__ float GetZPercentage(float z) const { return (z - min_bound_(2)) / (max_bound_(2) - min_bound_(2)); } @@ -172,7 +167,7 @@ class AxisAlignedBoundingBox : public Geometry3D { /// /// \param points A list of points. utility::device_vector GetPointIndicesWithinBoundingBox( - const utility::device_vector& points) const; + const utility::device_vector &points) const; /// Returns the 3D dimensions of the bounding box in string format. std::string GetPrintInfo() const; @@ -181,7 +176,7 @@ class AxisAlignedBoundingBox : public Geometry3D { /// /// \param points A list of points. static AxisAlignedBoundingBox CreateFromPoints( - const utility::device_vector& points); + const utility::device_vector &points); public: /// The lower x, y, z bounds of the bounding box. diff --git a/src/cupoch/geometry/down_sample.cu b/src/cupoch/geometry/down_sample.cu index 28dbaf57..d6b82951 100644 --- a/src/cupoch/geometry/down_sample.cu +++ b/src/cupoch/geometry/down_sample.cu @@ -1,84 +1,92 @@ -#include "cupoch/geometry/pointcloud.h" +#include +#include + #include "cupoch/geometry/kdtree_flann.h" +#include "cupoch/geometry/pointcloud.h" #include "cupoch/utility/console.h" #include "cupoch/utility/helper.h" #include "cupoch/utility/platform.h" #include "cupoch/utility/range.h" -#include -#include using namespace cupoch; using namespace cupoch::geometry; namespace { -void SelectDownSampleImpl(const geometry::PointCloud& src, geometry::PointCloud& dst, +void SelectDownSampleImpl(const geometry::PointCloud &src, + geometry::PointCloud &dst, const utility::device_vector &indices) { const bool has_normals = src.HasNormals(); const bool has_colors = src.HasColors(); if (has_normals) dst.normals_.resize(indices.size()); if (has_colors) dst.colors_.resize(indices.size()); dst.points_.resize(indices.size()); - thrust::gather(utility::exec_policy(utility::GetStream(0))->on(utility::GetStream(0)), - indices.begin(), indices.end(), src.points_.begin(), dst.points_.begin()); + thrust::gather(utility::exec_policy(utility::GetStream(0)) + ->on(utility::GetStream(0)), + indices.begin(), indices.end(), src.points_.begin(), + dst.points_.begin()); if (has_normals) { - thrust::gather(utility::exec_policy(utility::GetStream(1))->on(utility::GetStream(1)), - indices.begin(), indices.end(), src.normals_.begin(), dst.normals_.begin()); + thrust::gather(utility::exec_policy(utility::GetStream(1)) + ->on(utility::GetStream(1)), + indices.begin(), indices.end(), src.normals_.begin(), + dst.normals_.begin()); } if (has_colors) { - thrust::gather(utility::exec_policy(utility::GetStream(2))->on(utility::GetStream(2)), - indices.begin(), indices.end(), src.colors_.begin(), dst.colors_.begin()); + thrust::gather(utility::exec_policy(utility::GetStream(2)) + ->on(utility::GetStream(2)), + indices.begin(), indices.end(), src.colors_.begin(), + dst.colors_.begin()); } cudaSafeCall(cudaDeviceSynchronize()); } struct compute_key_functor { - compute_key_functor(const Eigen::Vector3f& voxel_min_bound, float voxel_size) - : voxel_min_bound_(voxel_min_bound), voxel_size_(voxel_size) {}; + compute_key_functor(const Eigen::Vector3f &voxel_min_bound, + float voxel_size) + : voxel_min_bound_(voxel_min_bound), voxel_size_(voxel_size){}; const Eigen::Vector3f voxel_min_bound_; const float voxel_size_; - __device__ - Eigen::Vector3i operator()(const Eigen::Vector3f& pt) { + __device__ Eigen::Vector3i operator()(const Eigen::Vector3f &pt) { auto ref_coord = (pt - voxel_min_bound_) / voxel_size_; - return Eigen::Vector3i(int(floor(ref_coord(0))), int(floor(ref_coord(1))), int(floor(ref_coord(2)))); + return Eigen::Vector3i(int(floor(ref_coord(0))), + int(floor(ref_coord(1))), + int(floor(ref_coord(2)))); } }; -template -__host__ -int CalcAverageByKey(utility::device_vector& keys, - OutputIterator buf_begins, OutputIterator output_begins) { +template +__host__ int CalcAverageByKey(utility::device_vector &keys, + OutputIterator buf_begins, + OutputIterator output_begins) { const size_t n = keys.size(); thrust::sort_by_key(keys.begin(), keys.end(), buf_begins); utility::device_vector counts(n); - auto end1 = thrust::reduce_by_key(keys.begin(), keys.end(), - thrust::make_constant_iterator(1), - thrust::make_discard_iterator(), counts.begin()); + auto end1 = thrust::reduce_by_key( + keys.begin(), keys.end(), thrust::make_constant_iterator(1), + thrust::make_discard_iterator(), counts.begin()); int n_out = thrust::distance(counts.begin(), end1.second); counts.resize(n_out); thrust::equal_to binary_pred; add_tuple_functor add_func; auto end2 = thrust::reduce_by_key(keys.begin(), keys.end(), buf_begins, - thrust::make_discard_iterator(), output_begins, - binary_pred, add_func); + thrust::make_discard_iterator(), + output_begins, binary_pred, add_func); devided_tuple_functor dv_func; - thrust::transform(output_begins, output_begins + n_out, - counts.begin(), output_begins, - dv_func); + thrust::transform(output_begins, output_begins + n_out, counts.begin(), + output_begins, dv_func); return n_out; } struct has_radius_points_functor { - has_radius_points_functor(const int* indices, int n_points, int knn) - : indices_(indices), n_points_(n_points), knn_(knn) {}; - const int* indices_; + has_radius_points_functor(const int *indices, int n_points, int knn) + : indices_(indices), n_points_(n_points), knn_(knn){}; + const int *indices_; const int n_points_; const int knn_; - __device__ - bool operator() (int idx) const { + __device__ bool operator()(int idx) const { int count = 0; for (int i = 0; i < knn_; ++i) { if (indices_[idx * knn_ + i] >= 0) count++; @@ -88,11 +96,11 @@ struct has_radius_points_functor { }; struct average_distance_functor { - average_distance_functor(const float* distance, int knn) : distance_(distance), knn_(knn) {}; - const float* distance_; + average_distance_functor(const float *distance, int knn) + : distance_(distance), knn_(knn){}; + const float *distance_; const int knn_; - __device__ - float operator() (int idx) const { + __device__ float operator()(int idx) const { int count = 0; float avg = 0; for (int i = 0; i < knn_; ++i) { @@ -106,19 +114,20 @@ struct average_distance_functor { }; struct check_distance_threshold_functor { - check_distance_threshold_functor(const float* distances, float distance_threshold) - : distances_(distances), distance_threshold_(distance_threshold) {}; - const float* distances_; + check_distance_threshold_functor(const float *distances, + float distance_threshold) + : distances_(distances), distance_threshold_(distance_threshold){}; + const float *distances_; const float distance_threshold_; - __device__ - bool operator() (int idx) const { + __device__ bool operator()(int idx) const { return (distances_[idx] > 0 && distances_[idx] < distance_threshold_); } }; -} +} // namespace -std::shared_ptr PointCloud::SelectDownSample(const utility::device_vector &indices, bool invert) const { +std::shared_ptr PointCloud::SelectDownSample( + const utility::device_vector &indices, bool invert) const { auto output = std::make_shared(); if (invert) { @@ -126,8 +135,10 @@ std::shared_ptr PointCloud::SelectDownSample(const utility::device_v utility::device_vector sorted_indices = indices; thrust::sort(sorted_indices.begin(), sorted_indices.end()); utility::device_vector inv_indices(n_out); - thrust::set_difference(thrust::make_counting_iterator(0), thrust::make_counting_iterator(points_.size()), - sorted_indices.begin(), sorted_indices.end(), inv_indices.begin()); + thrust::set_difference(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(points_.size()), + sorted_indices.begin(), sorted_indices.end(), + inv_indices.begin()); SelectDownSampleImpl(*this, *output, inv_indices); } else { SelectDownSampleImpl(*this, *output, indices); @@ -135,18 +146,21 @@ std::shared_ptr PointCloud::SelectDownSample(const utility::device_v return output; } -std::shared_ptr PointCloud::VoxelDownSample(float voxel_size) const { +std::shared_ptr PointCloud::VoxelDownSample( + float voxel_size) const { auto output = std::make_shared(); if (voxel_size <= 0.0) { utility::LogWarning("[VoxelDownSample] voxel_size <= 0.\n"); return output; } - const Eigen::Vector3f voxel_size3 = Eigen::Vector3f(voxel_size, voxel_size, voxel_size); + const Eigen::Vector3f voxel_size3 = + Eigen::Vector3f(voxel_size, voxel_size, voxel_size); const Eigen::Vector3f voxel_min_bound = GetMinBound() - voxel_size3 * 0.5; const Eigen::Vector3f voxel_max_bound = GetMaxBound() + voxel_size3 * 0.5; - if (voxel_size * std::numeric_limits::max() < (voxel_max_bound - voxel_min_bound).maxCoeff()) { + if (voxel_size * std::numeric_limits::max() < + (voxel_max_bound - voxel_min_bound).maxCoeff()) { utility::LogWarning("[VoxelDownSample] voxel_size is too small.\n"); return output; } @@ -161,31 +175,46 @@ std::shared_ptr PointCloud::VoxelDownSample(float voxel_size) const utility::device_vector sorted_points = points_; output->points_.resize(n); if (!has_normals && !has_colors) { - typedef thrust::tuple::iterator> IteratorTuple; + typedef thrust::tuple::iterator> + IteratorTuple; typedef thrust::zip_iterator ZipIterator; - auto n_out = CalcAverageByKey(keys, - make_tuple_iterator(sorted_points.begin()), - make_tuple_iterator(output->points_.begin())); + auto n_out = CalcAverageByKey( + keys, make_tuple_iterator(sorted_points.begin()), + make_tuple_iterator(output->points_.begin())); output->points_.resize(n_out); } else if (has_normals && !has_colors) { utility::device_vector sorted_normals = normals_; output->normals_.resize(n); - typedef thrust::tuple::iterator, utility::device_vector::iterator> IteratorTuple; + typedef thrust::tuple::iterator, + utility::device_vector::iterator> + IteratorTuple; typedef thrust::zip_iterator ZipIterator; - auto n_out = CalcAverageByKey(keys, - make_tuple_iterator(sorted_points.begin(), sorted_normals.begin()), - make_tuple_iterator(output->points_.begin(), output->normals_.begin())); + auto n_out = + CalcAverageByKey( + keys, + make_tuple_iterator(sorted_points.begin(), + sorted_normals.begin()), + make_tuple_iterator(output->points_.begin(), + output->normals_.begin())); output->points_.resize(n_out); output->normals_.resize(n_out); - thrust::for_each(output->normals_.begin(), output->normals_.end(), [] __device__ (Eigen::Vector3f& nl) {nl.normalize();}); + thrust::for_each( + output->normals_.begin(), output->normals_.end(), + [] __device__(Eigen::Vector3f & nl) { nl.normalize(); }); } else if (!has_normals && has_colors) { utility::device_vector sorted_colors = colors_; output->colors_.resize(n); - typedef thrust::tuple::iterator, utility::device_vector::iterator> IteratorTuple; + typedef thrust::tuple::iterator, + utility::device_vector::iterator> + IteratorTuple; typedef thrust::zip_iterator ZipIterator; - auto n_out = CalcAverageByKey(keys, - make_tuple_iterator(sorted_points.begin(), sorted_colors.begin()), - make_tuple_iterator(output->points_.begin(), output->colors_.begin())); + auto n_out = + CalcAverageByKey( + keys, + make_tuple_iterator(sorted_points.begin(), + sorted_colors.begin()), + make_tuple_iterator(output->points_.begin(), + output->colors_.begin())); output->points_.resize(n_out); output->colors_.resize(n_out); } else { @@ -193,15 +222,26 @@ std::shared_ptr PointCloud::VoxelDownSample(float voxel_size) const utility::device_vector sorted_colors = colors_; output->normals_.resize(n); output->colors_.resize(n); - typedef thrust::tuple::iterator, utility::device_vector::iterator, utility::device_vector::iterator> IteratorTuple; + typedef thrust::tuple::iterator, + utility::device_vector::iterator, + utility::device_vector::iterator> + IteratorTuple; typedef thrust::zip_iterator ZipIterator; - auto n_out = CalcAverageByKey(keys, - make_tuple_iterator(sorted_points.begin(), sorted_normals.begin(), sorted_colors.begin()), - make_tuple_iterator(output->points_.begin(), output->normals_.begin(), output->colors_.begin())); + auto n_out = CalcAverageByKey( + keys, + make_tuple_iterator(sorted_points.begin(), + sorted_normals.begin(), + sorted_colors.begin()), + make_tuple_iterator(output->points_.begin(), + output->normals_.begin(), + output->colors_.begin())); output->points_.resize(n_out); output->normals_.resize(n_out); output->colors_.resize(n_out); - thrust::for_each(output->normals_.begin(), output->normals_.end(), [] __device__ (Eigen::Vector3f& nl) {nl.normalize();}); + thrust::for_each( + output->normals_.begin(), output->normals_.end(), + [] __device__(Eigen::Vector3f & nl) { nl.normalize(); }); } utility::LogDebug( @@ -211,7 +251,7 @@ std::shared_ptr PointCloud::VoxelDownSample(float voxel_size) const } std::shared_ptr PointCloud::UniformDownSample( - size_t every_k_points) const { + size_t every_k_points) const { const bool has_normals = HasNormals(); const bool has_colors = HasColors(); auto output = std::make_shared(); @@ -223,18 +263,30 @@ std::shared_ptr PointCloud::UniformDownSample( output->points_.resize(n_out); if (has_normals) output->normals_.resize(n_out); if (has_colors) output->colors_.resize(n_out); - thrust::strided_range::const_iterator> range_points(points_.begin(), points_.end(), every_k_points); - thrust::copy(utility::exec_policy(utility::GetStream(0))->on(utility::GetStream(0)), - range_points.begin(), range_points.end(), output->points_.begin()); + thrust::strided_range< + utility::device_vector::const_iterator> + range_points(points_.begin(), points_.end(), every_k_points); + thrust::copy(utility::exec_policy(utility::GetStream(0)) + ->on(utility::GetStream(0)), + range_points.begin(), range_points.end(), + output->points_.begin()); if (has_normals) { - thrust::strided_range::const_iterator> range_normals(normals_.begin(), normals_.end(), every_k_points); - thrust::copy(utility::exec_policy(utility::GetStream(1))->on(utility::GetStream(1)), - range_normals.begin(), range_normals.end(), output->normals_.begin()); + thrust::strided_range< + utility::device_vector::const_iterator> + range_normals(normals_.begin(), normals_.end(), every_k_points); + thrust::copy(utility::exec_policy(utility::GetStream(1)) + ->on(utility::GetStream(1)), + range_normals.begin(), range_normals.end(), + output->normals_.begin()); } if (has_colors) { - thrust::strided_range::const_iterator> range_colors(colors_.begin(), colors_.end(), every_k_points); - thrust::copy(utility::exec_policy(utility::GetStream(2))->on(utility::GetStream(2)), - range_colors.begin(), range_colors.end(), output->colors_.begin()); + thrust::strided_range< + utility::device_vector::const_iterator> + range_colors(colors_.begin(), colors_.end(), every_k_points); + thrust::copy(utility::exec_policy(utility::GetStream(2)) + ->on(utility::GetStream(2)), + range_colors.begin(), range_colors.end(), + output->colors_.begin()); } cudaSafeCall(cudaDeviceSynchronize()); return output; @@ -254,8 +306,10 @@ PointCloud::RemoveRadiusOutliers(size_t nb_points, float search_radius) const { kdtree.SearchRadius(points_, search_radius, tmp_indices, dist); const size_t n_pt = points_.size(); utility::device_vector indices(n_pt); - has_radius_points_functor func(thrust::raw_pointer_cast(tmp_indices.data()), nb_points, NUM_MAX_NN); - auto end = thrust::copy_if(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_pt), + has_radius_points_functor func(thrust::raw_pointer_cast(tmp_indices.data()), + nb_points, NUM_MAX_NN); + auto end = thrust::copy_if(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_pt), indices.begin(), func); indices.resize(thrust::distance(indices.begin(), end)); return std::make_tuple(SelectDownSample(indices), indices); @@ -281,26 +335,37 @@ PointCloud::RemoveStatisticalOutliers(size_t nb_neighbors, utility::device_vector tmp_indices; utility::device_vector dist; kdtree.SearchKNN(points_, int(nb_neighbors), tmp_indices, dist); - average_distance_functor avg_func(thrust::raw_pointer_cast(dist.data()), nb_neighbors); - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator((size_t)n_pt), + average_distance_functor avg_func(thrust::raw_pointer_cast(dist.data()), + nb_neighbors); + thrust::transform(thrust::make_counting_iterator(0), + thrust::make_counting_iterator((size_t)n_pt), avg_distances.begin(), avg_func); - const size_t valid_distances = thrust::count_if(avg_distances.begin(), avg_distances.end(), [] __device__ (float x) {return (x >= 0.0);}); + const size_t valid_distances = + thrust::count_if(avg_distances.begin(), avg_distances.end(), + [] __device__(float x) { return (x >= 0.0); }); if (valid_distances == 0) { return std::make_tuple(std::make_shared(), utility::device_vector()); } - float cloud_mean = thrust::reduce(avg_distances.begin(), avg_distances.end(), 0.0, - [] __device__ (float const &x, float const &y) { return (y > 0) ? x + y : x; }); + float cloud_mean = + thrust::reduce(avg_distances.begin(), avg_distances.end(), 0.0, + [] __device__(float const &x, float const &y) { + return (y > 0) ? x + y : x; + }); cloud_mean /= valid_distances; const float sq_sum = thrust::transform_reduce( avg_distances.begin(), avg_distances.end(), - [cloud_mean] __device__ (const float x) {return (x > 0) ? (x - cloud_mean) * (x - cloud_mean) : 0;}, + [cloud_mean] __device__(const float x) { + return (x > 0) ? (x - cloud_mean) * (x - cloud_mean) : 0; + }, 0.0, thrust::plus()); // Bessel's correction const float std_dev = std::sqrt(sq_sum / (valid_distances - 1)); const float distance_threshold = cloud_mean + std_ratio * std_dev; - check_distance_threshold_functor th_func(thrust::raw_pointer_cast(avg_distances.data()), distance_threshold); - auto end = thrust::copy_if(thrust::make_counting_iterator(0), thrust::make_counting_iterator((size_t)n_pt), + check_distance_threshold_functor th_func( + thrust::raw_pointer_cast(avg_distances.data()), distance_threshold); + auto end = thrust::copy_if(thrust::make_counting_iterator(0), + thrust::make_counting_iterator((size_t)n_pt), indices.begin(), th_func); indices.resize(thrust::distance(indices.begin(), end)); return std::make_tuple(SelectDownSample(indices), indices); diff --git a/src/cupoch/geometry/estimate_normals.cu b/src/cupoch/geometry/estimate_normals.cu index 1df1bc6f..a8fab76d 100644 --- a/src/cupoch/geometry/estimate_normals.cu +++ b/src/cupoch/geometry/estimate_normals.cu @@ -1,6 +1,7 @@ #include -#include "cupoch/geometry/pointcloud.h" + #include "cupoch/geometry/kdtree_flann.h" +#include "cupoch/geometry/pointcloud.h" #include "cupoch/utility/console.h" using namespace cupoch; @@ -8,8 +9,8 @@ using namespace cupoch::geometry; namespace { -__device__ -Eigen::Vector3f ComputeEigenvector0(const Eigen::Matrix3f &A, float eval0) { +__device__ Eigen::Vector3f ComputeEigenvector0(const Eigen::Matrix3f &A, + float eval0) { Eigen::Vector3f row0(A(0, 0) - eval0, A(0, 1), A(0, 2)); Eigen::Vector3f row1(A(0, 1), A(1, 1) - eval0, A(1, 2)); Eigen::Vector3f row2(A(0, 2), A(1, 2), A(2, 2) - eval0); @@ -38,11 +39,10 @@ Eigen::Vector3f ComputeEigenvector0(const Eigen::Matrix3f &A, float eval0) { return r1xr2 / std::sqrt(d2); } } - -__device__ -Eigen::Vector3f ComputeEigenvector1(const Eigen::Matrix3f &A, - const Eigen::Vector3f &evec0, - float eval1) { + +__device__ Eigen::Vector3f ComputeEigenvector1(const Eigen::Matrix3f &A, + const Eigen::Vector3f &evec0, + float eval1) { Eigen::Vector3f U, V; if (std::abs(evec0(0)) > std::abs(evec0(1))) { float inv_length = @@ -56,17 +56,17 @@ Eigen::Vector3f ComputeEigenvector1(const Eigen::Matrix3f &A, V = evec0.cross(U); Eigen::Vector3f AU(A(0, 0) * U(0) + A(0, 1) * U(1) + A(0, 2) * U(2), - A(0, 1) * U(0) + A(1, 1) * U(1) + A(1, 2) * U(2), - A(0, 2) * U(0) + A(1, 2) * U(1) + A(2, 2) * U(2)); + A(0, 1) * U(0) + A(1, 1) * U(1) + A(1, 2) * U(2), + A(0, 2) * U(0) + A(1, 2) * U(1) + A(2, 2) * U(2)); Eigen::Vector3f AV = {A(0, 0) * V(0) + A(0, 1) * V(1) + A(0, 2) * V(2), A(0, 1) * V(0) + A(1, 1) * V(1) + A(1, 2) * V(2), A(0, 2) * V(0) + A(1, 2) * V(1) + A(2, 2) * V(2)}; - + float m00 = U(0) * AU(0) + U(1) * AU(1) + U(2) * AU(2) - eval1; float m01 = U(0) * AV(0) + U(1) * AV(1) + U(2) * AV(2); float m11 = V(0) * AV(0) + V(1) * AV(1) + V(2) * AV(2) - eval1; - + float absM00 = std::abs(m00); float absM01 = std::abs(m01); float absM11 = std::abs(m11); @@ -106,8 +106,7 @@ Eigen::Vector3f ComputeEigenvector1(const Eigen::Matrix3f &A, } } -__device__ -Eigen::Vector3f FastEigen3x3(Eigen::Matrix3f &A) { +__device__ Eigen::Vector3f FastEigen3x3(Eigen::Matrix3f &A) { // Previous version based on: // https://en.wikipedia.org/wiki/Eigenvalue_algorithm#3.C3.973_matrices // Current version based on @@ -133,8 +132,7 @@ Eigen::Vector3f FastEigen3x3(Eigen::Matrix3f &A) { float b11 = A(1, 1) - q; float b22 = A(2, 2) - q; - float p = - std::sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2) / 6); + float p = std::sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2) / 6); float c00 = b11 * b22 - A(1, 2) * A(1, 2); float c01 = A(0, 1) * b22 - A(1, 2) * A(0, 2); @@ -193,9 +191,9 @@ Eigen::Vector3f FastEigen3x3(Eigen::Matrix3f &A) { } } -__device__ -Eigen::Vector3f ComputeNormal(const Eigen::Vector3f* points, - const KNNIndices &indices, int knn) { +__device__ Eigen::Vector3f ComputeNormal(const Eigen::Vector3f *points, + const KNNIndices &indices, + int knn) { if (indices[0] < 0) return Eigen::Vector3f(0.0, 0.0, 1.0); Eigen::Matrix3f covariance; @@ -204,7 +202,7 @@ Eigen::Vector3f ComputeNormal(const Eigen::Vector3f* points, int count = 0; for (size_t i = 0; i < knn; i++) { if (indices[i] < 0) break; - const Eigen::Vector3f& point = points[indices[i]]; + const Eigen::Vector3f &point = points[indices[i]]; cumulants(0) += point(0); cumulants(1) += point(1); cumulants(2) += point(2); @@ -232,14 +230,14 @@ Eigen::Vector3f ComputeNormal(const Eigen::Vector3f* points, } struct compute_normal_functor { - compute_normal_functor(const Eigen::Vector3f* points, - const int* indices, int knn) - : points_(points), indices_(indices), knn_(knn) {}; - const Eigen::Vector3f* points_; - const int* indices_; + compute_normal_functor(const Eigen::Vector3f *points, + const int *indices, + int knn) + : points_(points), indices_(indices), knn_(knn){}; + const Eigen::Vector3f *points_; + const int *indices_; const int knn_; - __device__ - Eigen::Vector3f operator()(const int& idx) const { + __device__ Eigen::Vector3f operator()(const int &idx) const { KNNIndices idxs = KNNIndices::Constant(-1); for (int k = 0; k < knn_; ++k) idxs[k] = indices_[idx * knn_ + k]; Eigen::Vector3f normal = ComputeNormal(points_, idxs, knn_); @@ -251,11 +249,11 @@ struct compute_normal_functor { }; struct align_normals_direction_functor { - align_normals_direction_functor(const Eigen::Vector3f& orientation_reference) - : orientation_reference_(orientation_reference) {}; + align_normals_direction_functor( + const Eigen::Vector3f &orientation_reference) + : orientation_reference_(orientation_reference){}; const Eigen::Vector3f orientation_reference_; - __device__ - void operator()(Eigen::Vector3f& normal) const { + __device__ void operator()(Eigen::Vector3f &normal) const { if (normal.norm() == 0.0) { normal = orientation_reference_; } else if (normal.dot(orientation_reference_) < 0.0) { @@ -264,7 +262,7 @@ struct align_normals_direction_functor { } }; -} +} // namespace bool PointCloud::EstimateNormals(const KDTreeSearchParam &search_param) { if (HasNormals() == false) { @@ -276,7 +274,7 @@ bool PointCloud::EstimateNormals(const KDTreeSearchParam &search_param) { utility::device_vector distance2; kdtree.Search(points_, search_param, indices, distance2); int knn; - switch(search_param.GetSearchType()) { + switch (search_param.GetSearchType()) { case KDTreeSearchParam::SearchType::Knn: knn = ((const KDTreeSearchParamKNN &)search_param).knn_; break; @@ -291,14 +289,15 @@ bool PointCloud::EstimateNormals(const KDTreeSearchParam &search_param) { return false; } compute_normal_functor func(thrust::raw_pointer_cast(points_.data()), - thrust::raw_pointer_cast(indices.data()), - knn); - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator((int)points_.size()), + thrust::raw_pointer_cast(indices.data()), knn); + thrust::transform(thrust::make_counting_iterator(0), + thrust::make_counting_iterator((int)points_.size()), normals_.begin(), func); return true; } -bool PointCloud::OrientNormalsToAlignWithDirection(const Eigen::Vector3f &orientation_reference) { +bool PointCloud::OrientNormalsToAlignWithDirection( + const Eigen::Vector3f &orientation_reference) { if (HasNormals() == false) { utility::LogWarning( "[OrientNormalsToAlignWithDirection] No normals in the " diff --git a/src/cupoch/geometry/geometry.h b/src/cupoch/geometry/geometry.h index 45abf852..e55acc25 100644 --- a/src/cupoch/geometry/geometry.h +++ b/src/cupoch/geometry/geometry.h @@ -37,15 +37,14 @@ class Geometry { }; public: - __host__ __device__ - ~Geometry() {} // non-virtual + __host__ __device__ ~Geometry() {} // non-virtual protected: Geometry(GeometryType type, int dimension) : geometry_type_(type), dimension_(dimension) {} public: - virtual Geometry& Clear() = 0; + virtual Geometry &Clear() = 0; virtual bool IsEmpty() const = 0; GeometryType GetGeometryType() const { return geometry_type_; } int Dimension() const { return dimension_; } diff --git a/src/cupoch/geometry/geometry2d.h b/src/cupoch/geometry/geometry2d.h index 830c0407..d960a3cd 100644 --- a/src/cupoch/geometry/geometry2d.h +++ b/src/cupoch/geometry/geometry2d.h @@ -1,8 +1,9 @@ #pragma once -#include "cupoch/geometry/geometry.h" #include +#include "cupoch/geometry/geometry.h" + namespace cupoch { namespace geometry { @@ -22,7 +23,7 @@ class Geometry2D : public Geometry { Geometry2D(GeometryType type) : Geometry(type, 2) {} public: - Geometry& Clear() override = 0; + Geometry &Clear() override = 0; bool IsEmpty() const override = 0; /// Returns min bounds for geometry coordinates. virtual Eigen::Vector2f GetMinBound() const = 0; @@ -30,5 +31,5 @@ class Geometry2D : public Geometry { virtual Eigen::Vector2f GetMaxBound() const = 0; }; -} -} +} // namespace geometry +} // namespace cupoch diff --git a/src/cupoch/geometry/geometry3d.cu b/src/cupoch/geometry/geometry3d.cu index 34b5d405..3938c1cb 100644 --- a/src/cupoch/geometry/geometry3d.cu +++ b/src/cupoch/geometry/geometry3d.cu @@ -1,5 +1,6 @@ -#include "cupoch/geometry/geometry3d.h" #include + +#include "cupoch/geometry/geometry3d.h" #include "cupoch/utility/console.h" using namespace cupoch; @@ -8,72 +9,87 @@ using namespace cupoch::geometry; namespace { struct elementwise_min_functor { - __device__ - Eigen::Vector3f operator()(const Eigen::Vector3f& a, const Eigen::Vector3f& b) { + __device__ Eigen::Vector3f operator()(const Eigen::Vector3f &a, + const Eigen::Vector3f &b) { return a.array().min(b.array()).matrix(); } }; - + struct elementwise_max_functor { - __device__ - Eigen::Vector3f operator()(const Eigen::Vector3f& a, const Eigen::Vector3f& b) { + __device__ Eigen::Vector3f operator()(const Eigen::Vector3f &a, + const Eigen::Vector3f &b) { return a.array().max(b.array()).matrix(); } }; struct transform_points_functor { - transform_points_functor(const Eigen::Matrix4f& transform) : transform_(transform){}; + transform_points_functor(const Eigen::Matrix4f &transform) + : transform_(transform){}; const Eigen::Matrix4f transform_; - __device__ - void operator()(Eigen::Vector3f& pt) { - const Eigen::Vector4f new_pt = transform_ * Eigen::Vector4f(pt(0), pt(1), pt(2), 1.0); + __device__ void operator()(Eigen::Vector3f &pt) { + const Eigen::Vector4f new_pt = + transform_ * Eigen::Vector4f(pt(0), pt(1), pt(2), 1.0); pt = new_pt.head<3>() / new_pt(3); } }; struct transform_normals_functor { - transform_normals_functor(const Eigen::Matrix4f& transform) : transform_(transform){}; + transform_normals_functor(const Eigen::Matrix4f &transform) + : transform_(transform){}; const Eigen::Matrix4f transform_; - __device__ - void operator()(Eigen::Vector3f& nl) { - const Eigen::Vector4f new_pt = transform_ * Eigen::Vector4f(nl(0), nl(1), nl(2), 0.0); + __device__ void operator()(Eigen::Vector3f &nl) { + const Eigen::Vector4f new_pt = + transform_ * Eigen::Vector4f(nl(0), nl(1), nl(2), 0.0); nl = new_pt.head<3>(); } }; -} +} // namespace Geometry3D::Geometry3D(GeometryType type) : Geometry(type, 3) {} -Eigen::Vector3f Geometry3D::ComputeMinBound(const utility::device_vector& points) const { +Eigen::Vector3f Geometry3D::ComputeMinBound( + const utility::device_vector &points) const { return ComputeMinBound(0, points); } -Eigen::Vector3f Geometry3D::ComputeMinBound(cudaStream_t stream, const utility::device_vector& points) const { +Eigen::Vector3f Geometry3D::ComputeMinBound( + cudaStream_t stream, + const utility::device_vector &points) const { if (points.empty()) return Eigen::Vector3f::Zero(); Eigen::Vector3f init = points[0]; - return thrust::reduce(utility::exec_policy(stream)->on(stream), points.begin(), points.end(), init, elementwise_min_functor()); + return thrust::reduce(utility::exec_policy(stream)->on(stream), + points.begin(), points.end(), init, + elementwise_min_functor()); } -Eigen::Vector3f Geometry3D::ComputeMaxBound(const utility::device_vector& points) const { +Eigen::Vector3f Geometry3D::ComputeMaxBound( + const utility::device_vector &points) const { return ComputeMaxBound(0, points); } -Eigen::Vector3f Geometry3D::ComputeMaxBound(cudaStream_t stream, const utility::device_vector& points) const { +Eigen::Vector3f Geometry3D::ComputeMaxBound( + cudaStream_t stream, + const utility::device_vector &points) const { if (points.empty()) return Eigen::Vector3f::Zero(); Eigen::Vector3f init = points[0]; - return thrust::reduce(utility::exec_policy(stream)->on(stream), points.begin(), points.end(), init, elementwise_max_functor()); + return thrust::reduce(utility::exec_policy(stream)->on(stream), + points.begin(), points.end(), init, + elementwise_max_functor()); } -Eigen::Vector3f Geometry3D::ComputeCenter(const utility::device_vector& points) const { +Eigen::Vector3f Geometry3D::ComputeCenter( + const utility::device_vector &points) const { Eigen::Vector3f init = Eigen::Vector3f::Zero(); if (points.empty()) return init; - Eigen::Vector3f sum = thrust::reduce(points.begin(), points.end(), init, thrust::plus()); + Eigen::Vector3f sum = thrust::reduce(points.begin(), points.end(), init, + thrust::plus()); return sum / points.size(); } -void Geometry3D::ResizeAndPaintUniformColor(utility::device_vector& colors, - const size_t size, - const Eigen::Vector3f& color) { +void Geometry3D::ResizeAndPaintUniformColor( + utility::device_vector &colors, + const size_t size, + const Eigen::Vector3f &color) { colors.resize(size); Eigen::Vector3f clipped_color = color; if (color.minCoeff() < 0 || color.maxCoeff() > 1) { @@ -89,128 +105,147 @@ void Geometry3D::ResizeAndPaintUniformColor(utility::device_vector& points) { +void Geometry3D::TransformPoints( + const Eigen::Matrix4f &transformation, + utility::device_vector &points) { TransformPoints(0, transformation, points); } -void Geometry3D::TransformPoints(cudaStream_t stream, const Eigen::Matrix4f& transformation, - utility::device_vector& points) { +void Geometry3D::TransformPoints( + cudaStream_t stream, + const Eigen::Matrix4f &transformation, + utility::device_vector &points) { transform_points_functor func(transformation); - thrust::for_each(utility::exec_policy(stream)->on(stream), points.begin(), points.end(), func); + thrust::for_each(utility::exec_policy(stream)->on(stream), points.begin(), + points.end(), func); } -void Geometry3D::TransformNormals(const Eigen::Matrix4f& transformation, - utility::device_vector& normals) { +void Geometry3D::TransformNormals( + const Eigen::Matrix4f &transformation, + utility::device_vector &normals) { TransformNormals(0, transformation, normals); } -void Geometry3D::TransformNormals(cudaStream_t stream, const Eigen::Matrix4f& transformation, - utility::device_vector& normals) { +void Geometry3D::TransformNormals( + cudaStream_t stream, + const Eigen::Matrix4f &transformation, + utility::device_vector &normals) { transform_normals_functor func(transformation); - thrust::for_each(utility::exec_policy(stream)->on(stream), normals.begin(), normals.end(), func); + thrust::for_each(utility::exec_policy(stream)->on(stream), normals.begin(), + normals.end(), func); } -void Geometry3D::TranslatePoints(const Eigen::Vector3f& translation, - utility::device_vector& points, - bool relative) const { +void Geometry3D::TranslatePoints( + const Eigen::Vector3f &translation, + utility::device_vector &points, + bool relative) const { Eigen::Vector3f transform = translation; if (!relative) { transform -= ComputeCenter(points); } - thrust::for_each(points.begin(), points.end(), [=] __device__ (Eigen::Vector3f& pt) {pt += transform;}); + thrust::for_each(points.begin(), points.end(), + [=] __device__(Eigen::Vector3f & pt) { pt += transform; }); } void Geometry3D::ScalePoints(const float scale, - utility::device_vector& points, + utility::device_vector &points, bool center) const { Eigen::Vector3f points_center(0, 0, 0); if (center && !points.empty()) { points_center = ComputeCenter(points); } thrust::for_each(points.begin(), points.end(), - [=] __device__ (Eigen::Vector3f& pt) {pt = (pt - points_center) * scale + points_center;}); + [=] __device__(Eigen::Vector3f & pt) { + pt = (pt - points_center) * scale + points_center; + }); } -void Geometry3D::RotatePoints(const Eigen::Matrix3f& R, - utility::device_vector& points, +void Geometry3D::RotatePoints(const Eigen::Matrix3f &R, + utility::device_vector &points, bool center) const { RotatePoints(0, R, points, center); } -void Geometry3D::RotatePoints(cudaStream_t stream, const Eigen::Matrix3f& R, - utility::device_vector& points, +void Geometry3D::RotatePoints(cudaStream_t stream, + const Eigen::Matrix3f &R, + utility::device_vector &points, bool center) const { Eigen::Vector3f points_center(0, 0, 0); if (center && !points.empty()) { points_center = ComputeCenter(points); } - thrust::for_each(utility::exec_policy(stream)->on(stream), points.begin(), points.end(), - [=] __device__ (Eigen::Vector3f& pt) {pt = R * (pt - points_center) + points_center;}); + thrust::for_each(utility::exec_policy(stream)->on(stream), points.begin(), + points.end(), [=] __device__(Eigen::Vector3f & pt) { + pt = R * (pt - points_center) + points_center; + }); } - -void Geometry3D::RotateNormals(const Eigen::Matrix3f& R, - utility::device_vector& normals) const { +void Geometry3D::RotateNormals( + const Eigen::Matrix3f &R, + utility::device_vector &normals) const { RotateNormals(0, R, normals); } -void Geometry3D::RotateNormals(cudaStream_t stream, const Eigen::Matrix3f& R, - utility::device_vector& normals) const { - thrust::for_each(utility::exec_policy(stream)->on(stream), normals.begin(), normals.end(), - [=] __device__ (Eigen::Vector3f& normal) {normal = R * normal;}); +void Geometry3D::RotateNormals( + cudaStream_t stream, + const Eigen::Matrix3f &R, + utility::device_vector &normals) const { + thrust::for_each(utility::exec_policy(stream)->on(stream), normals.begin(), + normals.end(), [=] __device__(Eigen::Vector3f & normal) { + normal = R * normal; + }); } Eigen::Matrix3f Geometry3D::GetRotationMatrixFromXYZ( - const Eigen::Vector3f& rotation) { + const Eigen::Vector3f &rotation) { return cupoch::utility::RotationMatrixX(rotation(0)) * cupoch::utility::RotationMatrixY(rotation(1)) * cupoch::utility::RotationMatrixZ(rotation(2)); } Eigen::Matrix3f Geometry3D::GetRotationMatrixFromYZX( - const Eigen::Vector3f& rotation) { + const Eigen::Vector3f &rotation) { return cupoch::utility::RotationMatrixY(rotation(0)) * cupoch::utility::RotationMatrixZ(rotation(1)) * cupoch::utility::RotationMatrixX(rotation(2)); } Eigen::Matrix3f Geometry3D::GetRotationMatrixFromZXY( - const Eigen::Vector3f& rotation) { + const Eigen::Vector3f &rotation) { return cupoch::utility::RotationMatrixZ(rotation(0)) * cupoch::utility::RotationMatrixX(rotation(1)) * cupoch::utility::RotationMatrixY(rotation(2)); } Eigen::Matrix3f Geometry3D::GetRotationMatrixFromXZY( - const Eigen::Vector3f& rotation) { + const Eigen::Vector3f &rotation) { return cupoch::utility::RotationMatrixX(rotation(0)) * cupoch::utility::RotationMatrixZ(rotation(1)) * cupoch::utility::RotationMatrixY(rotation(2)); } Eigen::Matrix3f Geometry3D::GetRotationMatrixFromZYX( - const Eigen::Vector3f& rotation) { + const Eigen::Vector3f &rotation) { return cupoch::utility::RotationMatrixZ(rotation(0)) * cupoch::utility::RotationMatrixY(rotation(1)) * cupoch::utility::RotationMatrixX(rotation(2)); } Eigen::Matrix3f Geometry3D::GetRotationMatrixFromYXZ( - const Eigen::Vector3f& rotation) { + const Eigen::Vector3f &rotation) { return cupoch::utility::RotationMatrixY(rotation(0)) * cupoch::utility::RotationMatrixX(rotation(1)) * cupoch::utility::RotationMatrixZ(rotation(2)); } Eigen::Matrix3f Geometry3D::GetRotationMatrixFromAxisAngle( - const Eigen::Vector3f& rotation) { + const Eigen::Vector3f &rotation) { const float phi = rotation.norm(); return Eigen::AngleAxisf(phi, rotation / phi).toRotationMatrix(); } Eigen::Matrix3f Geometry3D::GetRotationMatrixFromQuaternion( - const Eigen::Vector4f& rotation) { + const Eigen::Vector4f &rotation) { return Eigen::Quaternionf(rotation(0), rotation(1), rotation(2), rotation(3)) .normalized() diff --git a/src/cupoch/geometry/geometry3d.h b/src/cupoch/geometry/geometry3d.h index d1ed6d2b..9befdf9c 100644 --- a/src/cupoch/geometry/geometry3d.h +++ b/src/cupoch/geometry/geometry3d.h @@ -1,7 +1,7 @@ #pragma once #include "cupoch/geometry/geometry.h" -#include "cupoch/utility/eigen.h" #include "cupoch/utility/device_vector.h" +#include "cupoch/utility/eigen.h" namespace cupoch { namespace geometry { @@ -11,8 +11,7 @@ class OrientedBoundingBox; class Geometry3D : public Geometry { public: - __host__ __device__ - ~Geometry3D() {}; // non-virtual + __host__ __device__ ~Geometry3D(){}; // non-virtual protected: /// \brief Parameterized Constructor. @@ -21,7 +20,7 @@ class Geometry3D : public Geometry { Geometry3D(GeometryType type); public: - Geometry3D& Clear() override = 0; + Geometry3D &Clear() override = 0; bool IsEmpty() const override = 0; /// Returns min bounds for geometry coordinates. virtual Eigen::Vector3f GetMinBound() const = 0; @@ -32,13 +31,13 @@ class Geometry3D : public Geometry { /// Returns an axis-aligned bounding box of the geometry. virtual AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const = 0; /// \brief Apply transformation (4x4 matrix) to the geometry coordinates. - virtual Geometry3D& Transform(const Eigen::Matrix4f& transformation) = 0; + virtual Geometry3D &Transform(const Eigen::Matrix4f &transformation) = 0; /// \brief Apply translation to the geometry coordinates. /// /// \param translation A 3D vector to transform the geometry. /// \param relative If `true`, the \p translation is directly applied to the /// geometry. Otherwise, the geometry center is moved to the \p translation. - virtual Geometry3D& Translate(const Eigen::Vector3f& translation, + virtual Geometry3D &Translate(const Eigen::Vector3f &translation, bool relative = true) = 0; /// \brief Apply scaling to the geometry coordinates. /// @@ -47,7 +46,7 @@ class Geometry3D : public Geometry { /// \param center If `true`, the scale is applied relative to the center of /// the geometry. Otherwise, the scale is directly applied to the geometry, /// i.e. relative to the origin. - virtual Geometry3D& Scale(const float scale, bool center = true) = 0; + virtual Geometry3D &Scale(const float scale, bool center = true) = 0; /// \brief Apply rotation to the geometry coordinates and normals. /// /// \param R A 3D vector that either defines the three angles for Euler @@ -56,63 +55,73 @@ class Geometry3D : public Geometry { /// \param center If `true`, the rotation is applied relative to the center /// of the geometry. Otherwise, the rotation is directly applied to the /// geometry, i.e. relative to the origin. - virtual Geometry3D& Rotate(const Eigen::Matrix3f& R, + virtual Geometry3D &Rotate(const Eigen::Matrix3f &R, bool center = true) = 0; /// Get Rotation Matrix from XYZ RotationType. static Eigen::Matrix3f GetRotationMatrixFromXYZ( - const Eigen::Vector3f& rotation); + const Eigen::Vector3f &rotation); /// Get Rotation Matrix from YZX RotationType. static Eigen::Matrix3f GetRotationMatrixFromYZX( - const Eigen::Vector3f& rotation); + const Eigen::Vector3f &rotation); /// Get Rotation Matrix from ZXY RotationType. static Eigen::Matrix3f GetRotationMatrixFromZXY( - const Eigen::Vector3f& rotation); + const Eigen::Vector3f &rotation); /// Get Rotation Matrix from XZY RotationType. static Eigen::Matrix3f GetRotationMatrixFromXZY( - const Eigen::Vector3f& rotation); + const Eigen::Vector3f &rotation); /// Get Rotation Matrix from ZYX RotationType. static Eigen::Matrix3f GetRotationMatrixFromZYX( - const Eigen::Vector3f& rotation); + const Eigen::Vector3f &rotation); /// Get Rotation Matrix from YXZ RotationType. static Eigen::Matrix3f GetRotationMatrixFromYXZ( - const Eigen::Vector3f& rotation); + const Eigen::Vector3f &rotation); /// Get Rotation Matrix from AxisAngle RotationType. static Eigen::Matrix3f GetRotationMatrixFromAxisAngle( - const Eigen::Vector3f& rotation); + const Eigen::Vector3f &rotation); /// Get Rotation Matrix from Quaternion. static Eigen::Matrix3f GetRotationMatrixFromQuaternion( - const Eigen::Vector4f& rotation); + const Eigen::Vector4f &rotation); public: - Eigen::Vector3f ComputeMinBound(const utility::device_vector& points) const; - Eigen::Vector3f ComputeMinBound(cudaStream_t stream, const utility::device_vector& points) const; + Eigen::Vector3f ComputeMinBound( + const utility::device_vector &points) const; + Eigen::Vector3f ComputeMinBound( + cudaStream_t stream, + const utility::device_vector &points) const; - Eigen::Vector3f ComputeMaxBound(const utility::device_vector& points) const; - Eigen::Vector3f ComputeMaxBound(cudaStream_t stream, const utility::device_vector& points) const; + Eigen::Vector3f ComputeMaxBound( + const utility::device_vector &points) const; + Eigen::Vector3f ComputeMaxBound( + cudaStream_t stream, + const utility::device_vector &points) const; - Eigen::Vector3f ComputeCenter(const utility::device_vector& points) const; + Eigen::Vector3f ComputeCenter( + const utility::device_vector &points) const; - void ResizeAndPaintUniformColor(utility::device_vector& colors, - const size_t size, - const Eigen::Vector3f& color); + void ResizeAndPaintUniformColor( + utility::device_vector &colors, + const size_t size, + const Eigen::Vector3f &color); /// \brief Transforms all points with the transformation matrix. /// /// \param transformation 4x4 matrix for transformation. /// \param points A list of points to be transformed. - void TransformPoints(const Eigen::Matrix4f& transformation, - utility::device_vector& points); - void TransformPoints(cudaStream_t stream, const Eigen::Matrix4f& transformation, - utility::device_vector& points); + void TransformPoints(const Eigen::Matrix4f &transformation, + utility::device_vector &points); + void TransformPoints(cudaStream_t stream, + const Eigen::Matrix4f &transformation, + utility::device_vector &points); /// \brief Transforms the normals with the transformation matrix. /// /// \param transformation 4x4 matrix for transformation. /// \param normals A list of normals to be transformed. - void TransformNormals(const Eigen::Matrix4f& transformation, - utility::device_vector& normals); - void TransformNormals(cudaStream_t stream, const Eigen::Matrix4f& transformation, - utility::device_vector& normals); + void TransformNormals(const Eigen::Matrix4f &transformation, + utility::device_vector &normals); + void TransformNormals(cudaStream_t stream, + const Eigen::Matrix4f &transformation, + utility::device_vector &normals); /// \brief Apply translation to the geometry coordinates. /// /// \param translation A 3D vector to transform the geometry. @@ -120,8 +129,8 @@ class Geometry3D : public Geometry { /// \param relative If `true`, the \p translation is directly applied to the /// \points. Otherwise, the center of the \points is moved to the \p /// translation. - void TranslatePoints(const Eigen::Vector3f& translation, - utility::device_vector& points, + void TranslatePoints(const Eigen::Vector3f &translation, + utility::device_vector &points, bool relative) const; /// \brief Scale the coordinates of all points by the scaling factor \p /// scale. @@ -132,7 +141,7 @@ class Geometry3D : public Geometry { /// transformed. \param center If `true`, then the scale is applied to the /// centered geometry. void ScalePoints(const float scale, - utility::device_vector& points, + utility::device_vector &points, bool center) const; /// \brief Rotate all points with the rotation matrix \p R. /// @@ -143,11 +152,12 @@ class Geometry3D : public Geometry { /// \param center If `true`, the rotation is applied relative to the center /// of the geometry. Otherwise, the rotation is directly applied to the /// geometry, i.e. relative to the origin. - void RotatePoints(const Eigen::Matrix3f& R, - utility::device_vector& points, + void RotatePoints(const Eigen::Matrix3f &R, + utility::device_vector &points, bool center) const; - void RotatePoints(cudaStream_t stream, const Eigen::Matrix3f& R, - utility::device_vector& points, + void RotatePoints(cudaStream_t stream, + const Eigen::Matrix3f &R, + utility::device_vector &points, bool center) const; /// \brief Rotate all normals with the rotation matrix \p R. /// @@ -155,11 +165,12 @@ class Geometry3D : public Geometry { /// rotation, or in the axis-angle representation the normalized vector /// defines the axis of rotation and the norm the angle around this axis. /// \param normals A list of normals to be transformed. - void RotateNormals(const Eigen::Matrix3f& R, - utility::device_vector& normals) const; - void RotateNormals(cudaStream_t stream, const Eigen::Matrix3f& R, - utility::device_vector& normals) const; + void RotateNormals(const Eigen::Matrix3f &R, + utility::device_vector &normals) const; + void RotateNormals(cudaStream_t stream, + const Eigen::Matrix3f &R, + utility::device_vector &normals) const; }; -} -} \ No newline at end of file +} // namespace geometry +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/geometry/image.cu b/src/cupoch/geometry/image.cu index c289363f..eb89d2dd 100644 --- a/src/cupoch/geometry/image.cu +++ b/src/cupoch/geometry/image.cu @@ -8,18 +8,17 @@ namespace { /// Isotropic 2D kernels are separable: /// two 1D kernels are applied in x and y direction. -std::pair, utility::device_vector> GetFilterKernel(Image::FilterType ftype) { +std::pair, utility::device_vector> +GetFilterKernel(Image::FilterType ftype) { switch (ftype) { - case Image::FilterType::Gaussian3: - { + case Image::FilterType::Gaussian3: { utility::device_vector g3(3); g3[0] = 0.25; g3[1] = 0.5; g3[2] = 0.25; return std::make_pair(g3, g3); } - case Image::FilterType::Gaussian5: - { + case Image::FilterType::Gaussian5: { utility::device_vector g5(5); g5[0] = 0.0625; g5[1] = 0.25; @@ -28,8 +27,7 @@ std::pair, utility::device_vector> GetFilte g5[4] = 0.0625; return std::make_pair(g5, g5); } - case Image::FilterType::Gaussian7: - { + case Image::FilterType::Gaussian7: { utility::device_vector g7(7); g7[0] = 0.03125; g7[1] = 0.109375; @@ -40,8 +38,7 @@ std::pair, utility::device_vector> GetFilte g7[6] = 0.03125; return std::make_pair(g7, g7); } - case Image::FilterType::Sobel3Dx: - { + case Image::FilterType::Sobel3Dx: { utility::device_vector s31(3); utility::device_vector s32(3); s31[0] = -1.0; @@ -52,8 +49,7 @@ std::pair, utility::device_vector> GetFilte s32[2] = 1.0; return std::make_pair(s31, s32); } - case Image::FilterType::Sobel3Dy: - { + case Image::FilterType::Sobel3Dy: { utility::device_vector s31(3); utility::device_vector s32(3); s31[0] = -1.0; @@ -64,30 +60,34 @@ std::pair, utility::device_vector> GetFilte s32[2] = 1.0; return std::make_pair(s32, s31); } - default: - { + default: { utility::LogError("[Filter] Unsupported filter type."); - return std::make_pair(utility::device_vector(), utility::device_vector()); + return std::make_pair(utility::device_vector(), + utility::device_vector()); } } } struct transpose_functor { - transpose_functor(const uint8_t* src, int width, - int in_bytes_per_line, int out_bytes_per_line, - int bytes_per_pixel, uint8_t* dst) - : src_(src), width_(width), - in_bytes_per_line_(in_bytes_per_line), - out_bytes_per_line_(out_bytes_per_line), - bytes_per_pixel_(bytes_per_pixel), dst_(dst) {}; - const uint8_t* src_; + transpose_functor(const uint8_t *src, + int width, + int in_bytes_per_line, + int out_bytes_per_line, + int bytes_per_pixel, + uint8_t *dst) + : src_(src), + width_(width), + in_bytes_per_line_(in_bytes_per_line), + out_bytes_per_line_(out_bytes_per_line), + bytes_per_pixel_(bytes_per_pixel), + dst_(dst){}; + const uint8_t *src_; const int width_; const int in_bytes_per_line_; const int out_bytes_per_line_; const int bytes_per_pixel_; - uint8_t* dst_; - __device__ - void operator() (size_t idx) { + uint8_t *dst_; + __device__ void operator()(size_t idx) { const int y = idx / width_; const int x = idx % width_; memcpy(dst_ + x * out_bytes_per_line_ + y * bytes_per_pixel_, @@ -97,76 +97,83 @@ struct transpose_functor { }; struct clip_intensity_functor { - clip_intensity_functor(uint8_t* fimage, float min, float max) - : fimage_(fimage), min_(min), max_(max) {}; - uint8_t* fimage_; + clip_intensity_functor(uint8_t *fimage, float min, float max) + : fimage_(fimage), min_(min), max_(max){}; + uint8_t *fimage_; const float min_; const float max_; - __device__ - void operator() (size_t idx) { - float *p = (float*)(fimage_ + idx * sizeof(float)); + __device__ void operator()(size_t idx) { + float *p = (float *)(fimage_ + idx * sizeof(float)); if (*p > max_) *p = (float)max_; if (*p < min_) *p = (float)min_; } }; struct linear_transform_functor { - linear_transform_functor(uint8_t* fimage, float scale, float offset) - : fimage_(fimage), scale_(scale), offset_(offset) {}; - uint8_t* fimage_; + linear_transform_functor(uint8_t *fimage, float scale, float offset) + : fimage_(fimage), scale_(scale), offset_(offset){}; + uint8_t *fimage_; const float scale_; const float offset_; - __device__ - void operator() (size_t idx) { - float *p = (float*)(fimage_ + idx * sizeof(float)); + __device__ void operator()(size_t idx) { + float *p = (float *)(fimage_ + idx * sizeof(float)); (*p) = (float)(scale_ * (*p) + offset_); } }; struct downsample_functor { - downsample_functor(const uint8_t* src, int src_width, - uint8_t* dst, int dst_width) - : src_(src), src_width_(src_width), dst_(dst), dst_width_(dst_width) {}; - const uint8_t* src_; + downsample_functor(const uint8_t *src, + int src_width, + uint8_t *dst, + int dst_width) + : src_(src), src_width_(src_width), dst_(dst), dst_width_(dst_width){}; + const uint8_t *src_; const int src_width_; - uint8_t* dst_; + uint8_t *dst_; const int dst_width_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { const int y = idx / dst_width_; const int x = idx % dst_width_; - float *p1 = (float*)(src_ + (y * 2 * src_width_ + x * 2) * sizeof(float)); - float *p2 = (float*)(src_ + (y * 2 * src_width_ + x * 2 + 1) * sizeof(float)); - float *p3 = (float*)(src_ + ((y * 2 + 1) * src_width_ + x * 2) * sizeof(float)); - float *p4 = (float*)(src_ + ((y * 2 + 1) * src_width_ + x * 2 + 1) * sizeof(float)); - float *p = (float*)(dst_ + idx * sizeof(float)); + float *p1 = + (float *)(src_ + (y * 2 * src_width_ + x * 2) * sizeof(float)); + float *p2 = (float *)(src_ + + (y * 2 * src_width_ + x * 2 + 1) * sizeof(float)); + float *p3 = (float *)(src_ + ((y * 2 + 1) * src_width_ + x * 2) * + sizeof(float)); + float *p4 = (float *)(src_ + ((y * 2 + 1) * src_width_ + x * 2 + 1) * + sizeof(float)); + float *p = (float *)(dst_ + idx * sizeof(float)); *p = (*p1 + *p2 + *p3 + *p4) / 4.0f; } }; struct filter_horizontal_functor { - filter_horizontal_functor(const uint8_t* src, int width, - const float* kernel, + filter_horizontal_functor(const uint8_t *src, + int width, + const float *kernel, int half_kernel_size, - uint8_t* dst) - : src_(src), width_(width), kernel_(kernel), - half_kernel_size_(half_kernel_size), dst_(dst) {}; - const uint8_t* src_; + uint8_t *dst) + : src_(src), + width_(width), + kernel_(kernel), + half_kernel_size_(half_kernel_size), + dst_(dst){}; + const uint8_t *src_; const int width_; - const float* kernel_; + const float *kernel_; const int half_kernel_size_; - uint8_t* dst_; - __device__ - void operator() (size_t idx) { + uint8_t *dst_; + __device__ void operator()(size_t idx) { const int y = idx / width_; const int x = idx % width_; - float *po = (float*)(dst_ + idx * sizeof(float)); + float *po = (float *)(dst_ + idx * sizeof(float)); float temp = 0; for (int i = -half_kernel_size_; i <= half_kernel_size_; i++) { int x_shift = x + i; if (x_shift < 0) x_shift = 0; if (x_shift > width_ - 1) x_shift = width_ - 1; - float *pi = (float*)(src_ + (y * width_ + x_shift) * sizeof(float)); + float *pi = + (float *)(src_ + (y * width_ + x_shift) * sizeof(float)); temp += (*pi * kernel_[i + half_kernel_size_]); } *po = temp; @@ -174,54 +181,66 @@ struct filter_horizontal_functor { }; struct vertical_flip_functor { - vertical_flip_functor(const uint8_t* src, int width, int height, - int bytes_per_pixel, uint8_t* dst) - : src_(src), width_(width), height_(height), bytes_per_pixel_(bytes_per_pixel), dst_(dst) {}; - const uint8_t* src_; + vertical_flip_functor(const uint8_t *src, + int width, + int height, + int bytes_per_pixel, + uint8_t *dst) + : src_(src), + width_(width), + height_(height), + bytes_per_pixel_(bytes_per_pixel), + dst_(dst){}; + const uint8_t *src_; const int width_; const int height_; const int bytes_per_pixel_; - uint8_t* dst_; - __device__ - void operator() (size_t idx) { + uint8_t *dst_; + __device__ void operator()(size_t idx) { const int y = idx / width_; const int x = idx % width_; - memcpy(&dst_[(height_ - y - 1) * width_ * bytes_per_pixel_ + x], &src_[idx], bytes_per_pixel_ * sizeof(uint8_t)); + memcpy(&dst_[(height_ - y - 1) * width_ * bytes_per_pixel_ + x], + &src_[idx], bytes_per_pixel_ * sizeof(uint8_t)); } }; struct horizontal_flip_functor { - horizontal_flip_functor(const uint8_t* src, int width, - int bytes_per_pixel, uint8_t* dst) - : src_(src), width_(width), bytes_per_pixel_(bytes_per_pixel), dst_(dst) {}; - const uint8_t* src_; + horizontal_flip_functor(const uint8_t *src, + int width, + int bytes_per_pixel, + uint8_t *dst) + : src_(src), + width_(width), + bytes_per_pixel_(bytes_per_pixel), + dst_(dst){}; + const uint8_t *src_; const int width_; const int bytes_per_pixel_; - uint8_t* dst_; - __device__ - void operator() (size_t idx) { + uint8_t *dst_; + __device__ void operator()(size_t idx) { const int y = idx / width_; const int x = idx % width_; - memcpy(&dst_[y * width_ * bytes_per_pixel_ + (width_ - x - 1)], &src_[idx], bytes_per_pixel_ * sizeof(uint8_t)); + memcpy(&dst_[y * width_ * bytes_per_pixel_ + (width_ - x - 1)], + &src_[idx], bytes_per_pixel_ * sizeof(uint8_t)); } }; struct depth_to_float_functor { - depth_to_float_functor(int depth_scale, int depth_trunc, uint8_t* fimage) - : depth_scale_(depth_scale), depth_trunc_(depth_trunc), fimage_(fimage) {}; + depth_to_float_functor(int depth_scale, int depth_trunc, uint8_t *fimage) + : depth_scale_(depth_scale), + depth_trunc_(depth_trunc), + fimage_(fimage){}; const int depth_scale_; const int depth_trunc_; - uint8_t* fimage_; - __device__ - void operator() (size_t idx) { - float *p = (float*)(fimage_ + idx * sizeof(float)); + uint8_t *fimage_; + __device__ void operator()(size_t idx) { + float *p = (float *)(fimage_ + idx * sizeof(float)); *p /= (float)depth_scale_; if (*p >= depth_trunc_) *p = 0.0f; } }; -} - +} // namespace Image::Image() : Geometry2D(Geometry::GeometryType::Image) {} Image::~Image() {} @@ -235,13 +254,9 @@ Image &Image::Clear() { return *this; } -bool Image::IsEmpty() const { - return !HasData(); -} +bool Image::IsEmpty() const { return !HasData(); } -Eigen::Vector2f Image::GetMinBound() const { - return Eigen::Vector2f(0.0, 0.0); -} +Eigen::Vector2f Image::GetMinBound() const { return Eigen::Vector2f(0.0, 0.0); } Eigen::Vector2f Image::GetMaxBound() const { return Eigen::Vector2f(width_, height_); @@ -252,9 +267,7 @@ thrust::host_vector Image::GetData() const { return data; } -void Image::SetData(const thrust::host_vector& data) { - data_ = data; -} +void Image::SetData(const thrust::host_vector &data) { data_ = data; } bool Image::TestImageBoundary(float u, float v, @@ -277,8 +290,8 @@ std::shared_ptr Image::ConvertDepthToFloatImage( auto output = CreateFloatImage(); depth_to_float_functor func(depth_scale, depth_trunc, thrust::raw_pointer_cast(output->data_.data())); - for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(width_ * height_), - func); + for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(width_ * height_), func); return output; } @@ -286,8 +299,11 @@ Image &Image::ClipIntensity(float min /* = 0.0*/, float max /* = 1.0*/) { if (num_of_channels_ != 1 || bytes_per_channel_ != 4) { utility::LogError("[ClipIntensity] Unsupported image format."); } - clip_intensity_functor func(thrust::raw_pointer_cast(data_.data()), min, max); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(width_ * height_), func); + clip_intensity_functor func(thrust::raw_pointer_cast(data_.data()), min, + max); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(width_ * height_), + func); return *this; } @@ -295,8 +311,11 @@ Image &Image::LinearTransform(float scale, float offset /* = 0.0*/) { if (num_of_channels_ != 1 || bytes_per_channel_ != 4) { utility::LogError("[LinearTransform] Unsupported image format."); } - linear_transform_functor func(thrust::raw_pointer_cast(data_.data()), scale, offset); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(width_ * height_), func); + linear_transform_functor func(thrust::raw_pointer_cast(data_.data()), scale, + offset); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(width_ * height_), + func); return *this; } @@ -310,9 +329,12 @@ std::shared_ptr Image::Downsample() const { output->Prepare(half_width, half_height, 1, 4); downsample_functor func(thrust::raw_pointer_cast(data_.data()), width_, - thrust::raw_pointer_cast(output->data_.data()), output->width_); + thrust::raw_pointer_cast(output->data_.data()), + output->width_); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(output->width_ * output->height_), func); + thrust::make_counting_iterator(output->width_ * + output->height_), + func); return output; } @@ -329,11 +351,13 @@ std::shared_ptr Image::FilterHorizontal( const int half_kernel_size = (int)(floor((float)kernel.size() / 2.0)); - filter_horizontal_functor func(thrust::raw_pointer_cast(data_.data()), width_, - thrust::raw_pointer_cast(kernel.data()), - half_kernel_size, - thrust::raw_pointer_cast(output->data_.data())); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(width_ * height_), func); + filter_horizontal_functor func( + thrust::raw_pointer_cast(data_.data()), width_, + thrust::raw_pointer_cast(kernel.data()), half_kernel_size, + thrust::raw_pointer_cast(output->data_.data())); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(width_ * height_), + func); return output; } @@ -358,8 +382,9 @@ ImagePyramid Image::FilterPyramid(const ImagePyramid &input, return output; } -std::shared_ptr Image::Filter(const utility::device_vector &dx, - const utility::device_vector &dy) const { +std::shared_ptr Image::Filter( + const utility::device_vector &dx, + const utility::device_vector &dy) const { auto output = std::make_shared(); if (num_of_channels_ != 1 || bytes_per_channel_ != 4) { utility::LogError("[Filter] Unsupported image format."); @@ -380,11 +405,13 @@ std::shared_ptr Image::Transpose() const { int in_bytes_per_line = BytesPerLine(); int bytes_per_pixel = num_of_channels_ * bytes_per_channel_; - transpose_functor func(thrust::raw_pointer_cast(data_.data()), - width_, in_bytes_per_line, out_bytes_per_line, + transpose_functor func(thrust::raw_pointer_cast(data_.data()), width_, + in_bytes_per_line, out_bytes_per_line, bytes_per_pixel, thrust::raw_pointer_cast(output->data_.data())); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(width_ * height_), func); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(width_ * height_), + func); return output; } @@ -392,10 +419,12 @@ std::shared_ptr Image::FlipVertical() const { auto output = std::make_shared(); output->Prepare(width_, height_, num_of_channels_, bytes_per_channel_); - vertical_flip_functor func(thrust::raw_pointer_cast(data_.data()), width_, height_, - num_of_channels_ * bytes_per_channel_, + vertical_flip_functor func(thrust::raw_pointer_cast(data_.data()), width_, + height_, num_of_channels_ * bytes_per_channel_, thrust::raw_pointer_cast(output->data_.data())); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(width_ * height_), func); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(width_ * height_), + func); return output; } @@ -403,10 +432,13 @@ std::shared_ptr Image::FlipHorizontal() const { auto output = std::make_shared(); output->Prepare(width_, height_, num_of_channels_, bytes_per_channel_); - horizontal_flip_functor func(thrust::raw_pointer_cast(data_.data()), width_, - num_of_channels_ * bytes_per_channel_, - thrust::raw_pointer_cast(output->data_.data())); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(width_ * height_), func); + horizontal_flip_functor func( + thrust::raw_pointer_cast(data_.data()), width_, + num_of_channels_ * bytes_per_channel_, + thrust::raw_pointer_cast(output->data_.data())); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(width_ * height_), + func); return output; } diff --git a/src/cupoch/geometry/image.h b/src/cupoch/geometry/image.h index f9541934..88a183eb 100644 --- a/src/cupoch/geometry/image.h +++ b/src/cupoch/geometry/image.h @@ -1,7 +1,8 @@ #pragma once +#include + #include "cupoch/geometry/geometry2d.h" #include "cupoch/utility/device_vector.h" -#include namespace cupoch { @@ -62,7 +63,7 @@ class Image : public Geometry2D { Eigen::Vector2f GetMaxBound() const override; thrust::host_vector GetData() const; - void SetData(const thrust::host_vector& data); + void SetData(const thrust::host_vector &data); /// \brief Test if coordinate `(u, v)` is located in the inner_marge of the /// image. @@ -81,11 +82,10 @@ class Image : public Geometry2D { } /// \brief Prepare Image properties and allocate Image buffer. - __host__ __device__ - Image &Prepare(int width, - int height, - int num_of_channels, - int bytes_per_channel) { + __host__ __device__ Image &Prepare(int width, + int height, + int num_of_channels, + int bytes_per_channel) { width_ = width; height_ = height; num_of_channels_ = num_of_channels; @@ -95,8 +95,7 @@ class Image : public Geometry2D { } /// \brief Returns data size per line (row, or the width) in bytes. - __host__ __device__ - int BytesPerLine() const { + __host__ __device__ int BytesPerLine() const { return width_ * num_of_channels_ * bytes_per_channel_; } @@ -137,8 +136,9 @@ class Image : public Geometry2D { std::shared_ptr Filter(Image::FilterType type) const; /// Function to filter image with arbitrary dx, dy separable filters. - std::shared_ptr Filter(const utility::device_vector &dx, - const utility::device_vector &dy) const; + std::shared_ptr Filter( + const utility::device_vector &dx, + const utility::device_vector &dy) const; std::shared_ptr FilterHorizontal( const utility::device_vector &kernel) const; @@ -187,20 +187,28 @@ class Image : public Geometry2D { }; template -__host__ __device__ -T *PointerAt(const uint8_t* data, int width, int u, int v) { +__host__ __device__ T *PointerAt(const uint8_t *data, int width, int u, int v) { return (T *)(data + (v * width + u) * sizeof(T)); } template -__host__ __device__ -T *PointerAt(const uint8_t* data, int width, int num_of_channels, int u, int v, int ch) { +__host__ __device__ T *PointerAt(const uint8_t *data, + int width, + int num_of_channels, + int u, + int v, + int ch) { return (T *)(data + ((v * width + u) * num_of_channels + ch) * sizeof(T)); } -__host__ __device__ -inline thrust::pair FloatValueAt(const uint8_t* data, float u, float v, int width, int height, - int num_of_channels, int bytes_per_channel) { +__host__ __device__ inline thrust::pair FloatValueAt( + const uint8_t *data, + float u, + float v, + int width, + int height, + int num_of_channels, + int bytes_per_channel) { if ((num_of_channels != 1) || (bytes_per_channel != 4) || (u < 0.0 || u > (float)(width - 1) || v < 0.0 || v > (float)(height - 1))) { @@ -210,13 +218,14 @@ inline thrust::pair FloatValueAt(const uint8_t* data, float u, floa int vi = std::max(std::min((int)v, height - 2), 0); float pu = u - ui; float pv = v - vi; - float value[4] = {*PointerAt(data, width, ui, vi), *PointerAt(data, width, ui, vi + 1), + float value[4] = {*PointerAt(data, width, ui, vi), + *PointerAt(data, width, ui, vi + 1), *PointerAt(data, width, ui + 1, vi), *PointerAt(data, width, ui + 1, vi + 1)}; - return thrust::make_pair(true, - (value[0] * (1 - pv) + value[1] * pv) * (1 - pu) + - (value[2] * (1 - pv) + value[3] * pv) * pu); + return thrust::make_pair( + true, (value[0] * (1 - pv) + value[1] * pv) * (1 - pu) + + (value[2] * (1 - pv) + value[3] * pv) * pu); } -} -} \ No newline at end of file +} // namespace geometry +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/geometry/image_factory.cu b/src/cupoch/geometry/image_factory.cu index b82133d1..239dfe35 100644 --- a/src/cupoch/geometry/image_factory.cu +++ b/src/cupoch/geometry/image_factory.cu @@ -8,39 +8,40 @@ using namespace cupoch::geometry; namespace { struct compute_camera_distance_functor { - compute_camera_distance_functor(uint8_t* data, int width, - const float* xx, const float* yy) - : data_(data), width_(width), - xx_(xx), yy_(yy) {}; - uint8_t* data_; + compute_camera_distance_functor(uint8_t *data, + int width, + const float *xx, + const float *yy) + : data_(data), width_(width), xx_(xx), yy_(yy){}; + uint8_t *data_; const int width_; - const float* xx_; - const float* yy_; - __device__ - void operator() (size_t idx) { + const float *xx_; + const float *yy_; + __device__ void operator()(size_t idx) { int i = idx / width_; int j = idx % width_; - float *fp = - (float *)(data_ + idx * sizeof(float)); + float *fp = (float *)(data_ + idx * sizeof(float)); *fp = sqrtf(xx_[j] * xx_[j] + yy_[i] * yy_[i] + 1.0f); } }; struct make_float_image_functor { - make_float_image_functor(const uint8_t* image, int num_of_channels, + make_float_image_functor(const uint8_t *image, + int num_of_channels, int bytes_per_channel, Image::ColorToIntensityConversionType type, - uint8_t* fimage) - : image_(image), num_of_channels_(num_of_channels), - bytes_per_channel_(bytes_per_channel), - type_(type), fimage_(fimage) {}; - const uint8_t* image_; + uint8_t *fimage) + : image_(image), + num_of_channels_(num_of_channels), + bytes_per_channel_(bytes_per_channel), + type_(type), + fimage_(fimage){}; + const uint8_t *image_; int num_of_channels_; int bytes_per_channel_; Image::ColorToIntensityConversionType type_; - uint8_t* fimage_; - __device__ - void operator() (size_t idx) { + uint8_t *fimage_; + __device__ void operator()(size_t idx) { float *p = (float *)(fimage_ + idx * 4); const uint8_t *pi = image_ + idx * num_of_channels_ * bytes_per_channel_; @@ -91,20 +92,20 @@ struct make_float_image_functor { } }; -template +template struct restore_from_float_image_functor { - restore_from_float_image_functor(const float* src, uint8_t* dst) - : src_(src), dst_(dst) {}; - const float* src_; - uint8_t* dst_; - __device__ - void operator() (size_t idx) { - if (sizeof(T) == 1) *(dst_ + idx) = static_cast(*(src_ + idx) * 255.0f); + restore_from_float_image_functor(const float *src, uint8_t *dst) + : src_(src), dst_(dst){}; + const float *src_; + uint8_t *dst_; + __device__ void operator()(size_t idx) { + if (sizeof(T) == 1) + *(dst_ + idx) = static_cast(*(src_ + idx) * 255.0f); if (sizeof(T) == 2) *(dst_ + idx) = static_cast(*(src_ + idx)); } }; -} +} // namespace std::shared_ptr Image::CreateDepthToCameraDistanceMultiplierFloatImage( const camera::PinholeCameraIntrinsic &intrinsic) { @@ -116,17 +117,24 @@ std::shared_ptr Image::CreateDepthToCameraDistanceMultiplierFloatImage( float fpp1 = (float)intrinsic.GetPrincipalPoint().second; utility::device_vector xx(intrinsic.width_); utility::device_vector yy(intrinsic.height_); - thrust::tabulate(utility::exec_policy(utility::GetStream(0))->on(utility::GetStream(0)), xx.begin(), xx.end(), - [=] __device__ (int idx) {return (idx - fpp0) * ffl_inv0;}); - thrust::tabulate(utility::exec_policy(utility::GetStream(1))->on(utility::GetStream(1)), yy.begin(), yy.end(), - [=] __device__ (int idx) {return (idx - fpp1) * ffl_inv1;}); + thrust::tabulate(utility::exec_policy(utility::GetStream(0)) + ->on(utility::GetStream(0)), + xx.begin(), xx.end(), [=] __device__(int idx) { + return (idx - fpp0) * ffl_inv0; + }); + thrust::tabulate(utility::exec_policy(utility::GetStream(1)) + ->on(utility::GetStream(1)), + yy.begin(), yy.end(), [=] __device__(int idx) { + return (idx - fpp1) * ffl_inv1; + }); cudaSafeCall(cudaDeviceSynchronize()); - compute_camera_distance_functor func(thrust::raw_pointer_cast(fimage->data_.data()), - intrinsic.width_, - thrust::raw_pointer_cast(xx.data()), - thrust::raw_pointer_cast(yy.data())); + compute_camera_distance_functor func( + thrust::raw_pointer_cast(fimage->data_.data()), intrinsic.width_, + thrust::raw_pointer_cast(xx.data()), + thrust::raw_pointer_cast(yy.data())); for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(intrinsic.height_ * intrinsic.width_), + thrust::make_counting_iterator(intrinsic.height_ * + intrinsic.width_), func); return fimage; } @@ -138,11 +146,13 @@ std::shared_ptr Image::CreateFloatImage( return fimage; } fimage->Prepare(width_, height_, 1, 4); - make_float_image_functor func(thrust::raw_pointer_cast(data_.data()), - num_of_channels_, bytes_per_channel_, type, - thrust::raw_pointer_cast(fimage->data_.data())); - thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(width_ * height_), func); + make_float_image_functor func( + thrust::raw_pointer_cast(data_.data()), num_of_channels_, + bytes_per_channel_, type, + thrust::raw_pointer_cast(fimage->data_.data())); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(width_ * height_), + func); return fimage; } @@ -155,9 +165,12 @@ std::shared_ptr Image::CreateImageFromFloatImage() const { } output->Prepare(width_, height_, num_of_channels_, sizeof(T)); - restore_from_float_image_functor func((const float*)thrust::raw_pointer_cast(data_.data()), - thrust::raw_pointer_cast(output->data_.data())); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(width_ * height_), func); + restore_from_float_image_functor func( + (const float *)thrust::raw_pointer_cast(data_.data()), + thrust::raw_pointer_cast(output->data_.data())); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(width_ * height_), + func); return output; } diff --git a/src/cupoch/geometry/intersection_test.h b/src/cupoch/geometry/intersection_test.h index 7d5c764a..f6ce438c 100644 --- a/src/cupoch/geometry/intersection_test.h +++ b/src/cupoch/geometry/intersection_test.h @@ -1,29 +1,29 @@ #pragma once -#include #include +#include + namespace cupoch { namespace geometry { namespace intersection_test { - __host__ __device__ - inline bool TriangleTriangle3d(const Eigen::Vector3f& p0, - const Eigen::Vector3f& p1, - const Eigen::Vector3f& p2, - const Eigen::Vector3f& q0, - const Eigen::Vector3f& q1, - const Eigen::Vector3f& q2); +__host__ __device__ inline bool TriangleTriangle3d(const Eigen::Vector3f &p0, + const Eigen::Vector3f &p1, + const Eigen::Vector3f &p2, + const Eigen::Vector3f &q0, + const Eigen::Vector3f &q1, + const Eigen::Vector3f &q2); - __host__ __device__ - inline bool TriangleAABB(const Eigen::Vector3f& box_center, - const Eigen::Vector3f& box_half_size, - const Eigen::Vector3f& vert0, - const Eigen::Vector3f& vert1, - const Eigen::Vector3f& vert2); -} +__host__ __device__ inline bool TriangleAABB( + const Eigen::Vector3f &box_center, + const Eigen::Vector3f &box_half_size, + const Eigen::Vector3f &vert0, + const Eigen::Vector3f &vert1, + const Eigen::Vector3f &vert2); +} // namespace intersection_test -} -} +} // namespace geometry +} // namespace cupoch #include "cupoch/geometry/intersection_test.inl" \ No newline at end of file diff --git a/src/cupoch/geometry/intersection_test.inl b/src/cupoch/geometry/intersection_test.inl index a5eaf530..e63609c0 100644 --- a/src/cupoch/geometry/intersection_test.inl +++ b/src/cupoch/geometry/intersection_test.inl @@ -1,19 +1,19 @@ -#include "cupoch/geometry/intersection_test.h" - #include #include +#include "cupoch/geometry/intersection_test.h" + namespace cupoch { namespace geometry { namespace intersection_test { -bool TriangleTriangle3d(const Eigen::Vector3f& p0, - const Eigen::Vector3f& p1, - const Eigen::Vector3f& p2, - const Eigen::Vector3f& q0, - const Eigen::Vector3f& q1, - const Eigen::Vector3f& q2) { +bool TriangleTriangle3d(const Eigen::Vector3f &p0, + const Eigen::Vector3f &p1, + const Eigen::Vector3f &p2, + const Eigen::Vector3f &q0, + const Eigen::Vector3f &q1, + const Eigen::Vector3f &q2) { const Eigen::Vector3f mu = (p0 + p1 + p2 + q0 + q1 + q2) / 6; const Eigen::Vector3f sigma = (((p0 - mu).array().square() + (p1 - mu).array().square() + @@ -32,20 +32,20 @@ bool TriangleTriangle3d(const Eigen::Vector3f& p0, q1m.data(), q2m.data()) != 0; } -bool TriangleAABB(const Eigen::Vector3f& box_center, - const Eigen::Vector3f& box_half_size, - const Eigen::Vector3f& vert0, - const Eigen::Vector3f& vert1, - const Eigen::Vector3f& vert2) { - float* tri_verts[3] = {const_cast(vert0.data()), - const_cast(vert1.data()), - const_cast(vert2.data())}; - return triBoxOverlap(const_cast(box_center.data()), - const_cast(box_half_size.data()), +bool TriangleAABB(const Eigen::Vector3f &box_center, + const Eigen::Vector3f &box_half_size, + const Eigen::Vector3f &vert0, + const Eigen::Vector3f &vert1, + const Eigen::Vector3f &vert2) { + float *tri_verts[3] = {const_cast(vert0.data()), + const_cast(vert1.data()), + const_cast(vert2.data())}; + return triBoxOverlap(const_cast(box_center.data()), + const_cast(box_half_size.data()), tri_verts) != 0; } -} +} // namespace intersection_test -} -} \ No newline at end of file +} // namespace geometry +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/geometry/kdtree_flann.cu b/src/cupoch/geometry/kdtree_flann.cu index 64e108d3..9a1842b7 100644 --- a/src/cupoch/geometry/kdtree_flann.cu +++ b/src/cupoch/geometry/kdtree_flann.cu @@ -2,11 +2,12 @@ #include "cupoch/geometry/kdtree_flann.h" #define FLANN_USE_CUDA #include + #include "cupoch/geometry/pointcloud.h" #include "cupoch/geometry/trianglemesh.h" +#include "cupoch/utility/console.h" #include "cupoch/utility/eigen.h" #include "cupoch/utility/helper.h" -#include "cupoch/utility/console.h" using namespace cupoch; using namespace cupoch::geometry; @@ -14,18 +15,16 @@ using namespace cupoch::geometry; namespace { struct convert_float4_functor { - __device__ - float4 operator() (const Eigen::Vector3f& x) const { + __device__ float4 operator()(const Eigen::Vector3f &x) const { return make_float4(x[0], x[1], x[2], 0.0f); } }; -} - +} // namespace KDTreeFlann::KDTreeFlann() {} -KDTreeFlann::KDTreeFlann(const Geometry &data) {SetGeometry(data);} +KDTreeFlann::KDTreeFlann(const Geometry &data) { SetGeometry(data); } KDTreeFlann::~KDTreeFlann() {} @@ -76,18 +75,24 @@ int KDTreeFlann::SearchKNN(const utility::device_vector &query, // This is optimized code for heavily repeated search. // Other flann::Index::knnSearch() implementations lose performance due to // memory allocation/deallocation. - if (data_.empty() || query.empty() || dataset_size_ <= 0 || knn < 0 || knn > NUM_MAX_NN) return -1; + if (data_.empty() || query.empty() || dataset_size_ <= 0 || knn < 0 || + knn > NUM_MAX_NN) + return -1; T query0 = query[0]; if (size_t(query0.size()) != dimension_) return -1; convert_float4_functor func; utility::device_vector query_f4(query.size()); thrust::transform(query.begin(), query.end(), query_f4.begin(), func); - flann::Matrix query_flann((float *)(thrust::raw_pointer_cast(query_f4.data())), query.size(), dimension_, sizeof(float) * 4); + flann::Matrix query_flann( + (float *)(thrust::raw_pointer_cast(query_f4.data())), query.size(), + dimension_, sizeof(float) * 4); const int total_size = query.size() * knn; indices.resize(total_size); distance2.resize(total_size); - flann::Matrix indices_flann(thrust::raw_pointer_cast(indices.data()), query_flann.rows, knn); - flann::Matrix dists_flann(thrust::raw_pointer_cast(distance2.data()), query_flann.rows, knn); + flann::Matrix indices_flann(thrust::raw_pointer_cast(indices.data()), + query_flann.rows, knn); + flann::Matrix dists_flann(thrust::raw_pointer_cast(distance2.data()), + query_flann.rows, knn); flann::SearchParams param; param.matrices_in_gpu_ram = true; int k = flann_index_->knnSearch(query_flann, indices_flann, dists_flann, @@ -110,14 +115,18 @@ int KDTreeFlann::SearchRadius(const utility::device_vector &query, convert_float4_functor func; utility::device_vector query_f4(query.size()); thrust::transform(query.begin(), query.end(), query_f4.begin(), func); - flann::Matrix query_flann((float *)(thrust::raw_pointer_cast(query_f4.data())), query.size(), dimension_, sizeof(float) * 4); + flann::Matrix query_flann( + (float *)(thrust::raw_pointer_cast(query_f4.data())), query.size(), + dimension_, sizeof(float) * 4); flann::SearchParams param(-1, 0.0); param.max_neighbors = NUM_MAX_NN; param.matrices_in_gpu_ram = true; indices.resize(query.size() * NUM_MAX_NN); distance2.resize(query.size() * NUM_MAX_NN); - flann::Matrix indices_flann(thrust::raw_pointer_cast(indices.data()), query_flann.rows, NUM_MAX_NN); - flann::Matrix dists_flann(thrust::raw_pointer_cast(distance2.data()), query_flann.rows, NUM_MAX_NN); + flann::Matrix indices_flann(thrust::raw_pointer_cast(indices.data()), + query_flann.rows, NUM_MAX_NN); + flann::Matrix dists_flann(thrust::raw_pointer_cast(distance2.data()), + query_flann.rows, NUM_MAX_NN); int k = flann_index_->radiusSearch(query_flann, indices_flann, dists_flann, float(radius * radius), param); return k; @@ -133,20 +142,26 @@ int KDTreeFlann::SearchHybrid(const utility::device_vector &query, // It is also the recommended setting for search. // Other flann::Index::radiusSearch() implementations lose performance due // to memory allocation/deallocation. - if (data_.empty() || query.empty() || dataset_size_ <= 0 || max_nn < 0 || max_nn > NUM_MAX_NN) return -1; + if (data_.empty() || query.empty() || dataset_size_ <= 0 || max_nn < 0 || + max_nn > NUM_MAX_NN) + return -1; T query0 = query[0]; if (size_t(query0.size()) != dimension_) return -1; convert_float4_functor func; utility::device_vector query_f4(query.size()); thrust::transform(query.begin(), query.end(), query_f4.begin(), func); - flann::Matrix query_flann((float *)(thrust::raw_pointer_cast(query_f4.data())), query.size(), dimension_, sizeof(float) * 4); + flann::Matrix query_flann( + (float *)(thrust::raw_pointer_cast(query_f4.data())), query.size(), + dimension_, sizeof(float) * 4); flann::SearchParams param(-1, 0.0); param.max_neighbors = max_nn; param.matrices_in_gpu_ram = true; indices.resize(query.size() * max_nn); distance2.resize(query.size() * max_nn); - flann::Matrix indices_flann(thrust::raw_pointer_cast(indices.data()), query_flann.rows, max_nn); - flann::Matrix dists_flann(thrust::raw_pointer_cast(distance2.data()), query_flann.rows, max_nn); + flann::Matrix indices_flann(thrust::raw_pointer_cast(indices.data()), + query_flann.rows, max_nn); + flann::Matrix dists_flann(thrust::raw_pointer_cast(distance2.data()), + query_flann.rows, max_nn); int k = flann_index_->radiusSearch(query_flann, indices_flann, dists_flann, float(radius * radius), param); return k; @@ -164,8 +179,9 @@ bool KDTreeFlann::SetRawData(const utility::device_vector &data) { data_.resize(dataset_size_); convert_float4_functor func; thrust::transform(data.begin(), data.end(), data_.begin(), func); - flann_dataset_.reset(new flann::Matrix((float*)thrust::raw_pointer_cast(data_.data()), - dataset_size_, dimension_, sizeof(float) * 4)); + flann_dataset_.reset(new flann::Matrix( + (float *)thrust::raw_pointer_cast(data_.data()), dataset_size_, + dimension_, sizeof(float) * 4)); flann::KDTreeCuda3dIndexParams index_params; flann_index_.reset(new flann::KDTreeCuda3dIndex>( *flann_dataset_, index_params)); @@ -224,7 +240,8 @@ int KDTreeFlann::SearchHybrid(const T &query, utility::device_vector query_dv(1, query); utility::device_vector indices_dv; utility::device_vector distance2_dv; - auto result = SearchHybrid(query_dv, radius, max_nn, indices_dv, distance2_dv); + auto result = + SearchHybrid(query_dv, radius, max_nn, indices_dv, distance2_dv); indices = indices_dv; distance2 = distance2_dv; return result; @@ -274,4 +291,3 @@ template int KDTreeFlann::SearchHybrid( thrust::host_vector &distance2) const; template bool KDTreeFlann::SetRawData( const utility::device_vector &data); - \ No newline at end of file diff --git a/src/cupoch/geometry/kdtree_flann.h b/src/cupoch/geometry/kdtree_flann.h index 788eff07..43ac7a5d 100644 --- a/src/cupoch/geometry/kdtree_flann.h +++ b/src/cupoch/geometry/kdtree_flann.h @@ -1,11 +1,12 @@ #pragma once +#include + #include #include #include "cupoch/geometry/kdtree_search_param.h" #include "cupoch/utility/device_vector.h" -#include namespace flann { template diff --git a/src/cupoch/geometry/lineset.cu b/src/cupoch/geometry/lineset.cu index 8338a58c..42a2fd62 100644 --- a/src/cupoch/geometry/lineset.cu +++ b/src/cupoch/geometry/lineset.cu @@ -1,8 +1,8 @@ -#include "cupoch/geometry/lineset.h" -#include "cupoch/geometry/boundingvolume.h" - #include +#include "cupoch/geometry/boundingvolume.h" +#include "cupoch/geometry/lineset.h" + using namespace cupoch; using namespace cupoch::geometry; @@ -10,19 +10,25 @@ LineSet::LineSet() : Geometry3D(Geometry::GeometryType::LineSet) {} LineSet::LineSet(const utility::device_vector &points, const utility::device_vector &lines) - : Geometry3D(Geometry::GeometryType::LineSet), points_(points), lines_(lines) {} + : Geometry3D(Geometry::GeometryType::LineSet), + points_(points), + lines_(lines) {} LineSet::LineSet(const thrust::host_vector &points, const thrust::host_vector &lines) - : Geometry3D(Geometry::GeometryType::LineSet), points_(points), lines_(lines) {} + : Geometry3D(Geometry::GeometryType::LineSet), + points_(points), + lines_(lines) {} LineSet::LineSet(const LineSet &other) - : Geometry3D(Geometry::GeometryType::LineSet), points_(other.points_), - lines_(other.lines_), colors_(other.colors_) {} + : Geometry3D(Geometry::GeometryType::LineSet), + points_(other.points_), + lines_(other.lines_), + colors_(other.colors_) {} LineSet::~LineSet() {} -void LineSet::SetPoints(const thrust::host_vector& points) { +void LineSet::SetPoints(const thrust::host_vector &points) { points_ = points; } @@ -31,7 +37,7 @@ thrust::host_vector LineSet::GetPoints() const { return points; } -void LineSet::SetLines(const thrust::host_vector& lines) { +void LineSet::SetLines(const thrust::host_vector &lines) { lines_ = lines; } @@ -40,7 +46,7 @@ thrust::host_vector LineSet::GetLines() const { return lines; } -void LineSet::SetColors(const thrust::host_vector& colors) { +void LineSet::SetColors(const thrust::host_vector &colors) { colors_ = colors; } @@ -93,8 +99,7 @@ LineSet &LineSet::Rotate(const Eigen::Matrix3f &R, bool center) { } thrust::pair LineSet::GetLineCoordinate( - size_t line_index) const { + size_t line_index) const { const Eigen::Vector2i idxs = lines_[line_index]; - return thrust::make_pair(points_[idxs[0]], - points_[idxs[1]]); + return thrust::make_pair(points_[idxs[0]], points_[idxs[1]]); } \ No newline at end of file diff --git a/src/cupoch/geometry/lineset.h b/src/cupoch/geometry/lineset.h index 64530d81..7ab3ce93 100644 --- a/src/cupoch/geometry/lineset.h +++ b/src/cupoch/geometry/lineset.h @@ -1,11 +1,12 @@ #pragma once +#include + #include #include -#include "cupoch/utility/device_vector.h" -#include #include "cupoch/geometry/geometry3d.h" +#include "cupoch/utility/device_vector.h" namespace cupoch { namespace geometry { @@ -25,13 +26,13 @@ class LineSet : public Geometry3D { LineSet(const LineSet &other); ~LineSet(); - void SetPoints(const thrust::host_vector& points); + void SetPoints(const thrust::host_vector &points); thrust::host_vector GetPoints() const; - void SetLines(const thrust::host_vector& lines); + void SetLines(const thrust::host_vector &lines); thrust::host_vector GetLines() const; - void SetColors(const thrust::host_vector& colors); + void SetColors(const thrust::host_vector &colors); thrust::host_vector GetColors() const; public: @@ -70,7 +71,8 @@ class LineSet : public Geometry3D { static std::shared_ptr CreateFromPointCloudCorrespondences( const PointCloud &cloud0, const PointCloud &cloud1, - const utility::device_vector> &correspondences); + const utility::device_vector> + &correspondences); static std::shared_ptr CreateFromOrientedBoundingBox( const OrientedBoundingBox &box); diff --git a/src/cupoch/geometry/lineset_factory.cu b/src/cupoch/geometry/lineset_factory.cu index 5775fa43..854c5942 100644 --- a/src/cupoch/geometry/lineset_factory.cu +++ b/src/cupoch/geometry/lineset_factory.cu @@ -12,22 +12,25 @@ using namespace cupoch::geometry; namespace { struct convert_trianglemesh_line_functor { - convert_trianglemesh_line_functor(const Eigen::Vector3i* triangles, Eigen::Vector2i* lines) + convert_trianglemesh_line_functor(const Eigen::Vector3i *triangles, + Eigen::Vector2i *lines) : triangles_(triangles), lines_(lines){}; - const Eigen::Vector3i* triangles_; - Eigen::Vector2i* lines_; - __device__ - void operator() (size_t idx) const { - const Eigen::Vector3i& vidx = triangles_[idx]; + const Eigen::Vector3i *triangles_; + Eigen::Vector2i *lines_; + __device__ void operator()(size_t idx) const { + const Eigen::Vector3i &vidx = triangles_[idx]; thrust::minimum min; thrust::maximum max; - lines_[3 * idx] = Eigen::Vector2i(min(vidx[0], vidx[1]), max(vidx[0], vidx[1])); - lines_[3 * idx + 1] = Eigen::Vector2i(min(vidx[1], vidx[2]), max(vidx[1], vidx[2])); - lines_[3 * idx + 2] = Eigen::Vector2i(min(vidx[2], vidx[0]), max(vidx[2], vidx[0])); + lines_[3 * idx] = + Eigen::Vector2i(min(vidx[0], vidx[1]), max(vidx[0], vidx[1])); + lines_[3 * idx + 1] = + Eigen::Vector2i(min(vidx[1], vidx[2]), max(vidx[1], vidx[2])); + lines_[3 * idx + 2] = + Eigen::Vector2i(min(vidx[2], vidx[0]), max(vidx[2], vidx[0])); } }; -} +} // namespace std::shared_ptr LineSet::CreateFromPointCloudCorrespondences( const PointCloud &cloud0, @@ -39,14 +42,22 @@ std::shared_ptr LineSet::CreateFromPointCloudCorrespondences( const size_t corr_size = correspondences.size(); lineset_ptr->points_.resize(point0_size + point1_size); lineset_ptr->lines_.resize(corr_size); - thrust::copy_n(utility::exec_policy(utility::GetStream(0))->on(utility::GetStream(0)), - cloud0.points_.begin(), point0_size, lineset_ptr->points_.begin()); - thrust::copy_n(utility::exec_policy(utility::GetStream(1))->on(utility::GetStream(1)), - cloud1.points_.begin(), point1_size, lineset_ptr->points_.begin() + point0_size); - thrust::transform(utility::exec_policy(utility::GetStream(2))->on(utility::GetStream(2)), + thrust::copy_n(utility::exec_policy(utility::GetStream(0)) + ->on(utility::GetStream(0)), + cloud0.points_.begin(), point0_size, + lineset_ptr->points_.begin()); + thrust::copy_n(utility::exec_policy(utility::GetStream(1)) + ->on(utility::GetStream(1)), + cloud1.points_.begin(), point1_size, + lineset_ptr->points_.begin() + point0_size); + thrust::transform(utility::exec_policy(utility::GetStream(2)) + ->on(utility::GetStream(2)), correspondences.begin(), correspondences.end(), lineset_ptr->lines_.begin(), - [=] __device__ (const thrust::pair& corrs) {return Eigen::Vector2i(corrs.first, point0_size + corrs.second);}); + [=] __device__(const thrust::pair &corrs) { + return Eigen::Vector2i(corrs.first, + point0_size + corrs.second); + }); cudaSafeCall(cudaDeviceSynchronize()); return lineset_ptr; } @@ -56,16 +67,24 @@ std::shared_ptr LineSet::CreateFromTriangleMesh( auto lineset_ptr = std::make_shared(); lineset_ptr->points_.resize(mesh.vertices_.size()); lineset_ptr->lines_.resize(mesh.triangles_.size() * 3); - convert_trianglemesh_line_functor func(thrust::raw_pointer_cast(mesh.triangles_.data()), - thrust::raw_pointer_cast(lineset_ptr->lines_.data())); - thrust::copy(utility::exec_policy(utility::GetStream(0))->on(utility::GetStream(0)), mesh.vertices_.begin(), - mesh.vertices_.end(), lineset_ptr->points_.begin()); - thrust::for_each(utility::exec_policy(utility::GetStream(1))->on(utility::GetStream(1)), + convert_trianglemesh_line_functor func( + thrust::raw_pointer_cast(mesh.triangles_.data()), + thrust::raw_pointer_cast(lineset_ptr->lines_.data())); + thrust::copy(utility::exec_policy(utility::GetStream(0)) + ->on(utility::GetStream(0)), + mesh.vertices_.begin(), mesh.vertices_.end(), + lineset_ptr->points_.begin()); + thrust::for_each(utility::exec_policy(utility::GetStream(1)) + ->on(utility::GetStream(1)), thrust::make_counting_iterator(0), - thrust::make_counting_iterator(mesh.triangles_.size()), func); - auto end = thrust::unique(utility::exec_policy(utility::GetStream(1))->on(utility::GetStream(1)), - lineset_ptr->lines_.begin(), lineset_ptr->lines_.end()); - lineset_ptr->lines_.resize(thrust::distance(lineset_ptr->lines_.begin(), end)); + thrust::make_counting_iterator(mesh.triangles_.size()), + func); + auto end = thrust::unique(utility::exec_policy(utility::GetStream(1)) + ->on(utility::GetStream(1)), + lineset_ptr->lines_.begin(), + lineset_ptr->lines_.end()); + lineset_ptr->lines_.resize( + thrust::distance(lineset_ptr->lines_.begin(), end)); cudaSafeCall(cudaDeviceSynchronize()); return lineset_ptr; } @@ -74,7 +93,7 @@ std::shared_ptr LineSet::CreateFromOrientedBoundingBox( const OrientedBoundingBox &box) { auto line_set = std::make_shared(); const auto points = box.GetBoxPoints(); - for (const auto& pt: points) line_set->points_.push_back(pt); + for (const auto &pt : points) line_set->points_.push_back(pt); line_set->lines_.push_back(Eigen::Vector2i(0, 1)); line_set->lines_.push_back(Eigen::Vector2i(1, 7)); line_set->lines_.push_back(Eigen::Vector2i(7, 2)); @@ -95,7 +114,7 @@ std::shared_ptr LineSet::CreateFromAxisAlignedBoundingBox( const AxisAlignedBoundingBox &box) { auto line_set = std::make_shared(); const auto points = box.GetBoxPoints(); - for (const auto& pt: points) line_set->points_.push_back(pt); + for (const auto &pt : points) line_set->points_.push_back(pt); line_set->lines_.push_back(Eigen::Vector2i(0, 1)); line_set->lines_.push_back(Eigen::Vector2i(1, 7)); line_set->lines_.push_back(Eigen::Vector2i(7, 2)); diff --git a/src/cupoch/geometry/meshbase.cu b/src/cupoch/geometry/meshbase.cu index 4d159468..c1024cce 100644 --- a/src/cupoch/geometry/meshbase.cu +++ b/src/cupoch/geometry/meshbase.cu @@ -1,5 +1,5 @@ -#include "cupoch/geometry/meshbase.h" #include "cupoch/geometry/boundingvolume.h" +#include "cupoch/geometry/meshbase.h" #include "cupoch/utility/platform.h" using namespace cupoch; @@ -7,11 +7,13 @@ using namespace cupoch::geometry; MeshBase::MeshBase() : Geometry3D(Geometry::GeometryType::MeshBase) {} MeshBase::~MeshBase() {} -MeshBase::MeshBase(const MeshBase& other) - : Geometry3D(Geometry::GeometryType::MeshBase), vertices_(other.vertices_), - vertex_normals_(other.vertex_normals_), vertex_colors_(other.vertex_colors_) {} +MeshBase::MeshBase(const MeshBase &other) + : Geometry3D(Geometry::GeometryType::MeshBase), + vertices_(other.vertices_), + vertex_normals_(other.vertex_normals_), + vertex_colors_(other.vertex_colors_) {} -MeshBase& MeshBase::operator=(const MeshBase& other) { +MeshBase &MeshBase::operator=(const MeshBase &other) { vertices_ = other.vertices_; vertex_normals_ = other.vertex_normals_; vertex_colors_ = other.vertex_colors_; @@ -23,7 +25,8 @@ thrust::host_vector MeshBase::GetVertices() const { return vertices; } -void MeshBase::SetVertices(const thrust::host_vector& vertices) { +void MeshBase::SetVertices( + const thrust::host_vector &vertices) { vertices_ = vertices; } @@ -32,7 +35,8 @@ thrust::host_vector MeshBase::GetVertexNormals() const { return vertex_normals; } -void MeshBase::SetVertexNormals(const thrust::host_vector& vertex_normals) { +void MeshBase::SetVertexNormals( + const thrust::host_vector &vertex_normals) { vertex_normals_ = vertex_normals; } @@ -41,7 +45,8 @@ thrust::host_vector MeshBase::GetVertexColors() const { return vertex_colors; } -void MeshBase::SetVertexColors(const thrust::host_vector& vertex_colors) { +void MeshBase::SetVertexColors( + const thrust::host_vector &vertex_colors) { vertex_colors_ = vertex_colors; } @@ -52,7 +57,7 @@ MeshBase &MeshBase::Clear() { return *this; } -bool MeshBase::IsEmpty() const {return !HasVertices();} +bool MeshBase::IsEmpty() const { return !HasVertices(); } Eigen::Vector3f MeshBase::GetMinBound() const { return ComputeMinBound(vertices_); @@ -100,18 +105,21 @@ MeshBase &MeshBase::operator+=(const MeshBase &mesh) { size_t new_vert_num = old_vert_num + add_vert_num; if ((!HasVertices() || HasVertexNormals()) && mesh.HasVertexNormals()) { vertex_normals_.resize(new_vert_num); - thrust::copy(mesh.vertex_normals_.begin(), mesh.vertex_normals_.end(), vertex_normals_.begin() + old_vert_num); + thrust::copy(mesh.vertex_normals_.begin(), mesh.vertex_normals_.end(), + vertex_normals_.begin() + old_vert_num); } else { vertex_normals_.clear(); } if ((!HasVertices() || HasVertexColors()) && mesh.HasVertexColors()) { vertex_colors_.resize(new_vert_num); - thrust::copy(mesh.vertex_colors_.begin(), mesh.vertex_colors_.end(), vertex_colors_.begin() + old_vert_num); + thrust::copy(mesh.vertex_colors_.begin(), mesh.vertex_colors_.end(), + vertex_colors_.begin() + old_vert_num); } else { vertex_colors_.clear(); } vertices_.resize(new_vert_num); - thrust::copy(mesh.vertices_.begin(), mesh.vertices_.end(), vertices_.begin() + old_vert_num); + thrust::copy(mesh.vertices_.begin(), mesh.vertices_.end(), + vertices_.begin() + old_vert_num); return (*this); } @@ -121,11 +129,11 @@ MeshBase MeshBase::operator+(const MeshBase &mesh) const { MeshBase &MeshBase::NormalizeNormals() { thrust::for_each(vertex_normals_.begin(), vertex_normals_.end(), - [] __device__ (Eigen::Vector3f& nl) { + [] __device__(Eigen::Vector3f & nl) { nl.normalize(); - if (std::isnan(nl(0))) { - nl = Eigen::Vector3f(0.0, 0.0, 1.0); - } + if (isnan(nl(0))) { + nl = Eigen::Vector3f(0.0, 0.0, 1.0); + } }); return *this; } @@ -133,16 +141,19 @@ MeshBase &MeshBase::NormalizeNormals() { MeshBase::MeshBase(Geometry::GeometryType type) : Geometry3D(type) {} MeshBase::MeshBase(Geometry::GeometryType type, - const utility::device_vector &vertices) + const utility::device_vector &vertices) : Geometry3D(type), vertices_(vertices) {} -MeshBase::MeshBase(Geometry::GeometryType type, - const utility::device_vector &vertices, - const utility::device_vector &vertex_normals, - const utility::device_vector &vertex_colors) - : Geometry3D(type), vertices_(vertices), - vertex_normals_(vertex_normals), vertex_colors_(vertex_colors) {} +MeshBase::MeshBase( + Geometry::GeometryType type, + const utility::device_vector &vertices, + const utility::device_vector &vertex_normals, + const utility::device_vector &vertex_colors) + : Geometry3D(type), + vertices_(vertices), + vertex_normals_(vertex_normals), + vertex_colors_(vertex_colors) {} MeshBase::MeshBase(Geometry::GeometryType type, - const thrust::host_vector &vertices) + const thrust::host_vector &vertices) : Geometry3D(type), vertices_(vertices) {} diff --git a/src/cupoch/geometry/meshbase.h b/src/cupoch/geometry/meshbase.h index fac5f711..667ca272 100644 --- a/src/cupoch/geometry/meshbase.h +++ b/src/cupoch/geometry/meshbase.h @@ -1,10 +1,11 @@ #pragma once -#include -#include "cupoch/utility/device_vector.h" #include +#include + #include "cupoch/geometry/geometry3d.h" +#include "cupoch/utility/device_vector.h" #include "cupoch/utility/helper.h" namespace cupoch { @@ -34,17 +35,19 @@ class MeshBase : public Geometry3D { MeshBase(); virtual ~MeshBase(); - MeshBase(const MeshBase& other); - MeshBase& operator=(const MeshBase& other); + MeshBase(const MeshBase &other); + MeshBase &operator=(const MeshBase &other); thrust::host_vector GetVertices() const; - void SetVertices(const thrust::host_vector& vertices); + void SetVertices(const thrust::host_vector &vertices); thrust::host_vector GetVertexNormals() const; - void SetVertexNormals(const thrust::host_vector& vertex_normals); + void SetVertexNormals( + const thrust::host_vector &vertex_normals); thrust::host_vector GetVertexColors() const; - void SetVertexColors(const thrust::host_vector& vertex_colors); + void SetVertexColors( + const thrust::host_vector &vertex_colors); public: virtual MeshBase &Clear() override; @@ -63,17 +66,16 @@ class MeshBase : public Geometry3D { MeshBase &operator+=(const MeshBase &mesh); MeshBase operator+(const MeshBase &mesh) const; - __host__ __device__ - bool HasVertices() const { return vertices_.size() > 0; } + __host__ __device__ bool HasVertices() const { + return vertices_.size() > 0; + } - __host__ __device__ - bool HasVertexNormals() const { + __host__ __device__ bool HasVertexNormals() const { return vertices_.size() > 0 && vertex_normals_.size() == vertices_.size(); } - __host__ __device__ - bool HasVertexColors() const { + __host__ __device__ bool HasVertexColors() const { return vertices_.size() > 0 && vertex_colors_.size() == vertices_.size(); } @@ -97,11 +99,12 @@ class MeshBase : public Geometry3D { const utility::device_vector &vertex_colors); MeshBase(Geometry::GeometryType type, const thrust::host_vector &vertices); + public: utility::device_vector vertices_; utility::device_vector vertex_normals_; utility::device_vector vertex_colors_; }; -} -} \ No newline at end of file +} // namespace geometry +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/geometry/pointcloud.cu b/src/cupoch/geometry/pointcloud.cu index 162277d6..e1f82a24 100644 --- a/src/cupoch/geometry/pointcloud.cu +++ b/src/cupoch/geometry/pointcloud.cu @@ -1,52 +1,57 @@ -#include "cupoch/geometry/pointcloud.h" +#include + +#include "cupoch/camera/pinhole_camera_intrinsic.h" #include "cupoch/geometry/boundingvolume.h" #include "cupoch/geometry/image.h" -#include "cupoch/camera/pinhole_camera_intrinsic.h" +#include "cupoch/geometry/pointcloud.h" #include "cupoch/utility/console.h" #include "cupoch/utility/helper.h" #include "cupoch/utility/platform.h" -#include using namespace cupoch; using namespace cupoch::geometry; namespace { -template +template struct check_nan_functor { check_nan_functor(bool remove_nan, bool remove_infinite) - : remove_nan_(remove_nan), remove_infinite_(remove_infinite) {}; + : remove_nan_(remove_nan), remove_infinite_(remove_infinite){}; const bool remove_nan_; const bool remove_infinite_; - __device__ - bool operator()(const thrust::tuple& x) const { - const Eigen::Vector3f& point = thrust::get<0>(x); + __device__ bool operator()( + const thrust::tuple &x) const { + const Eigen::Vector3f &point = thrust::get<0>(x); bool is_nan = remove_nan_ && - (std::isnan(point(0)) || std::isnan(point(1)) || - std::isnan(point(2))); - bool is_infinite = remove_infinite_ && (std::isinf(point(0)) || - std::isinf(point(1)) || - std::isinf(point(2))); + (isnan(point(0)) || isnan(point(1)) || isnan(point(2))); + bool is_infinite = + remove_infinite_ && + (isinf(point(0)) || isinf(point(1)) || isinf(point(2))); return is_nan || is_infinite; } }; -} +} // namespace PointCloud::PointCloud() : Geometry3D(Geometry::GeometryType::PointCloud) {} -PointCloud::PointCloud(const thrust::host_vector& points) : Geometry3D(Geometry::GeometryType::PointCloud), points_(points) {} -PointCloud::PointCloud(const PointCloud& other) : Geometry3D(Geometry::GeometryType::PointCloud), points_(other.points_), normals_(other.normals_), colors_(other.colors_) {} +PointCloud::PointCloud(const thrust::host_vector &points) + : Geometry3D(Geometry::GeometryType::PointCloud), points_(points) {} +PointCloud::PointCloud(const PointCloud &other) + : Geometry3D(Geometry::GeometryType::PointCloud), + points_(other.points_), + normals_(other.normals_), + colors_(other.colors_) {} PointCloud::~PointCloud() {} -PointCloud& PointCloud::operator=(const PointCloud& other) { +PointCloud &PointCloud::operator=(const PointCloud &other) { points_ = other.points_; normals_ = other.normals_; colors_ = other.colors_; return *this; } -void PointCloud::SetPoints(const thrust::host_vector& points) { +void PointCloud::SetPoints(const thrust::host_vector &points) { points_ = points; } @@ -55,7 +60,8 @@ thrust::host_vector PointCloud::GetPoints() const { return points; } -void PointCloud::SetNormals(const thrust::host_vector& normals) { +void PointCloud::SetNormals( + const thrust::host_vector &normals) { normals_ = normals; } @@ -64,7 +70,7 @@ thrust::host_vector PointCloud::GetNormals() const { return normals; } -void PointCloud::SetColors(const thrust::host_vector& colors) { +void PointCloud::SetColors(const thrust::host_vector &colors) { colors_ = colors; } @@ -80,7 +86,7 @@ PointCloud &PointCloud::Clear() { return *this; } -bool PointCloud::IsEmpty() const {return !HasPoints();} +bool PointCloud::IsEmpty() const { return !HasPoints(); } Eigen::Vector3f PointCloud::GetMinBound() const { return ComputeMinBound(points_); @@ -90,21 +96,19 @@ Eigen::Vector3f PointCloud::GetMaxBound() const { return ComputeMaxBound(points_); } -Eigen::Vector3f PointCloud::GetCenter() const { - return ComputeCenter(points_); -} +Eigen::Vector3f PointCloud::GetCenter() const { return ComputeCenter(points_); } AxisAlignedBoundingBox PointCloud::GetAxisAlignedBoundingBox() const { return AxisAlignedBoundingBox::CreateFromPoints(points_); } -PointCloud& PointCloud::Translate(const Eigen::Vector3f &translation, +PointCloud &PointCloud::Translate(const Eigen::Vector3f &translation, bool relative) { TranslatePoints(translation, points_, relative); return *this; } -PointCloud& PointCloud::Scale(const float scale, bool center) { +PointCloud &PointCloud::Scale(const float scale, bool center) { ScalePoints(scale, points_, center); return *this; } @@ -117,7 +121,8 @@ PointCloud &PointCloud::Rotate(const Eigen::Matrix3f &R, bool center) { } PointCloud &PointCloud::NormalizeNormals() { - thrust::for_each(normals_.begin(), normals_.end(), [] __device__ (Eigen::Vector3f& nl) {nl.normalize();}); + thrust::for_each(normals_.begin(), normals_.end(), + [] __device__(Eigen::Vector3f & nl) { nl.normalize(); }); return *this; } @@ -126,7 +131,7 @@ PointCloud &PointCloud::PaintUniformColor(const Eigen::Vector3f &color) { return *this; } -PointCloud& PointCloud::Transform(const Eigen::Matrix4f& transformation) { +PointCloud &PointCloud::Transform(const Eigen::Matrix4f &transformation) { TransformPoints(utility::GetStream(0), transformation, points_); TransformNormals(utility::GetStream(1), transformation, normals_); cudaSafeCall(cudaDeviceSynchronize()); @@ -143,7 +148,8 @@ std::shared_ptr PointCloud::Crop( return SelectDownSample(bbox.GetPointIndicesWithinBoundingBox(points_)); } -PointCloud &PointCloud::RemoveNoneFinitePoints(bool remove_nan, bool remove_infinite) { +PointCloud &PointCloud::RemoveNoneFinitePoints(bool remove_nan, + bool remove_infinite) { bool has_normal = HasNormals(); bool has_color = HasColors(); size_t old_point_num = points_.size(); @@ -155,17 +161,26 @@ PointCloud &PointCloud::RemoveNoneFinitePoints(bool remove_nan, bool remove_infi } else if (has_normal && !has_color) { check_nan_functor func(remove_nan, remove_infinite); auto begin = make_tuple_iterator(points_.begin(), normals_.begin()); - auto end = thrust::remove_if(begin, make_tuple_iterator(points_.end(), normals_.end()), func); + auto end = thrust::remove_if( + begin, make_tuple_iterator(points_.end(), normals_.end()), + func); k = thrust::distance(begin, end); } else if (!has_normal && has_color) { check_nan_functor func(remove_nan, remove_infinite); auto begin = make_tuple_iterator(points_.begin(), colors_.begin()); - auto end = thrust::remove_if(begin, make_tuple_iterator(points_.end(), colors_.end()), func); + auto end = thrust::remove_if( + begin, make_tuple_iterator(points_.end(), colors_.end()), func); k = thrust::distance(begin, end); } else { - check_nan_functor func(remove_nan, remove_infinite); - auto begin = make_tuple_iterator(points_.begin(), normals_.begin(), colors_.begin()); - auto end = thrust::remove_if(begin, make_tuple_iterator(points_.end(), normals_.end(), colors_.end()), func); + check_nan_functor func( + remove_nan, remove_infinite); + auto begin = make_tuple_iterator(points_.begin(), normals_.begin(), + colors_.begin()); + auto end = thrust::remove_if( + begin, + make_tuple_iterator(points_.end(), normals_.end(), + colors_.end()), + func); k = thrust::distance(begin, end); } points_.resize(k); @@ -174,5 +189,5 @@ PointCloud &PointCloud::RemoveNoneFinitePoints(bool remove_nan, bool remove_infi utility::LogDebug( "[RemoveNoneFinitePoints] {:d} nan points have been removed.", (int)(old_point_num - k)); - return *this; + return *this; } diff --git a/src/cupoch/geometry/pointcloud.h b/src/cupoch/geometry/pointcloud.h index e47567b6..459127ed 100644 --- a/src/cupoch/geometry/pointcloud.h +++ b/src/cupoch/geometry/pointcloud.h @@ -1,9 +1,10 @@ #pragma once +#include + #include "cupoch/geometry/geometry3d.h" -#include "cupoch/utility/eigen.h" #include "cupoch/geometry/kdtree_search_param.h" #include "cupoch/utility/device_vector.h" -#include +#include "cupoch/utility/eigen.h" namespace cupoch { @@ -19,18 +20,18 @@ class RGBDImage; class PointCloud : public Geometry3D { public: PointCloud(); - PointCloud(const thrust::host_vector& points); - PointCloud(const PointCloud& other); + PointCloud(const thrust::host_vector &points); + PointCloud(const PointCloud &other); ~PointCloud(); - PointCloud& operator=(const PointCloud& other); + PointCloud &operator=(const PointCloud &other); - void SetPoints(const thrust::host_vector& points); + void SetPoints(const thrust::host_vector &points); thrust::host_vector GetPoints() const; - void SetNormals(const thrust::host_vector& normals); + void SetNormals(const thrust::host_vector &normals); thrust::host_vector GetNormals() const; - void SetColors(const thrust::host_vector& colors); + void SetColors(const thrust::host_vector &colors); thrust::host_vector GetColors() const; PointCloud &Clear() override; @@ -39,22 +40,19 @@ class PointCloud : public Geometry3D { Eigen::Vector3f GetMaxBound() const override; Eigen::Vector3f GetCenter() const override; AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override; - PointCloud& Transform(const Eigen::Matrix4f& transformation) override; - PointCloud& Translate(const Eigen::Vector3f &translation, + PointCloud &Transform(const Eigen::Matrix4f &transformation) override; + PointCloud &Translate(const Eigen::Vector3f &translation, bool relative = true) override; - PointCloud& Scale(const float scale, bool center = true) override; - PointCloud& Rotate(const Eigen::Matrix3f &R, bool center = true) override; + PointCloud &Scale(const float scale, bool center = true) override; + PointCloud &Rotate(const Eigen::Matrix3f &R, bool center = true) override; - __host__ __device__ - bool HasPoints() const { return !points_.empty(); } + __host__ __device__ bool HasPoints() const { return !points_.empty(); } - __host__ __device__ - bool HasNormals() const { + __host__ __device__ bool HasNormals() const { return !points_.empty() && normals_.size() == points_.size(); } - __host__ __device__ - bool HasColors() const { + __host__ __device__ bool HasColors() const { return !points_.empty() && colors_.size() == points_.size(); } @@ -72,7 +70,9 @@ class PointCloud : public Geometry3D { /// Function to select points from \param input pointcloud into /// \return output pointcloud /// Points with indices in \param indices are selected. - std::shared_ptr SelectDownSample(const utility::device_vector &indices, bool invert = false) const; + std::shared_ptr SelectDownSample( + const utility::device_vector &indices, + bool invert = false) const; /// Function to downsample \param input pointcloud into output pointcloud /// with a voxel \param voxel_size defines the resolution of the voxel grid, @@ -84,7 +84,6 @@ class PointCloud : public Geometry3D { /// uniformly \param every_k_points indicates the sample rate. std::shared_ptr UniformDownSample(size_t every_k_points) const; - std::tuple, utility::device_vector> RemoveRadiusOutliers(size_t nb_points, float search_radius) const; @@ -94,28 +93,30 @@ class PointCloud : public Geometry3D { /// Function to crop pointcloud into output pointcloud /// All points with coordinates outside the bounding box \param bbox are /// clipped. - std::shared_ptr Crop(const AxisAlignedBoundingBox& bbox) const; + std::shared_ptr Crop(const AxisAlignedBoundingBox &bbox) const; /// Function to compute the normals of a point cloud /// \param cloud is the input point cloud. It also stores the output /// normals. Normals are oriented with respect to the input point cloud if /// normals exist in the input. \param search_param The KDTree search /// parameters - bool EstimateNormals(const KDTreeSearchParam& search_param = KDTreeSearchParamKNN()); + bool EstimateNormals( + const KDTreeSearchParam &search_param = KDTreeSearchParamKNN()); /// Function to orient the normals of a point cloud /// \param cloud is the input point cloud. It must have normals. /// Normals are oriented with respect to \param orientation_reference - bool OrientNormalsToAlignWithDirection(const Eigen::Vector3f &orientation_reference = Eigen::Vector3f(0.0, 0.0, 1.0)); + bool OrientNormalsToAlignWithDirection( + const Eigen::Vector3f &orientation_reference = + Eigen::Vector3f(0.0, 0.0, 1.0)); /// Cluster PointCloud using the DBSCAN algorithm /// Ester et al., "A Density-Based Algorithm for Discovering Clusters /// in Large Spatial Databases with Noise", 1996 /// Returns a vector of point labels, -1 indicates noise according to /// the algorithm. - utility::device_vector ClusterDBSCAN(float eps, - size_t min_points, - bool print_progress = false) const; + utility::device_vector ClusterDBSCAN( + float eps, size_t min_points, bool print_progress = false) const; /// Factory function to create a pointcloud from a depth image and a camera /// model (PointCloudFactory.cpp) @@ -151,6 +152,5 @@ class PointCloud : public Geometry3D { utility::device_vector colors_; }; - -} -} \ No newline at end of file +} // namespace geometry +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/geometry/pointcloud_cluster.cu b/src/cupoch/geometry/pointcloud_cluster.cu index 0fb501d3..f487773c 100644 --- a/src/cupoch/geometry/pointcloud_cluster.cu +++ b/src/cupoch/geometry/pointcloud_cluster.cu @@ -1,5 +1,5 @@ -#include "cupoch/geometry/pointcloud.h" #include "cupoch/geometry/kdtree_flann.h" +#include "cupoch/geometry/pointcloud.h" #include "cupoch/utility/console.h" using namespace cupoch; @@ -8,26 +8,34 @@ using namespace cupoch::geometry; namespace { struct initialize_cluster_matrix_functor { - initialize_cluster_matrix_functor(const int* indices, const float* dists2, - float eps, int n_points, int* cluster_matrix, - int* valid, int* reroute) - : indices_(indices), dists2_(dists2), eps_(eps), - n_points_(n_points), cluster_matrix_(cluster_matrix), - valid_(valid), reroute_(reroute) {}; - const int* indices_; - const float* dists2_; + initialize_cluster_matrix_functor(const int *indices, + const float *dists2, + float eps, + int n_points, + int *cluster_matrix, + int *valid, + int *reroute) + : indices_(indices), + dists2_(dists2), + eps_(eps), + n_points_(n_points), + cluster_matrix_(cluster_matrix), + valid_(valid), + reroute_(reroute){}; + const int *indices_; + const float *dists2_; const float eps_; const int n_points_; - int* cluster_matrix_; - int* valid_; - int* reroute_; - __device__ - void operator() (size_t idx) { + int *cluster_matrix_; + int *valid_; + int *reroute_; + __device__ void operator()(size_t idx) { cluster_matrix_[idx * n_points_ + idx] = 1; for (int k = 0; k < NUM_MAX_NN; ++k) { if (indices_[idx * n_points_ + k] < 0) continue; if (dists2_[idx * n_points_ + k] <= eps_) { - cluster_matrix_[indices_[idx * n_points_ + k] * n_points_ + idx] = 1; + cluster_matrix_[indices_[idx * n_points_ + k] * n_points_ + + idx] = 1; } } valid_[idx] = 1; @@ -37,18 +45,21 @@ struct initialize_cluster_matrix_functor { struct merge_cluster_functor { merge_cluster_functor(int cluster_index, - int* cluster_matrix, - int* valid, int* reroute, + int *cluster_matrix, + int *valid, + int *reroute, int n_points) - : cluster_index_(cluster_index), cluster_matrix_(cluster_matrix), valid_(valid), - reroute_(reroute), n_points_(n_points) {}; + : cluster_index_(cluster_index), + cluster_matrix_(cluster_matrix), + valid_(valid), + reroute_(reroute), + n_points_(n_points){}; const int cluster_index_; - int* cluster_matrix_; - int* valid_; - int* reroute_; + int *cluster_matrix_; + int *valid_; + int *reroute_; const int n_points_; - __device__ - int get_reroute_index(int idx) { + __device__ int get_reroute_index(int idx) { int ans_idx = idx; while (ans_idx != -1) { if (valid_[ans_idx]) return ans_idx; @@ -57,14 +68,14 @@ struct merge_cluster_functor { return -1; } - __device__ - int operator() (int idx) { + __device__ int operator()(int idx) { if (valid_[idx] != 1 || idx == cluster_index_) return 0; int target_cluster = get_reroute_index(cluster_index_); if (idx == target_cluster) return 0; bool do_merge = false; for (int i = 0; i < n_points_; ++i) { - if (cluster_matrix_[i * n_points_ + idx] && cluster_matrix_[i * n_points_ + target_cluster]) { + if (cluster_matrix_[i * n_points_ + idx] && + cluster_matrix_[i * n_points_ + target_cluster]) { do_merge = true; break; } @@ -81,23 +92,26 @@ struct merge_cluster_functor { return 1; } return 0; - } + } }; - struct assign_cluster_functor { - assign_cluster_functor(const int* cluster_matrix, const int* valid, - int n_points, int min_points, int* labels) - : cluster_matrix_(cluster_matrix), valid_(valid), - n_points_(n_points), min_points_(min_points), - labels_(labels) {}; - const int* cluster_matrix_; - const int* valid_; + assign_cluster_functor(const int *cluster_matrix, + const int *valid, + int n_points, + int min_points, + int *labels) + : cluster_matrix_(cluster_matrix), + valid_(valid), + n_points_(n_points), + min_points_(min_points), + labels_(labels){}; + const int *cluster_matrix_; + const int *valid_; const int n_points_; const int min_points_; - int* labels_; - __device__ - void operator() (size_t idx) { + int *labels_; + __device__ void operator()(size_t idx) { if (!valid_[idx]) return; int count = 0; for (int i = 0; i < n_points_; ++i) { @@ -113,16 +127,15 @@ struct assign_cluster_functor { } }; -} +} // namespace -utility::device_vector PointCloud::ClusterDBSCAN(float eps, - size_t min_points, - bool print_progress) const { +utility::device_vector PointCloud::ClusterDBSCAN( + float eps, size_t min_points, bool print_progress) const { KDTreeFlann kdtree(*this); // precompute all neighbours utility::LogDebug("Precompute Neighbours"); utility::ConsoleProgressBar progress_bar( - points_.size(), "Precompute Neighbours", print_progress); + points_.size(), "Precompute Neighbours", print_progress); utility::device_vector indices; utility::device_vector dists2; kdtree.SearchRadius(points_, eps, indices, dists2); @@ -131,34 +144,37 @@ utility::device_vector PointCloud::ClusterDBSCAN(float eps, utility::device_vector cluster_matrix(n_pt * n_pt); utility::device_vector valid(n_pt); utility::device_vector reroute(n_pt); - initialize_cluster_matrix_functor func(thrust::raw_pointer_cast(indices.data()), - thrust::raw_pointer_cast(dists2.data()), - eps, n_pt, - thrust::raw_pointer_cast(cluster_matrix.data()), - thrust::raw_pointer_cast(valid.data()), - thrust::raw_pointer_cast(reroute.data())); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_pt), func); + initialize_cluster_matrix_functor func( + thrust::raw_pointer_cast(indices.data()), + thrust::raw_pointer_cast(dists2.data()), eps, n_pt, + thrust::raw_pointer_cast(cluster_matrix.data()), + thrust::raw_pointer_cast(valid.data()), + thrust::raw_pointer_cast(reroute.data())); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_pt), func); bool is_merged = true; for (int i = 0; i < n_pt; ++i) { ++progress_bar; while (is_merged) { - merge_cluster_functor func(i, thrust::raw_pointer_cast(cluster_matrix.data()), - thrust::raw_pointer_cast(valid.data()), - thrust::raw_pointer_cast(reroute.data()), n_pt); - const int n_merged = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator((int)n_pt), - func, 0, thrust::plus()); + merge_cluster_functor func( + i, thrust::raw_pointer_cast(cluster_matrix.data()), + thrust::raw_pointer_cast(valid.data()), + thrust::raw_pointer_cast(reroute.data()), n_pt); + const int n_merged = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator((int)n_pt), func, 0, + thrust::plus()); is_merged = n_merged > 0; } } utility::device_vector labels(points_.size(), -1); - assign_cluster_functor assign_func(thrust::raw_pointer_cast(cluster_matrix.data()), - thrust::raw_pointer_cast(valid.data()), - n_pt, min_points, - thrust::raw_pointer_cast(labels.data())); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_pt), - assign_func); + assign_cluster_functor assign_func( + thrust::raw_pointer_cast(cluster_matrix.data()), + thrust::raw_pointer_cast(valid.data()), n_pt, min_points, + thrust::raw_pointer_cast(labels.data())); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_pt), assign_func); return labels; } \ No newline at end of file diff --git a/src/cupoch/geometry/pointcloud_factory.cu b/src/cupoch/geometry/pointcloud_factory.cu index e5c80ad1..f4fa06c8 100644 --- a/src/cupoch/geometry/pointcloud_factory.cu +++ b/src/cupoch/geometry/pointcloud_factory.cu @@ -1,12 +1,12 @@ #include +#include #include "cupoch/camera/pinhole_camera_intrinsic.h" #include "cupoch/geometry/image.h" #include "cupoch/geometry/pointcloud.h" #include "cupoch/geometry/rgbdimage.h" -#include "cupoch/utility/helper.h" #include "cupoch/utility/console.h" -#include +#include "cupoch/utility/helper.h" using namespace cupoch; using namespace cupoch::geometry; @@ -14,16 +14,24 @@ using namespace cupoch::geometry; namespace { struct depth_to_pointcloud_functor { - depth_to_pointcloud_functor(const uint8_t* depth, const int width, - int num_of_channels, - int bytes_per_channel, const int stride, - const thrust::pair& principal_point, - const thrust::pair& focal_length, - const Eigen::Matrix4f& camera_pose) - : depth_(depth), width_(width), num_of_channels_(num_of_channels), - bytes_per_channel_(bytes_per_channel), stride_(stride), principal_point_(principal_point), - focal_length_(focal_length), camera_pose_(camera_pose) {}; - const uint8_t* depth_; + depth_to_pointcloud_functor( + const uint8_t *depth, + const int width, + int num_of_channels, + int bytes_per_channel, + const int stride, + const thrust::pair &principal_point, + const thrust::pair &focal_length, + const Eigen::Matrix4f &camera_pose) + : depth_(depth), + width_(width), + num_of_channels_(num_of_channels), + bytes_per_channel_(bytes_per_channel), + stride_(stride), + principal_point_(principal_point), + focal_length_(focal_length), + camera_pose_(camera_pose){}; + const uint8_t *depth_; const int width_; const int num_of_channels_; const int bytes_per_channel_; @@ -31,11 +39,11 @@ struct depth_to_pointcloud_functor { const thrust::pair principal_point_; const thrust::pair focal_length_; const Eigen::Matrix4f camera_pose_; - __device__ - Eigen::Vector3f operator() (size_t idx) { + __device__ Eigen::Vector3f operator()(size_t idx) { int row = idx / width_; int col = idx % width_; - const float d = *(float *)(&depth_[idx * num_of_channels_ * bytes_per_channel_ * stride_]); + const float d = *(float *)(&depth_[idx * num_of_channels_ * + bytes_per_channel_ * stride_]); if (d <= 0.0) { return Eigen::Vector3f(std::numeric_limits::infinity(), std::numeric_limits::infinity(), @@ -63,42 +71,46 @@ std::shared_ptr CreatePointCloudFromFloatDepthImage( const auto principal_point = intrinsic.GetPrincipalPoint(); const size_t depth_size = depth.width_ * depth.height_; pointcloud->points_.resize(depth_size); - depth_to_pointcloud_functor func(thrust::raw_pointer_cast(depth.data_.data()), - depth.width_, depth.num_of_channels_, - depth.bytes_per_channel_, stride, - principal_point, focal_length, - camera_pose); - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(depth_size), + depth_to_pointcloud_functor func( + thrust::raw_pointer_cast(depth.data_.data()), depth.width_, + depth.num_of_channels_, depth.bytes_per_channel_, stride, + principal_point, focal_length, camera_pose); + thrust::transform(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(depth_size), pointcloud->points_.begin(), func); pointcloud->RemoveNoneFinitePoints(true, true); return pointcloud; } template -struct convert_from_rgbdimage_functor{ - convert_from_rgbdimage_functor(const uint8_t* depth, - const uint8_t* color, - int width, - const Eigen::Matrix4f& camera_pose, - const thrust::pair& principal_point, - const thrust::pair& focal_length, - float scale, - bool project_valid_depth_only) - : depth_(depth), color_(color), width_(width), - camera_pose_(camera_pose), - principal_point_(principal_point), focal_length_(focal_length), - scale_(scale), - project_valid_depth_only_(project_valid_depth_only) {}; - const uint8_t* depth_; - const uint8_t* color_; +struct convert_from_rgbdimage_functor { + convert_from_rgbdimage_functor( + const uint8_t *depth, + const uint8_t *color, + int width, + const Eigen::Matrix4f &camera_pose, + const thrust::pair &principal_point, + const thrust::pair &focal_length, + float scale, + bool project_valid_depth_only) + : depth_(depth), + color_(color), + width_(width), + camera_pose_(camera_pose), + principal_point_(principal_point), + focal_length_(focal_length), + scale_(scale), + project_valid_depth_only_(project_valid_depth_only){}; + const uint8_t *depth_; + const uint8_t *color_; const int width_; const Eigen::Matrix4f camera_pose_; const thrust::pair principal_point_; const thrust::pair focal_length_; const float scale_; const bool project_valid_depth_only_; - __device__ - thrust::tuple operator() (size_t idx) const { + __device__ thrust::tuple operator()( + size_t idx) const { int i = idx / width_; int j = idx % width_; float *p = (float *)(depth_ + idx * sizeof(float)); @@ -118,18 +130,20 @@ struct convert_from_rgbdimage_functor{ float z = std::numeric_limits::quiet_NaN(); float x = std::numeric_limits::quiet_NaN(); float y = std::numeric_limits::quiet_NaN(); - return thrust::make_tuple(Eigen::Vector3f(x, y, z), - Eigen::Vector3f(std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN())); + return thrust::make_tuple( + Eigen::Vector3f(x, y, z), + Eigen::Vector3f(std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN())); } else { float z = std::numeric_limits::infinity(); float x = std::numeric_limits::infinity(); float y = std::numeric_limits::infinity(); - return thrust::make_tuple(Eigen::Vector3f(x, y, z), - Eigen::Vector3f(std::numeric_limits::infinity(), - std::numeric_limits::infinity(), - std::numeric_limits::infinity())); + return thrust::make_tuple( + Eigen::Vector3f(x, y, z), + Eigen::Vector3f(std::numeric_limits::infinity(), + std::numeric_limits::infinity(), + std::numeric_limits::infinity())); } } }; @@ -148,29 +162,31 @@ std::shared_ptr CreatePointCloudFromRGBDImageT( int num_valid_pixels = image.depth_.height_ * image.depth_.width_; pointcloud->points_.resize(num_valid_pixels); pointcloud->colors_.resize(num_valid_pixels); - convert_from_rgbdimage_functor func(thrust::raw_pointer_cast(image.depth_.data_.data()), - thrust::raw_pointer_cast(image.color_.data_.data()), - image.depth_.width_, - camera_pose, - principal_point, focal_length, scale, - project_valid_depth_only); + convert_from_rgbdimage_functor func( + thrust::raw_pointer_cast(image.depth_.data_.data()), + thrust::raw_pointer_cast(image.color_.data_.data()), + image.depth_.width_, camera_pose, principal_point, focal_length, + scale, project_valid_depth_only); thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(num_valid_pixels), - make_tuple_iterator(pointcloud->points_.begin(), pointcloud->colors_.begin()), func); - auto begin = make_tuple_iterator(pointcloud->points_.begin(), pointcloud->colors_.begin()); + make_tuple_iterator(pointcloud->points_.begin(), + pointcloud->colors_.begin()), + func); + auto begin = make_tuple_iterator(pointcloud->points_.begin(), + pointcloud->colors_.begin()); pointcloud->RemoveNoneFinitePoints(project_valid_depth_only, true); return pointcloud; } -} +} // namespace std::shared_ptr PointCloud::CreateFromDepthImage( - const Image &depth, - const camera::PinholeCameraIntrinsic &intrinsic, - const Eigen::Matrix4f &extrinsic /* = Eigen::Matrix4f::Identity()*/, - float depth_scale /* = 1000.0*/, - float depth_trunc /* = 1000.0*/, - int stride /* = 1*/) { + const Image &depth, + const camera::PinholeCameraIntrinsic &intrinsic, + const Eigen::Matrix4f &extrinsic /* = Eigen::Matrix4f::Identity()*/, + float depth_scale /* = 1000.0*/, + float depth_trunc /* = 1000.0*/, + int stride /* = 1*/) { if (depth.num_of_channels_ == 1) { if (depth.bytes_per_channel_ == 2) { auto float_depth = diff --git a/src/cupoch/geometry/rgbdimage.h b/src/cupoch/geometry/rgbdimage.h index 87697b34..05ca210b 100644 --- a/src/cupoch/geometry/rgbdimage.h +++ b/src/cupoch/geometry/rgbdimage.h @@ -1,8 +1,9 @@ #pragma once +#include + #include "cupoch/geometry/geometry2d.h" #include "cupoch/geometry/image.h" -#include namespace cupoch { namespace geometry { @@ -79,4 +80,4 @@ class RGBDImage : public Geometry2D { }; } // namespace geometry -} // namespace open3d \ No newline at end of file +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/geometry/rgbdimage_factory.cu b/src/cupoch/geometry/rgbdimage_factory.cu index 0dcec0ae..22bd0ff2 100644 --- a/src/cupoch/geometry/rgbdimage_factory.cu +++ b/src/cupoch/geometry/rgbdimage_factory.cu @@ -7,21 +7,19 @@ using namespace cupoch::geometry; namespace { struct convert_sun_format_functor { - convert_sun_format_functor(uint8_t* depth) : depth_(depth) {}; - uint8_t* depth_; - __device__ - void operator() (size_t idx) { - uint16_t &d = *(uint16_t*)(depth_ + idx * sizeof(uint16_t)); + convert_sun_format_functor(uint8_t *depth) : depth_(depth){}; + uint8_t *depth_; + __device__ void operator()(size_t idx) { + uint16_t &d = *(uint16_t *)(depth_ + idx * sizeof(uint16_t)); d = (d >> 3) | (d << 13); } }; struct convert_nyu_format_functor { - convert_nyu_format_functor(uint8_t* depth) : depth_(depth) {}; - uint8_t* depth_; - __device__ - void operator() (size_t idx) { - uint16_t *d = (uint16_t*)(depth_ + idx * sizeof(uint16_t)); + convert_nyu_format_functor(uint8_t *depth) : depth_(depth){}; + uint8_t *depth_; + __device__ void operator()(size_t idx) { + uint16_t *d = (uint16_t *)(depth_ + idx * sizeof(uint16_t)); uint8_t *p = (uint8_t *)d; uint8_t x = *p; *p = *(p + 1); @@ -35,7 +33,7 @@ struct convert_nyu_format_functor { } }; -} +} // namespace std::shared_ptr RGBDImage::CreateFromColorAndDepth( const Image &color, @@ -88,9 +86,12 @@ std::shared_ptr RGBDImage::CreateFromSUNFormat( } std::shared_ptr depth_t = std::make_shared(); *depth_t = depth; - convert_sun_format_functor func(thrust::raw_pointer_cast(depth_t->data_.data())); + convert_sun_format_functor func( + thrust::raw_pointer_cast(depth_t->data_.data())); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(depth_t->width_ * depth_t->height_), func); + thrust::make_counting_iterator(depth_t->width_ * + depth_t->height_), + func); // SUN depth map has long range depth. We set depth_trunc as 7.0 return CreateFromColorAndDepth(color, *depth_t, 1000.0, 7.0, convert_rgb_to_intensity); @@ -107,9 +108,12 @@ std::shared_ptr RGBDImage::CreateFromNYUFormat( } std::shared_ptr depth_t = std::make_shared(); *depth_t = depth; - convert_nyu_format_functor func(thrust::raw_pointer_cast(depth_t->data_.data())); + convert_nyu_format_functor func( + thrust::raw_pointer_cast(depth_t->data_.data())); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(depth_t->width_ * depth_t->height_), func); + thrust::make_counting_iterator(depth_t->width_ * + depth_t->height_), + func); // NYU depth map has long range depth. We set depth_trunc as 7.0 return CreateFromColorAndDepth(color, *depth_t, 1000.0, 7.0, convert_rgb_to_intensity); diff --git a/src/cupoch/geometry/trianglemesh.cu b/src/cupoch/geometry/trianglemesh.cu index 5766defb..2e424c87 100644 --- a/src/cupoch/geometry/trianglemesh.cu +++ b/src/cupoch/geometry/trianglemesh.cu @@ -1,21 +1,22 @@ -#include "cupoch/geometry/trianglemesh.h" -#include "cupoch/geometry/intersection_test.h" -#include "cupoch/utility/console.h" -#include "cupoch/utility/range.h" #include #include +#include "cupoch/geometry/intersection_test.h" +#include "cupoch/geometry/trianglemesh.h" +#include "cupoch/utility/console.h" +#include "cupoch/utility/range.h" + using namespace cupoch; using namespace cupoch::geometry; namespace { struct compute_triangle_normals_functor { - compute_triangle_normals_functor(const Eigen::Vector3f* vertices) : vertices_(vertices) {}; - const Eigen::Vector3f* vertices_; - __device__ - Eigen::Vector3f operator() (const Eigen::Vector3i& tri) const { + compute_triangle_normals_functor(const Eigen::Vector3f *vertices) + : vertices_(vertices){}; + const Eigen::Vector3f *vertices_; + __device__ Eigen::Vector3f operator()(const Eigen::Vector3i &tri) const { Eigen::Vector3f v01 = vertices_[tri(1)] - vertices_[tri(0)]; Eigen::Vector3f v02 = vertices_[tri(2)] - vertices_[tri(0)]; return v01.cross(v02); @@ -23,12 +24,11 @@ struct compute_triangle_normals_functor { }; struct compute_adjacency_matrix_functor { - compute_adjacency_matrix_functor(int* adjacency_matrix, size_t n_vertices) - : adjacency_matrix_(adjacency_matrix), n_vertices_(n_vertices) {}; - int* adjacency_matrix_; + compute_adjacency_matrix_functor(int *adjacency_matrix, size_t n_vertices) + : adjacency_matrix_(adjacency_matrix), n_vertices_(n_vertices){}; + int *adjacency_matrix_; size_t n_vertices_; - __device__ - void operator() (const Eigen::Vector3i& triangle) { + __device__ void operator()(const Eigen::Vector3i &triangle) { adjacency_matrix_[triangle(0) * n_vertices_ + triangle(1)] = 1; adjacency_matrix_[triangle(0) * n_vertices_ + triangle(2)] = 1; adjacency_matrix_[triangle(1) * n_vertices_ + triangle(0)] = 1; @@ -38,17 +38,17 @@ struct compute_adjacency_matrix_functor { } }; -struct check_self_intersecting_triangles{ - check_self_intersecting_triangles(const Eigen::Vector3i* triangles, - const Eigen::Vector3f* vertices, +struct check_self_intersecting_triangles { + check_self_intersecting_triangles(const Eigen::Vector3i *triangles, + const Eigen::Vector3f *vertices, int n_triangles) - : triangles_(triangles), vertices_(vertices), - n_triangles_(n_triangles) {}; - const Eigen::Vector3i* triangles_; - const Eigen::Vector3f* vertices_; + : triangles_(triangles), + vertices_(vertices), + n_triangles_(n_triangles){}; + const Eigen::Vector3i *triangles_; + const Eigen::Vector3f *vertices_; const int n_triangles_; - __device__ - Eigen::Vector2i operator() (size_t idx) const { + __device__ Eigen::Vector2i operator()(size_t idx) const { int tidx0 = idx / n_triangles_; int tidx1 = idx % n_triangles_; if (tidx0 >= tidx1 || tidx0 == n_triangles_ - 1) { @@ -78,25 +78,34 @@ struct check_self_intersecting_triangles{ } }; -} +} // namespace TriangleMesh::TriangleMesh() : MeshBase(Geometry::GeometryType::TriangleMesh) {} TriangleMesh::~TriangleMesh() {} -TriangleMesh::TriangleMesh(const utility::device_vector &vertices, - const utility::device_vector &triangles) - : MeshBase(Geometry::GeometryType::TriangleMesh, vertices), triangles_(triangles) {} +TriangleMesh::TriangleMesh( + const utility::device_vector &vertices, + const utility::device_vector &triangles) + : MeshBase(Geometry::GeometryType::TriangleMesh, vertices), + triangles_(triangles) {} -TriangleMesh::TriangleMesh(const thrust::host_vector &vertices, - const thrust::host_vector &triangles) - : MeshBase(Geometry::GeometryType::TriangleMesh, vertices), triangles_(triangles) {} +TriangleMesh::TriangleMesh( + const thrust::host_vector &vertices, + const thrust::host_vector &triangles) + : MeshBase(Geometry::GeometryType::TriangleMesh, vertices), + triangles_(triangles) {} -TriangleMesh::TriangleMesh(const geometry::TriangleMesh& other) - : MeshBase(Geometry::GeometryType::TriangleMesh, other.vertices_, other.vertex_normals_, other.vertex_colors_), - triangles_(other.triangles_), triangle_normals_(other.triangle_normals_), - adjacency_matrix_(other.adjacency_matrix_), triangle_uvs_(other.triangle_uvs_) {} +TriangleMesh::TriangleMesh(const geometry::TriangleMesh &other) + : MeshBase(Geometry::GeometryType::TriangleMesh, + other.vertices_, + other.vertex_normals_, + other.vertex_colors_), + triangles_(other.triangles_), + triangle_normals_(other.triangle_normals_), + adjacency_matrix_(other.adjacency_matrix_), + triangle_uvs_(other.triangle_uvs_) {} -TriangleMesh& TriangleMesh::operator=(const TriangleMesh& other) { +TriangleMesh &TriangleMesh::operator=(const TriangleMesh &other) { MeshBase::operator=(other); triangles_ = other.triangles_; triangle_normals_ = other.triangle_normals_; @@ -110,7 +119,8 @@ thrust::host_vector TriangleMesh::GetTriangles() const { return triangles; } -void TriangleMesh::SetTriangles(const thrust::host_vector& triangles) { +void TriangleMesh::SetTriangles( + const thrust::host_vector &triangles) { triangles_ = triangles; } @@ -119,7 +129,8 @@ thrust::host_vector TriangleMesh::GetTriangleNormals() const { return triangle_normals; } -void TriangleMesh::SetTriangleNormals(const thrust::host_vector& triangle_normals) { +void TriangleMesh::SetTriangleNormals( + const thrust::host_vector &triangle_normals) { triangle_normals_ = triangle_normals; } @@ -128,7 +139,8 @@ thrust::host_vector TriangleMesh::GetAdjacencyMatrix() const { return adjacency_matrix; } -void TriangleMesh::SetAdjacencyMatrix(const thrust::host_vector& adjacency_matrix) { +void TriangleMesh::SetAdjacencyMatrix( + const thrust::host_vector &adjacency_matrix) { adjacency_matrix_ = adjacency_matrix; } @@ -137,7 +149,8 @@ thrust::host_vector TriangleMesh::GetTriangleUVs() const { return triangle_uvs; } -void TriangleMesh::SetTriangleUVs(thrust::host_vector& triangle_uvs) { +void TriangleMesh::SetTriangleUVs( + thrust::host_vector &triangle_uvs) { triangle_uvs_ = triangle_uvs; } @@ -161,7 +174,8 @@ TriangleMesh &TriangleMesh::operator+=(const TriangleMesh &mesh) { if ((!HasTriangles() || HasTriangleNormals()) && mesh.HasTriangleNormals()) { triangle_normals_.resize(new_tri_num); - thrust::copy(mesh.triangle_normals_.begin(), mesh.triangle_normals_.end(), + thrust::copy(mesh.triangle_normals_.begin(), + mesh.triangle_normals_.end(), triangle_normals_.begin() + old_vert_num); } else { triangle_normals_.clear(); @@ -170,8 +184,11 @@ TriangleMesh &TriangleMesh::operator+=(const TriangleMesh &mesh) { triangles_.resize(triangles_.size() + mesh.triangles_.size()); Eigen::Vector3i index_shift((int)old_vert_num, (int)old_vert_num, (int)old_vert_num); - thrust::transform(mesh.triangles_.begin(), mesh.triangles_.end(), triangles_.begin() + n_tri_old, - [=] __device__ (const Eigen::Vector3i& tri) {return tri + index_shift;}); + thrust::transform(mesh.triangles_.begin(), mesh.triangles_.end(), + triangles_.begin() + n_tri_old, + [=] __device__(const Eigen::Vector3i &tri) { + return tri + index_shift; + }); if (HasAdjacencyMatrix()) { ComputeAdjacencyMatrix(); } @@ -190,11 +207,11 @@ TriangleMesh TriangleMesh::operator+(const TriangleMesh &mesh) const { TriangleMesh &TriangleMesh::NormalizeNormals() { MeshBase::NormalizeNormals(); thrust::for_each(triangle_normals_.begin(), triangle_normals_.end(), - [] __device__ (Eigen::Vector3f& nl) { + [] __device__(Eigen::Vector3f & nl) { nl.normalize(); - if (std::isnan(nl(0))) { - nl = Eigen::Vector3f(0.0, 0.0, 1.0); - } + if (isnan(nl(0))) { + nl = Eigen::Vector3f(0.0, 0.0, 1.0); + } }); return *this; } @@ -202,8 +219,10 @@ TriangleMesh &TriangleMesh::NormalizeNormals() { TriangleMesh &TriangleMesh::ComputeTriangleNormals( bool normalized /* = true*/) { triangle_normals_.resize(triangles_.size()); - compute_triangle_normals_functor func(thrust::raw_pointer_cast(vertices_.data())); - thrust::transform(triangles_.begin(), triangles_.end(), triangle_normals_.begin(), func); + compute_triangle_normals_functor func( + thrust::raw_pointer_cast(vertices_.data())); + thrust::transform(triangles_.begin(), triangles_.end(), + triangle_normals_.begin(), func); if (normalized) { NormalizeNormals(); } @@ -215,13 +234,18 @@ TriangleMesh &TriangleMesh::ComputeVertexNormals(bool normalized /* = true*/) { ComputeTriangleNormals(false); } vertex_normals_.resize(vertices_.size()); - thrust::repeated_range::iterator> range(triangle_normals_.begin(), triangle_normals_.end(), 3); - utility::device_vector nm_thrice(triangle_normals_.size() * 3); + thrust::repeated_range::iterator> + range(triangle_normals_.begin(), triangle_normals_.end(), 3); + utility::device_vector nm_thrice(triangle_normals_.size() * + 3); thrust::copy(range.begin(), range.end(), nm_thrice.begin()); - int* tri_ptr = (int*)(thrust::raw_pointer_cast(triangles_.data())); - thrust::sort_by_key(thrust::device, tri_ptr, tri_ptr + triangles_.size() * 3, nm_thrice.begin()); - auto end = thrust::reduce_by_key(thrust::device, tri_ptr, tri_ptr + triangles_.size() * 3, nm_thrice.begin(), - thrust::make_discard_iterator(), vertex_normals_.begin()); + int *tri_ptr = (int *)(thrust::raw_pointer_cast(triangles_.data())); + thrust::sort_by_key(thrust::device, tri_ptr, + tri_ptr + triangles_.size() * 3, nm_thrice.begin()); + auto end = thrust::reduce_by_key( + thrust::device, tri_ptr, tri_ptr + triangles_.size() * 3, + nm_thrice.begin(), thrust::make_discard_iterator(), + vertex_normals_.begin()); size_t n_out = thrust::distance(vertex_normals_.begin(), end.second); vertex_normals_.resize(n_out); if (normalized) { @@ -233,23 +257,29 @@ TriangleMesh &TriangleMesh::ComputeVertexNormals(bool normalized /* = true*/) { TriangleMesh &TriangleMesh::ComputeAdjacencyMatrix() { adjacency_matrix_.clear(); adjacency_matrix_.resize(vertices_.size() * vertices_.size(), 0); - compute_adjacency_matrix_functor func(thrust::raw_pointer_cast(adjacency_matrix_.data()), vertices_.size()); + compute_adjacency_matrix_functor func( + thrust::raw_pointer_cast(adjacency_matrix_.data()), + vertices_.size()); thrust::for_each(triangles_.begin(), triangles_.end(), func); return *this; } -utility::device_vector TriangleMesh::GetSelfIntersectingTriangles() - const { +utility::device_vector +TriangleMesh::GetSelfIntersectingTriangles() const { const size_t n_triangles2 = triangles_.size() * triangles_.size(); - utility::device_vector self_intersecting_triangles(n_triangles2); - check_self_intersecting_triangles func(thrust::raw_pointer_cast(triangles_.data()), - thrust::raw_pointer_cast(vertices_.data()), - triangles_.size()); + utility::device_vector self_intersecting_triangles( + n_triangles2); + check_self_intersecting_triangles func( + thrust::raw_pointer_cast(triangles_.data()), + thrust::raw_pointer_cast(vertices_.data()), triangles_.size()); thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_triangles2), self_intersecting_triangles.begin(), func); - auto end = thrust::remove_if(self_intersecting_triangles.begin(), self_intersecting_triangles.end(), - [] __device__ (const Eigen::Vector2i& idxs) {return idxs[0] < 0;}); - self_intersecting_triangles.resize(thrust::distance(self_intersecting_triangles.begin(), end)); + auto end = thrust::remove_if( + self_intersecting_triangles.begin(), + self_intersecting_triangles.end(), + [] __device__(const Eigen::Vector2i &idxs) { return idxs[0] < 0; }); + self_intersecting_triangles.resize( + thrust::distance(self_intersecting_triangles.begin(), end)); return self_intersecting_triangles; } \ No newline at end of file diff --git a/src/cupoch/geometry/trianglemesh.h b/src/cupoch/geometry/trianglemesh.h index 52399d66..d4692668 100644 --- a/src/cupoch/geometry/trianglemesh.h +++ b/src/cupoch/geometry/trianglemesh.h @@ -1,7 +1,6 @@ #pragma once -#include "cupoch/geometry/meshbase.h" #include "cupoch/geometry/image.h" - +#include "cupoch/geometry/meshbase.h" namespace cupoch { namespace geometry { @@ -13,45 +12,42 @@ class TriangleMesh : public MeshBase { const utility::device_vector &triangles); TriangleMesh(const thrust::host_vector &vertices, const thrust::host_vector &triangles); - TriangleMesh(const geometry::TriangleMesh& other); + TriangleMesh(const geometry::TriangleMesh &other); ~TriangleMesh() override; - TriangleMesh& operator=(const TriangleMesh& other); + TriangleMesh &operator=(const TriangleMesh &other); thrust::host_vector GetTriangles() const; - void SetTriangles(const thrust::host_vector& triangles); + void SetTriangles(const thrust::host_vector &triangles); thrust::host_vector GetTriangleNormals() const; - void SetTriangleNormals(const thrust::host_vector& triangle_normals); + void SetTriangleNormals( + const thrust::host_vector &triangle_normals); thrust::host_vector GetAdjacencyMatrix() const; - void SetAdjacencyMatrix(const thrust::host_vector& adjacency_matrix); + void SetAdjacencyMatrix(const thrust::host_vector &adjacency_matrix); thrust::host_vector GetTriangleUVs() const; - void SetTriangleUVs(thrust::host_vector& triangle_uvs); + void SetTriangleUVs(thrust::host_vector &triangle_uvs); public: virtual TriangleMesh &Clear() override; TriangleMesh &operator+=(const TriangleMesh &mesh); TriangleMesh operator+(const TriangleMesh &mesh) const; - __host__ __device__ - bool HasTriangles() const { + __host__ __device__ bool HasTriangles() const { return vertices_.size() > 0 && triangles_.size() > 0; } - __host__ __device__ - bool HasTriangleNormals() const { + __host__ __device__ bool HasTriangleNormals() const { return HasTriangles() && triangles_.size() == triangle_normals_.size(); } - __host__ __device__ - bool HasAdjacencyMatrix() const { + __host__ __device__ bool HasAdjacencyMatrix() const { return vertices_.size() > 0 && adjacency_matrix_.size() == vertices_.size() * vertices_.size(); } - __host__ __device__ - bool HasTriangleUvs() const { + __host__ __device__ bool HasTriangleUvs() const { return HasTriangles() && triangle_uvs_.size() == 3 * triangles_.size(); } @@ -65,12 +61,14 @@ class TriangleMesh : public MeshBase { /// Function to compute vertex normals, usually called before rendering TriangleMesh &ComputeVertexNormals(bool normalized = true); - /// Function to compute adjacency matrix, call before adjacency matrix is needed + /// Function to compute adjacency matrix, call before adjacency matrix is + /// needed TriangleMesh &ComputeAdjacencyMatrix(); /// Function that returns a list of triangles that are intersecting the /// mesh. - utility::device_vector GetSelfIntersectingTriangles() const; + utility::device_vector GetSelfIntersectingTriangles() + const; /// Factory function to create a tetrahedron mesh (trianglemeshfactory.cpp). /// the mesh centroid will be at (0,0,0) and \param radius defines the @@ -103,7 +101,6 @@ class TriangleMesh : public MeshBase { static std::shared_ptr CreateSphere(float radius = 1.0, int resolution = 20); - /// Factory function to create a cylinder mesh (TriangleMeshFactory.cpp) /// The axis of the cylinder will be from (0, 0, -height/2) to (0, 0, /// height/2). The circle with \param radius will be split into \param @@ -158,5 +155,5 @@ class TriangleMesh : public MeshBase { Image texture_; }; -} -} \ No newline at end of file +} // namespace geometry +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/geometry/trianglemesh_factory.cu b/src/cupoch/geometry/trianglemesh_factory.cu index e6cfcd2f..ae18bf6b 100644 --- a/src/cupoch/geometry/trianglemesh_factory.cu +++ b/src/cupoch/geometry/trianglemesh_factory.cu @@ -8,30 +8,30 @@ namespace { struct compute_sphere_vertices_functor { compute_sphere_vertices_functor(int resolution, float radius) - : resolution_(resolution), radius_(radius) {step_ = M_PI / (float)resolution;}; + : resolution_(resolution), radius_(radius) { + step_ = M_PI / (float)resolution; + }; const int resolution_; const float radius_; float step_; - __device__ - Eigen::Vector3f operator() (size_t idx) const { + __device__ Eigen::Vector3f operator()(size_t idx) const { int i = idx / (2 * resolution_) + 1; int j = idx % (2 * resolution_); float alpha = step_ * i; float theta = step_ * j; - return Eigen::Vector3f(sin(alpha) * cos(theta), - sin(alpha) * sin(theta), - cos(alpha)) * radius_; + return Eigen::Vector3f(sin(alpha) * cos(theta), sin(alpha) * sin(theta), + cos(alpha)) * + radius_; } }; struct compute_sphere_triangles_functor1 { - compute_sphere_triangles_functor1(Eigen::Vector3i* triangle, int resolution) - : triangles_(triangle), resolution_(resolution) {}; - Eigen::Vector3i* triangles_; + compute_sphere_triangles_functor1(Eigen::Vector3i *triangle, int resolution) + : triangles_(triangle), resolution_(resolution){}; + Eigen::Vector3i *triangles_; const int resolution_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { int j1 = (idx + 1) % (2 * resolution_); int base = 2; triangles_[2 * idx] = Eigen::Vector3i(0, base + idx, base + j1); @@ -41,32 +41,39 @@ struct compute_sphere_triangles_functor1 { }; struct compute_sphere_triangles_functor2 { - compute_sphere_triangles_functor2(Eigen::Vector3i* triangle, int resolution) - : triangles_(triangle), resolution_(resolution) {}; - Eigen::Vector3i* triangles_; + compute_sphere_triangles_functor2(Eigen::Vector3i *triangle, int resolution) + : triangles_(triangle), resolution_(resolution){}; + Eigen::Vector3i *triangles_; const int resolution_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { int i = idx / (2 * resolution_) + 1; int j = idx % (2 * resolution_); int base1 = 2 + 2 * resolution_ * (i - 1); int base2 = base1 + 2 * resolution_; int j1 = (j + 1) % (2 * resolution_); triangles_[2 * idx] = Eigen::Vector3i(base2 + j, base1 + j1, base1 + j); - triangles_[2 * idx + 1] = Eigen::Vector3i(base2 + j, base2 + j1, base1 + j1); + triangles_[2 * idx + 1] = + Eigen::Vector3i(base2 + j, base2 + j1, base1 + j1); } }; struct compute_cylinder_vertices_functor { - compute_cylinder_vertices_functor(int resolution, float radius, float height, float step, float h_step) - : resolution_(resolution), radius_(radius), height_(height), step_(step), h_step_(h_step) {}; + compute_cylinder_vertices_functor(int resolution, + float radius, + float height, + float step, + float h_step) + : resolution_(resolution), + radius_(radius), + height_(height), + step_(step), + h_step_(h_step){}; const int resolution_; const float radius_; const float height_; const float step_; const float h_step_; - __device__ - Eigen::Vector3f operator() (size_t idx) const { + __device__ Eigen::Vector3f operator()(size_t idx) const { int i = idx / resolution_; int j = idx % resolution_; float theta = step_ * j; @@ -76,13 +83,14 @@ struct compute_cylinder_vertices_functor { }; struct compute_cylinder_triangles_functor1 { - compute_cylinder_triangles_functor1(Eigen::Vector3i* triangle, int resolution, int split) - : triangles_(triangle), resolution_(resolution), split_(split) {}; - Eigen::Vector3i* triangles_; + compute_cylinder_triangles_functor1(Eigen::Vector3i *triangle, + int resolution, + int split) + : triangles_(triangle), resolution_(resolution), split_(split){}; + Eigen::Vector3i *triangles_; const int resolution_; const int split_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { int j1 = (idx + 1) % resolution_; int base = 2; triangles_[2 * idx] = Eigen::Vector3i(0, base + idx, base + j1); @@ -92,48 +100,54 @@ struct compute_cylinder_triangles_functor1 { }; struct compute_cylinder_triangles_functor2 { - compute_cylinder_triangles_functor2(Eigen::Vector3i* triangle, int resolution) - : triangles_(triangle), resolution_(resolution) {}; - Eigen::Vector3i* triangles_; + compute_cylinder_triangles_functor2(Eigen::Vector3i *triangle, + int resolution) + : triangles_(triangle), resolution_(resolution){}; + Eigen::Vector3i *triangles_; const int resolution_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { int i = idx / resolution_; int j = idx % resolution_; int base1 = 2 + resolution_ * i; int base2 = base1 + resolution_; int j1 = (j + 1) % resolution_; triangles_[2 * idx] = Eigen::Vector3i(base2 + j, base1 + j1, base1 + j); - triangles_[2 * idx + 1] = Eigen::Vector3i(base2 + j, base2 + j1, base1 + j1); + triangles_[2 * idx + 1] = + Eigen::Vector3i(base2 + j, base2 + j1, base1 + j1); } }; struct compute_cone_vertices_functor { - compute_cone_vertices_functor(int resolution, int split, float step, float r_step, float h_step) - : resolution_(resolution), split_(split), step_(step), r_step_(r_step), h_step_(h_step) {}; + compute_cone_vertices_functor( + int resolution, int split, float step, float r_step, float h_step) + : resolution_(resolution), + split_(split), + step_(step), + r_step_(r_step), + h_step_(h_step){}; const int resolution_; const int split_; const float step_; const float r_step_; const float h_step_; - __device__ - Eigen::Vector3f operator() (size_t idx) const { + __device__ Eigen::Vector3f operator()(size_t idx) const { int i = idx / resolution_; int j = idx % resolution_; float r = r_step_ * (split_ - i); float theta = step_ * j; return Eigen::Vector3f(cos(theta) * r, sin(theta) * r, h_step_ * i); - } + } }; struct compute_cone_triangles_functor1 { - compute_cone_triangles_functor1(Eigen::Vector3i* triangle, int resolution, int split) - : triangles_(triangle), resolution_(resolution), split_(split) {}; - Eigen::Vector3i* triangles_; + compute_cone_triangles_functor1(Eigen::Vector3i *triangle, + int resolution, + int split) + : triangles_(triangle), resolution_(resolution), split_(split){}; + Eigen::Vector3i *triangles_; const int resolution_; const int split_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { int j1 = (idx + 1) % resolution_; int base = 2; triangles_[2 * idx] = Eigen::Vector3i(0, base + j1, base + idx); @@ -143,23 +157,24 @@ struct compute_cone_triangles_functor1 { }; struct compute_cone_triangles_functor2 { - compute_cone_triangles_functor2(Eigen::Vector3i* triangle, int resolution) - : triangles_(triangle), resolution_(resolution) {}; - Eigen::Vector3i* triangles_; + compute_cone_triangles_functor2(Eigen::Vector3i *triangle, int resolution) + : triangles_(triangle), resolution_(resolution){}; + Eigen::Vector3i *triangles_; const int resolution_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { int i = idx / resolution_; int j = idx % resolution_; int base1 = 2 + resolution_ * i; int base2 = base1 + resolution_; int j1 = (j + 1) % resolution_; - triangles_[2 * idx] = Eigen::Vector3i(base2 + j1, base1 + j, base1 + j1); - triangles_[2 * idx + 1] = Eigen::Vector3i(base2 + j1, base2 + j, base1 + j); + triangles_[2 * idx] = + Eigen::Vector3i(base2 + j1, base1 + j, base1 + j1); + triangles_[2 * idx + 1] = + Eigen::Vector3i(base2 + j1, base2 + j, base1 + j); } }; -} +} // namespace std::shared_ptr TriangleMesh::CreateTetrahedron( float radius /* = 1.0*/) { @@ -302,14 +317,21 @@ std::shared_ptr TriangleMesh::CreateSphere( thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_vertices - 2), mesh_ptr->vertices_.begin() + 2, func_vt); - mesh_ptr->triangles_.resize(2 * resolution + 4 * (resolution - 2) * resolution); - compute_sphere_triangles_functor1 func_tr1(thrust::raw_pointer_cast(mesh_ptr->triangles_.data()), resolution); + mesh_ptr->triangles_.resize(2 * resolution + + 4 * (resolution - 2) * resolution); + compute_sphere_triangles_functor1 func_tr1( + thrust::raw_pointer_cast(mesh_ptr->triangles_.data()), resolution); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(2 * resolution), func_tr1); - compute_sphere_triangles_functor2 func_tr2(thrust::raw_pointer_cast(mesh_ptr->triangles_.data()) + 2 * resolution, - resolution); + thrust::make_counting_iterator(2 * resolution), + func_tr1); + compute_sphere_triangles_functor2 func_tr2( + thrust::raw_pointer_cast(mesh_ptr->triangles_.data()) + + 2 * resolution, + resolution); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(2 * (resolution - 1) * resolution), func_tr2); + thrust::make_counting_iterator( + 2 * (resolution - 1) * resolution), + func_tr2); return mesh_ptr; } @@ -337,15 +359,23 @@ std::shared_ptr TriangleMesh::CreateCylinder( mesh_ptr->vertices_[1] = Eigen::Vector3f(0.0, 0.0, -height * 0.5); float step = M_PI * 2.0 / (float)resolution; float h_step = height / (float)split; - compute_cylinder_vertices_functor func_vt(resolution, radius, height, step, h_step); + compute_cylinder_vertices_functor func_vt(resolution, radius, height, step, + h_step); thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_vertices - 2), mesh_ptr->vertices_.begin() + 2, func_vt); mesh_ptr->triangles_.resize(resolution + split * resolution); - compute_cylinder_triangles_functor1 func_tr1(thrust::raw_pointer_cast(mesh_ptr->triangles_.data()), resolution, split); - for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(resolution), func_tr1); - compute_cylinder_triangles_functor2 func_tr2(thrust::raw_pointer_cast(mesh_ptr->triangles_.data()) + resolution, resolution); - for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(resolution * split), func_tr2); + compute_cylinder_triangles_functor1 func_tr1( + thrust::raw_pointer_cast(mesh_ptr->triangles_.data()), resolution, + split); + for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(resolution), func_tr1); + compute_cylinder_triangles_functor2 func_tr2( + thrust::raw_pointer_cast(mesh_ptr->triangles_.data()) + resolution, + resolution); + for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(resolution * split), + func_tr2); return mesh_ptr; } @@ -372,16 +402,26 @@ std::shared_ptr TriangleMesh::CreateCone(float radius /* = 1.0*/, float step = M_PI * 2.0 / (float)resolution; float h_step = height / (float)split; float r_step = radius / (float)split; - compute_cone_vertices_functor func_vt(resolution, split, step, r_step, h_step); - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(resolution * split), - mesh_ptr->vertices_.begin() + 2, func_vt); + compute_cone_vertices_functor func_vt(resolution, split, step, r_step, + h_step); + thrust::transform( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(resolution * split), + mesh_ptr->vertices_.begin() + 2, func_vt); mesh_ptr->triangles_.resize(resolution + (split - 1) * resolution); - compute_cone_triangles_functor1 func_tr1(thrust::raw_pointer_cast(mesh_ptr->triangles_.data()), resolution, split); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(resolution), func_tr1); - compute_cone_triangles_functor2 func_tr2(thrust::raw_pointer_cast(mesh_ptr->triangles_.data()) + resolution, - resolution); + compute_cone_triangles_functor1 func_tr1( + thrust::raw_pointer_cast(mesh_ptr->triangles_.data()), resolution, + split); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator((split - 1) * resolution), func_tr2); + thrust::make_counting_iterator(resolution), + func_tr1); + compute_cone_triangles_functor2 func_tr2( + thrust::raw_pointer_cast(mesh_ptr->triangles_.data()) + resolution, + resolution); + thrust::for_each( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator((split - 1) * resolution), + func_tr2); return mesh_ptr; } diff --git a/src/cupoch/geometry/voxelgrid.cu b/src/cupoch/geometry/voxelgrid.cu index 51a9cda8..7ede7d0e 100644 --- a/src/cupoch/geometry/voxelgrid.cu +++ b/src/cupoch/geometry/voxelgrid.cu @@ -1,9 +1,9 @@ -#include "cupoch/geometry/voxelgrid.h" +#include #include "cupoch/camera/pinhole_camera_parameters.h" #include "cupoch/geometry/boundingvolume.h" #include "cupoch/geometry/image.h" -#include +#include "cupoch/geometry/voxelgrid.h" using namespace cupoch; using namespace cupoch::geometry; @@ -11,41 +11,42 @@ using namespace cupoch::geometry; namespace { struct elementwise_min_functor { - __device__ - Eigen::Vector3i operator()(const Eigen::Vector3i& a, const Eigen::Vector3i& b) { + __device__ Eigen::Vector3i operator()(const Eigen::Vector3i &a, + const Eigen::Vector3i &b) { return a.array().min(b.array()).matrix(); } }; struct elementwise_max_functor { - __device__ - Eigen::Vector3i operator()(const Eigen::Vector3i& a, const Eigen::Vector3i& b) { + __device__ Eigen::Vector3i operator()(const Eigen::Vector3i &a, + const Eigen::Vector3i &b) { return a.array().max(b.array()).matrix(); } }; struct compute_center_functor { - compute_center_functor(float voxel_size, const Eigen::Vector3f& origin, const Eigen::Vector3f& half_voxel_size) - : voxel_size_(voxel_size), origin_(origin), half_voxel_size_(half_voxel_size) {}; + compute_center_functor(float voxel_size, + const Eigen::Vector3f &origin, + const Eigen::Vector3f &half_voxel_size) + : voxel_size_(voxel_size), + origin_(origin), + half_voxel_size_(half_voxel_size){}; const float voxel_size_; const Eigen::Vector3f origin_; const Eigen::Vector3f half_voxel_size_; - __device__ - Eigen::Vector3f operator()(const Eigen::Vector3i& x) const { + __device__ Eigen::Vector3f operator()(const Eigen::Vector3i &x) const { return x.cast() * voxel_size_ + origin_ + half_voxel_size_; } }; struct extract_grid_index_functor { - __device__ - Eigen::Vector3i operator() (const Voxel& voxel) const { + __device__ Eigen::Vector3i operator()(const Voxel &voxel) const { return voxel.grid_index_; } }; struct add_voxel_color_functor { - __device__ - Voxel operator() (const Voxel& x, const Voxel& y) const { + __device__ Voxel operator()(const Voxel &x, const Voxel &y) const { Voxel ans; ans.grid_index_ = x.grid_index_; ans.color_ = x.color_ + y.color_; @@ -54,8 +55,7 @@ struct add_voxel_color_functor { }; struct devide_voxel_color_functor { - __device__ - Voxel operator() (const Voxel& x, int y) const { + __device__ Voxel operator()(const Voxel &x, int y) const { Voxel ans; ans.grid_index_ = x.grid_index_; ans.color_ = x.color_ / y; @@ -63,9 +63,9 @@ struct devide_voxel_color_functor { } }; -__host__ __device__ -void GetVoxelBoundingPoints(const Eigen::Vector3f& x, float r, - Eigen::Vector3f points[8]) { +__host__ __device__ void GetVoxelBoundingPoints(const Eigen::Vector3f &x, + float r, + Eigen::Vector3f points[8]) { points[0] = x + Eigen::Vector3f(-r, -r, -r); points[1] = x + Eigen::Vector3f(-r, -r, r); points[2] = x + Eigen::Vector3f(r, -r, -r); @@ -77,18 +77,29 @@ void GetVoxelBoundingPoints(const Eigen::Vector3f& x, float r, } struct compute_carve_functor { - compute_carve_functor(const uint8_t* image, int width, int height, - int num_of_channels, int bytes_per_channel, - float voxel_size, const Eigen::Vector3f& origin, - const Eigen::Matrix3f& intrinsic, - const Eigen::Matrix3f& rot, const Eigen::Vector3f& trans, + compute_carve_functor(const uint8_t *image, + int width, + int height, + int num_of_channels, + int bytes_per_channel, + float voxel_size, + const Eigen::Vector3f &origin, + const Eigen::Matrix3f &intrinsic, + const Eigen::Matrix3f &rot, + const Eigen::Vector3f &trans, bool keep_voxels_outside_image) - : image_(image), width_(width), height_(height), - num_of_channels_(num_of_channels), bytes_per_channel_(bytes_per_channel), - voxel_size_(voxel_size), origin_(origin), - intrinsic_(intrinsic), rot_(rot), trans_(trans), - keep_voxels_outside_image_(keep_voxels_outside_image) {}; - const uint8_t* image_; + : image_(image), + width_(width), + height_(height), + num_of_channels_(num_of_channels), + bytes_per_channel_(bytes_per_channel), + voxel_size_(voxel_size), + origin_(origin), + intrinsic_(intrinsic), + rot_(rot), + trans_(trans), + keep_voxels_outside_image_(keep_voxels_outside_image){}; + const uint8_t *image_; const int width_; const int height_; const int num_of_channels_; @@ -99,12 +110,15 @@ struct compute_carve_functor { const Eigen::Matrix3f rot_; const Eigen::Vector3f trans_; bool keep_voxels_outside_image_; - __device__ - bool operator() (const thrust::tuple& voxel) const { + __device__ bool operator()( + const thrust::tuple &voxel) const { bool carve = true; float r = voxel_size_ / 2.0; Voxel v = thrust::get<1>(voxel); - auto x = ((v.grid_index_.cast() + Eigen::Vector3f(0.5, 0.5, 0.5)) * voxel_size_) + origin_; + auto x = ((v.grid_index_.cast() + + Eigen::Vector3f(0.5, 0.5, 0.5)) * + voxel_size_) + + origin_; Eigen::Vector3f pts[8]; GetVoxelBoundingPoints(x, r, pts); for (int i = 0; i < 8; ++i) { @@ -115,9 +129,9 @@ struct compute_carve_functor { float v = uvz(1) / z; float d; bool within_boundary; - thrust::tie(within_boundary, d) = FloatValueAt(image_, - u, v, width_, height_, - num_of_channels_, bytes_per_channel_); + thrust::tie(within_boundary, d) = + FloatValueAt(image_, u, v, width_, height_, + num_of_channels_, bytes_per_channel_); if ((!within_boundary && keep_voxels_outside_image_) || (within_boundary && d > 0 && z >= d)) { carve = false; @@ -128,7 +142,7 @@ struct compute_carve_functor { } }; -} +} // namespace VoxelGrid::VoxelGrid() : Geometry3D(Geometry::GeometryType::VoxelGrid) {} VoxelGrid::~VoxelGrid() {} @@ -156,9 +170,12 @@ Eigen::Vector3f VoxelGrid::GetMinBound() const { } else { Voxel v = voxels_values_[0]; Eigen::Vector3i init = v.grid_index_; - Eigen::Vector3i min_grid_index = thrust::reduce(thrust::make_transform_iterator(voxels_values_.begin(), extract_grid_index_functor()), - thrust::make_transform_iterator(voxels_values_.end(), extract_grid_index_functor()), - init, elementwise_min_functor()); + Eigen::Vector3i min_grid_index = thrust::reduce( + thrust::make_transform_iterator(voxels_values_.begin(), + extract_grid_index_functor()), + thrust::make_transform_iterator(voxels_values_.end(), + extract_grid_index_functor()), + init, elementwise_min_functor()); return min_grid_index.cast() * voxel_size_ + origin_; } } @@ -169,10 +186,15 @@ Eigen::Vector3f VoxelGrid::GetMaxBound() const { } else { Voxel v = voxels_values_[0]; Eigen::Vector3i init = v.grid_index_; - Eigen::Vector3i min_grid_index = thrust::reduce(thrust::make_transform_iterator(voxels_values_.begin(), extract_grid_index_functor()), - thrust::make_transform_iterator(voxels_values_.end(), extract_grid_index_functor()), - init, elementwise_max_functor()); - return (min_grid_index.cast() + Eigen::Vector3f::Ones()) * voxel_size_ + origin_; + Eigen::Vector3i min_grid_index = thrust::reduce( + thrust::make_transform_iterator(voxels_values_.begin(), + extract_grid_index_functor()), + thrust::make_transform_iterator(voxels_values_.end(), + extract_grid_index_functor()), + init, elementwise_max_functor()); + return (min_grid_index.cast() + Eigen::Vector3f::Ones()) * + voxel_size_ + + origin_; } } @@ -184,9 +206,12 @@ Eigen::Vector3f VoxelGrid::GetCenter() const { const Eigen::Vector3f half_voxel_size(0.5 * voxel_size_, 0.5 * voxel_size_, 0.5 * voxel_size_); compute_center_functor func(voxel_size_, origin_, half_voxel_size); - Eigen::Vector3f center = thrust::transform_reduce(thrust::make_transform_iterator(voxels_values_.begin(), extract_grid_index_functor()), - thrust::make_transform_iterator(voxels_values_.end(), extract_grid_index_functor()), - func, init, thrust::plus()); + Eigen::Vector3f center = thrust::transform_reduce( + thrust::make_transform_iterator(voxels_values_.begin(), + extract_grid_index_functor()), + thrust::make_transform_iterator(voxels_values_.end(), + extract_grid_index_functor()), + func, init, thrust::plus()); center /= float(voxels_values_.size()); return center; } @@ -244,21 +269,26 @@ VoxelGrid &VoxelGrid::operator+=(const VoxelGrid &voxelgrid) { "the other not."); } if (voxelgrid.HasColors()) { - voxels_keys_.insert(voxels_keys_.end(), voxelgrid.voxels_keys_.begin(), voxelgrid.voxels_keys_.end()); - voxels_values_.insert(voxels_values_.end(), voxelgrid.voxels_values_.begin(), voxelgrid.voxels_values_.end()); - thrust::sort_by_key(voxels_keys_.begin(), voxels_keys_.end(), voxels_values_.begin()); + voxels_keys_.insert(voxels_keys_.end(), voxelgrid.voxels_keys_.begin(), + voxelgrid.voxels_keys_.end()); + voxels_values_.insert(voxels_values_.end(), + voxelgrid.voxels_values_.begin(), + voxelgrid.voxels_values_.end()); + thrust::sort_by_key(voxels_keys_.begin(), voxels_keys_.end(), + voxels_values_.begin()); utility::device_vector counts(voxels_keys_.size()); utility::device_vector new_keys(voxels_keys_.size()); - auto end1 = thrust::reduce_by_key(voxels_keys_.begin(), voxels_keys_.end(), - thrust::make_constant_iterator(1), - thrust::make_discard_iterator(), counts.begin()); + auto end1 = thrust::reduce_by_key( + voxels_keys_.begin(), voxels_keys_.end(), + thrust::make_constant_iterator(1), + thrust::make_discard_iterator(), counts.begin()); int n_out = thrust::distance(counts.begin(), end1.second); counts.resize(n_out); - auto end2 = thrust::reduce_by_key(voxels_keys_.begin(), voxels_keys_.end(), - voxels_values_.begin(), new_keys.begin(), - voxels_values_.begin(), - thrust::equal_to(), - add_voxel_color_functor()); + auto end2 = thrust::reduce_by_key( + voxels_keys_.begin(), voxels_keys_.end(), + voxels_values_.begin(), new_keys.begin(), + voxels_values_.begin(), thrust::equal_to(), + add_voxel_color_functor()); new_keys.resize(n_out); voxels_keys_ = new_keys; voxels_values_.resize(n_out); @@ -278,8 +308,10 @@ VoxelGrid VoxelGrid::operator+(const VoxelGrid &voxelgrid) const { void VoxelGrid::AddVoxel(const Voxel &voxel) { voxels_keys_.push_back(voxel.grid_index_); voxels_values_.push_back(voxel); - thrust::sort_by_key(voxels_keys_.begin(), voxels_keys_.end(), voxels_values_.begin()); - auto end = thrust::unique_by_key(voxels_keys_.begin(), voxels_keys_.end(), voxels_values_.begin()); + thrust::sort_by_key(voxels_keys_.begin(), voxels_keys_.end(), + voxels_values_.begin()); + auto end = thrust::unique_by_key(voxels_keys_.begin(), voxels_keys_.end(), + voxels_values_.begin()); size_t out_size = thrust::distance(voxels_keys_.begin(), end.first); voxels_keys_.resize(out_size); voxels_values_.resize(out_size); @@ -287,11 +319,15 @@ void VoxelGrid::AddVoxel(const Voxel &voxel) { void VoxelGrid::AddVoxels(const utility::device_vector &voxels) { voxels_keys_.insert(voxels_keys_.end(), - thrust::make_transform_iterator(voxels.begin(), extract_grid_index_functor()), - thrust::make_transform_iterator(voxels.end(), extract_grid_index_functor())); + thrust::make_transform_iterator( + voxels.begin(), extract_grid_index_functor()), + thrust::make_transform_iterator( + voxels.end(), extract_grid_index_functor())); voxels_values_.insert(voxels_values_.end(), voxels.begin(), voxels.end()); - thrust::sort_by_key(voxels_keys_.begin(), voxels_keys_.end(), voxels_values_.begin()); - auto end = thrust::unique_by_key(voxels_keys_.begin(), voxels_keys_.end(), voxels_values_.begin()); + thrust::sort_by_key(voxels_keys_.begin(), voxels_keys_.end(), + voxels_values_.begin()); + auto end = thrust::unique_by_key(voxels_keys_.begin(), voxels_keys_.end(), + voxels_values_.begin()); size_t out_size = thrust::distance(voxels_keys_.begin(), end.first); voxels_keys_.resize(out_size); voxels_values_.resize(out_size); @@ -302,12 +338,12 @@ Eigen::Vector3i VoxelGrid::GetVoxel(const Eigen::Vector3f &point) const { return (Eigen::floor(voxel_f.array())).cast(); } -Eigen::Vector3f VoxelGrid::GetVoxelCenterCoordinate(const Eigen::Vector3i &idx) const { +Eigen::Vector3f VoxelGrid::GetVoxelCenterCoordinate( + const Eigen::Vector3i &idx) const { auto it = thrust::find(voxels_keys_.begin(), voxels_keys_.end(), idx); if (it != voxels_keys_.end()) { Eigen::Vector3i voxel_idx = *it; - return ((voxel_idx.cast() + - Eigen::Vector3f(0.5, 0.5, 0.5)) * + return ((voxel_idx.cast() + Eigen::Vector3f(0.5, 0.5, 0.5)) * voxel_size_) + origin_; } else { @@ -330,7 +366,8 @@ thrust::host_vector VoxelGrid::CheckIfIncluded( output.resize(queries.size()); for (size_t i = 0; i < queries.size(); ++i) { auto query = GetVoxel(queries[i]); - auto itr = thrust::find(voxels_keys_.begin(), voxels_keys_.end(), query); + auto itr = + thrust::find(voxels_keys_.begin(), voxels_keys_.end(), query); output[i] = (itr != voxels_keys_.end()); } return output; @@ -353,13 +390,17 @@ VoxelGrid &VoxelGrid::CarveDepthMap( // get for each voxel if it projects to a valid pixel and check if the voxel // depth is behind the depth of the depth map at the projected pixel. - compute_carve_functor func(thrust::raw_pointer_cast(depth_map.data_.data()), - depth_map.width_, depth_map.height_, - depth_map.num_of_channels_, depth_map.bytes_per_channel_, - voxel_size_, origin_, - intrinsic, rot, trans, keep_voxels_outside_image); - auto begin = make_tuple_iterator(voxels_keys_.begin(), voxels_values_.begin()); - auto end = thrust::remove_if(begin, make_tuple_iterator(voxels_keys_.end(), voxels_values_.end()), func); + compute_carve_functor func( + thrust::raw_pointer_cast(depth_map.data_.data()), depth_map.width_, + depth_map.height_, depth_map.num_of_channels_, + depth_map.bytes_per_channel_, voxel_size_, origin_, intrinsic, rot, + trans, keep_voxels_outside_image); + auto begin = + make_tuple_iterator(voxels_keys_.begin(), voxels_values_.begin()); + auto end = thrust::remove_if( + begin, + make_tuple_iterator(voxels_keys_.end(), voxels_values_.end()), + func); size_t out_size = thrust::distance(begin, end); voxels_keys_.resize(out_size); voxels_values_.resize(out_size); @@ -383,13 +424,18 @@ VoxelGrid &VoxelGrid::CarveSilhouette( // get for each voxel if it projects to a valid pixel and check if the pixel // is set (>0). - compute_carve_functor func(thrust::raw_pointer_cast(silhouette_mask.data_.data()), - silhouette_mask.width_, silhouette_mask.height_, - silhouette_mask.num_of_channels_, silhouette_mask.bytes_per_channel_, - voxel_size_, origin_, - intrinsic, rot, trans, keep_voxels_outside_image); - auto begin = make_tuple_iterator(voxels_keys_.begin(), voxels_values_.begin()); - auto end = thrust::remove_if(begin, make_tuple_iterator(voxels_keys_.end(), voxels_values_.end()), func); + compute_carve_functor func( + thrust::raw_pointer_cast(silhouette_mask.data_.data()), + silhouette_mask.width_, silhouette_mask.height_, + silhouette_mask.num_of_channels_, + silhouette_mask.bytes_per_channel_, voxel_size_, origin_, intrinsic, + rot, trans, keep_voxels_outside_image); + auto begin = + make_tuple_iterator(voxels_keys_.begin(), voxels_values_.begin()); + auto end = thrust::remove_if( + begin, + make_tuple_iterator(voxels_keys_.end(), voxels_values_.end()), + func); size_t out_size = thrust::distance(begin, end); voxels_keys_.resize(out_size); voxels_values_.resize(out_size); diff --git a/src/cupoch/geometry/voxelgrid.h b/src/cupoch/geometry/voxelgrid.h index af58d27c..e34ed30b 100644 --- a/src/cupoch/geometry/voxelgrid.h +++ b/src/cupoch/geometry/voxelgrid.h @@ -1,13 +1,13 @@ #pragma once +#include + #include #include #include "cupoch/geometry/geometry3d.h" #include "cupoch/utility/console.h" - #include "cupoch/utility/helper.h" -#include namespace cupoch { @@ -23,15 +23,13 @@ class Image; class Voxel { public: - __host__ __device__ - Voxel() {} - __host__ __device__ - Voxel(const Eigen::Vector3i &grid_index) : grid_index_(grid_index) {} - __host__ __device__ - Voxel(const Eigen::Vector3i &grid_index, const Eigen::Vector3f &color) + __host__ __device__ Voxel() {} + __host__ __device__ Voxel(const Eigen::Vector3i &grid_index) + : grid_index_(grid_index) {} + __host__ __device__ Voxel(const Eigen::Vector3i &grid_index, + const Eigen::Vector3f &color) : grid_index_(grid_index), color_(color) {} - __host__ __device__ - ~Voxel() {} + __host__ __device__ ~Voxel() {} public: Eigen::Vector3i grid_index_ = Eigen::Vector3i(0, 0, 0); diff --git a/src/cupoch/geometry/voxelgrid_factory.cu b/src/cupoch/geometry/voxelgrid_factory.cu index 2714325e..86613fcb 100644 --- a/src/cupoch/geometry/voxelgrid_factory.cu +++ b/src/cupoch/geometry/voxelgrid_factory.cu @@ -1,3 +1,5 @@ +#include + #include #include "cupoch/geometry/intersection_test.h" @@ -6,7 +8,6 @@ #include "cupoch/geometry/voxelgrid.h" #include "cupoch/utility/console.h" #include "cupoch/utility/helper.h" -#include using namespace cupoch; using namespace cupoch::geometry; @@ -14,11 +15,11 @@ using namespace cupoch::geometry; namespace { struct create_dense_functor { - create_dense_functor(int num_w, int num_h) : num_w_(num_w), num_h_(num_h) {}; + create_dense_functor(int num_w, int num_h) : num_w_(num_w), num_h_(num_h){}; const int num_w_; const int num_h_; - __device__ - thrust::tuple operator() (size_t idx) const { + __device__ thrust::tuple operator()( + size_t idx) const { int whidx = idx / (num_w_ * num_h_); int widx = whidx / num_h_; int hidx = whidx % num_h_; @@ -29,8 +30,7 @@ struct create_dense_functor { }; struct add_voxel_color_functor { - __device__ - Voxel operator() (const Voxel& x, const Voxel& y) const { + __device__ Voxel operator()(const Voxel &x, const Voxel &y) const { Voxel ans; ans.grid_index_ = x.grid_index_; ans.color_ = x.color_ + y.color_; @@ -39,8 +39,7 @@ struct add_voxel_color_functor { }; struct devide_voxel_color_functor { - __device__ - Voxel operator() (const Voxel& x, int y) const { + __device__ Voxel operator()(const Voxel &x, int y) const { Voxel ans; ans.grid_index_ = x.grid_index_; ans.color_ = x.color_ / y; @@ -49,67 +48,83 @@ struct devide_voxel_color_functor { }; struct create_from_pointcloud_functor { - create_from_pointcloud_functor(const Eigen::Vector3f& min_bound, float voxel_size, bool has_colors) - : min_bound_(min_bound), voxel_size_(voxel_size), has_colors_(has_colors) {}; + create_from_pointcloud_functor(const Eigen::Vector3f &min_bound, + float voxel_size, + bool has_colors) + : min_bound_(min_bound), + voxel_size_(voxel_size), + has_colors_(has_colors){}; const Eigen::Vector3f min_bound_; const float voxel_size_; const bool has_colors_; - __device__ - thrust::tuple operator() (const Eigen::Vector3f& point, const Eigen::Vector3f& color) const { + __device__ thrust::tuple operator()( + const Eigen::Vector3f &point, const Eigen::Vector3f &color) const { Eigen::Vector3f ref_coord = (point - min_bound_) / voxel_size_; Eigen::Vector3i voxel_index; voxel_index << int(floor(ref_coord(0))), int(floor(ref_coord(1))), int(floor(ref_coord(2))); if (has_colors_) { - return thrust::make_tuple(voxel_index, geometry::Voxel(voxel_index, color)); + return thrust::make_tuple(voxel_index, + geometry::Voxel(voxel_index, color)); } else { - return thrust::make_tuple(voxel_index, geometry::Voxel(voxel_index)); + return thrust::make_tuple(voxel_index, + geometry::Voxel(voxel_index)); } } }; struct create_from_trianglemesh_functor { - create_from_trianglemesh_functor(const Eigen::Vector3f* vertices, const Eigen::Vector3i* triangles, - int n_triangles, const Eigen::Vector3f& min_bound, - float voxel_size, int num_h, int num_d) - : vertices_(vertices), triangles_(triangles), n_triangles_(n_triangles), - min_bound_(min_bound), voxel_size_(voxel_size), - box_half_size_(Eigen::Vector3f(voxel_size / 2, voxel_size / 2, voxel_size / 2)), - num_h_(num_h), num_d_(num_d) {}; - const Eigen::Vector3f* vertices_; - const Eigen::Vector3i* triangles_; + create_from_trianglemesh_functor(const Eigen::Vector3f *vertices, + const Eigen::Vector3i *triangles, + int n_triangles, + const Eigen::Vector3f &min_bound, + float voxel_size, + int num_h, + int num_d) + : vertices_(vertices), + triangles_(triangles), + n_triangles_(n_triangles), + min_bound_(min_bound), + voxel_size_(voxel_size), + box_half_size_(Eigen::Vector3f( + voxel_size / 2, voxel_size / 2, voxel_size / 2)), + num_h_(num_h), + num_d_(num_d){}; + const Eigen::Vector3f *vertices_; + const Eigen::Vector3i *triangles_; const int n_triangles_; const Eigen::Vector3f min_bound_; const float voxel_size_; const Eigen::Vector3f box_half_size_; const int num_h_; const int num_d_; - __device__ - thrust::tuple operator() (size_t idx) const { + __device__ thrust::tuple operator()( + size_t idx) const { int widx = idx / (num_h_ * num_d_); int hdidx = idx % (num_h_ * num_d_); int hidx = hdidx / num_d_; int didx = hdidx % num_d_; - const Eigen::Vector3f box_center = min_bound_ + - Eigen::Vector3f(widx, hidx, didx) * voxel_size_; + const Eigen::Vector3f box_center = + min_bound_ + Eigen::Vector3f(widx, hidx, didx) * voxel_size_; for (int i = 0; i < n_triangles_; ++i) { Eigen::Vector3i tri = triangles_[i]; const Eigen::Vector3f &v0 = vertices_[tri(0)]; const Eigen::Vector3f &v1 = vertices_[tri(1)]; const Eigen::Vector3f &v2 = vertices_[tri(2)]; - if (intersection_test::TriangleAABB( - box_center, box_half_size_, v0, v1, v2)) { + if (intersection_test::TriangleAABB(box_center, box_half_size_, v0, + v1, v2)) { Eigen::Vector3i grid_index(widx, hidx, didx); - return thrust::make_tuple(grid_index, geometry::Voxel(grid_index)); + return thrust::make_tuple(grid_index, + geometry::Voxel(grid_index)); } } - return thrust::make_tuple(Eigen::Vector3i(-1, -1, -1), geometry::Voxel()); + return thrust::make_tuple(Eigen::Vector3i(-1, -1, -1), + geometry::Voxel()); } }; -} - +} // namespace std::shared_ptr VoxelGrid::CreateDense(const Eigen::Vector3f &origin, float voxel_size, @@ -128,11 +143,14 @@ std::shared_ptr VoxelGrid::CreateDense(const Eigen::Vector3f &origin, create_dense_functor func(num_w, num_h); thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_total), - make_tuple_iterator(output->voxels_keys_.begin(), output->voxels_values_.begin()), + make_tuple_iterator(output->voxels_keys_.begin(), + output->voxels_values_.begin()), func); - thrust::sort_by_key(output->voxels_keys_.begin(), output->voxels_keys_.end(), + thrust::sort_by_key(output->voxels_keys_.begin(), + output->voxels_keys_.end(), output->voxels_values_.begin()); - auto end = thrust::unique_by_key(output->voxels_keys_.begin(), output->voxels_keys_.end(), + auto end = thrust::unique_by_key(output->voxels_keys_.begin(), + output->voxels_keys_.end(), output->voxels_values_.begin()); size_t out_size = thrust::distance(output->voxels_keys_.begin(), end.first); output->voxels_keys_.resize(out_size); @@ -161,29 +179,37 @@ std::shared_ptr VoxelGrid::CreateFromPointCloudWithinBounds( bool has_colors = input.HasColors(); create_from_pointcloud_functor func(min_bound, voxel_size, has_colors); if (!has_colors) { - thrust::transform(input.points_.begin(), input.points_.end(), thrust::make_constant_iterator(Eigen::Vector3f(0.0, 0.0, 0.0)), - make_tuple_iterator(voxels_keys.begin(), voxels_values.begin()), func); + thrust::transform( + input.points_.begin(), input.points_.end(), + thrust::make_constant_iterator(Eigen::Vector3f(0.0, 0.0, 0.0)), + make_tuple_iterator(voxels_keys.begin(), voxels_values.begin()), + func); } else { - thrust::transform(input.points_.begin(), input.points_.end(), input.colors_.begin(), - make_tuple_iterator(voxels_keys.begin(), voxels_values.begin()), func); + thrust::transform( + input.points_.begin(), input.points_.end(), + input.colors_.begin(), + make_tuple_iterator(voxels_keys.begin(), voxels_values.begin()), + func); } - thrust::sort_by_key(voxels_keys.begin(), voxels_keys.end(), voxels_values.begin()); + thrust::sort_by_key(voxels_keys.begin(), voxels_keys.end(), + voxels_values.begin()); utility::device_vector counts(voxels_keys.size()); auto end = thrust::reduce_by_key(voxels_keys.begin(), voxels_keys.end(), - thrust::make_constant_iterator(1), - thrust::make_discard_iterator(), counts.begin()); + thrust::make_constant_iterator(1), + thrust::make_discard_iterator(), + counts.begin()); int n_out = thrust::distance(counts.begin(), end.second); counts.resize(n_out); output->voxels_keys_.resize(n_out); output->voxels_values_.resize(n_out); - thrust::reduce_by_key(voxels_keys.begin(), voxels_keys.end(), - voxels_values.begin(), - output->voxels_keys_.begin(), output->voxels_values_.begin(), - thrust::equal_to(), - add_voxel_color_functor()); - thrust::transform(output->voxels_values_.begin(), output->voxels_values_.end(), - counts.begin(), output->voxels_values_.begin(), + thrust::reduce_by_key( + voxels_keys.begin(), voxels_keys.end(), voxels_values.begin(), + output->voxels_keys_.begin(), output->voxels_values_.begin(), + thrust::equal_to(), add_voxel_color_functor()); + thrust::transform(output->voxels_values_.begin(), + output->voxels_values_.end(), counts.begin(), + output->voxels_values_.begin(), devide_voxel_color_functor()); utility::LogDebug( "Pointcloud is voxelized from {:d} points to {:d} voxels.", @@ -222,21 +248,29 @@ std::shared_ptr VoxelGrid::CreateFromTriangleMeshWithinBounds( int num_h = int(std::round(grid_size(1) / voxel_size)); int num_d = int(std::round(grid_size(2) / voxel_size)); size_t n_total = num_w * num_h * num_d; - create_from_trianglemesh_functor func(thrust::raw_pointer_cast(input.vertices_.data()), - thrust::raw_pointer_cast(input.triangles_.data()), - input.triangles_.size(), - min_bound, voxel_size, num_h, num_d); + create_from_trianglemesh_functor func( + thrust::raw_pointer_cast(input.vertices_.data()), + thrust::raw_pointer_cast(input.triangles_.data()), + input.triangles_.size(), min_bound, voxel_size, num_h, num_d); output->voxels_keys_.resize(n_total); output->voxels_values_.resize(n_total); - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_total), - make_tuple_iterator(output->voxels_keys_.begin(), output->voxels_values_.begin()), func); - auto begin = make_tuple_iterator(output->voxels_keys_.begin(), output->voxels_values_.begin()); - auto end = thrust::remove_if(begin, - make_tuple_iterator(output->voxels_keys_.end(), output->voxels_values_.end()), - [] __device__ (const thrust::tuple& x) -> bool { - Eigen::Vector3i idxs = thrust::get<0>(x); - return idxs[0] < 0; - }); + thrust::transform(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_total), + make_tuple_iterator(output->voxels_keys_.begin(), + output->voxels_values_.begin()), + func); + auto begin = make_tuple_iterator(output->voxels_keys_.begin(), + output->voxels_values_.begin()); + auto end = thrust::remove_if( + begin, + make_tuple_iterator(output->voxels_keys_.end(), + output->voxels_values_.end()), + [] __device__( + const thrust::tuple &x) + -> bool { + Eigen::Vector3i idxs = thrust::get<0>(x); + return idxs[0] < 0; + }); size_t n_out = thrust::distance(begin, end); output->voxels_keys_.resize(n_out); output->voxels_values_.resize(n_out); diff --git a/src/cupoch/integration/marching_cubes_const.h b/src/cupoch/integration/marching_cubes_const.h index 91ed8f61..17143d47 100644 --- a/src/cupoch/integration/marching_cubes_const.h +++ b/src/cupoch/integration/marching_cubes_const.h @@ -2,6 +2,7 @@ #include #include + #include "cupoch/utility/platform.h" namespace { @@ -40,269 +41,3082 @@ const int edge_table_host[256] = { 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c, 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0}; -const short tri_table_host[256][12][6] = { - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}}, - {{0, 3, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {7, -1, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, 3, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{4, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 10, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}}, - {{7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {8, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, 6, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, 7, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}}, - {{7, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 5, 10, -1, -1, -1}}, - {{4, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, 5, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {3, 6, 9, -1, -1, -1}, {8, 10, -1, -1, -1, -1}, {2, 5, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}}, - {{0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}}, - {{0, 3, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {2, 4, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{3, -1, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}}, - {{2, 4, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, 3, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{5, 8, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}}, - {{8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}}, - {{3, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {2, 3, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {6, 9, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}}, - {{2, 4, 8, 10, 14, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}}, - {{2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{7, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 10, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 11, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{0, 3, 6, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 10, -1, -1, -1}}, - {{3, 6, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, 10, -1, -1, -1, -1}, {2, 5, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{7, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {6, 9, -1, -1, -1, -1}, {8, 10, -1, -1, -1, -1}, {2, 3, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{4, 6, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {12, -1, -1, -1, -1, -1}, {14, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {13, -1, -1, -1, -1, -1}, {4, 8, 10, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}}, - {{7, 12, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}}, - {{0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {13, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {4, 8, 10, -1, -1, -1}, {14, -1, -1, -1, -1, -1}, {12, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, 5, 8, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {2, 4, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, 9, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {8, 10, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}}, - {{3, 6, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 11, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}}, - {{5, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}}, - {{5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {1, 5, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {4, 8, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{11, -1, -1, -1, -1, -1}, {3, 6, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, 5, 7, -1, -1, -1}, {8, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 10, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{9, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {12, -1, -1, -1, -1, -1}, {13, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {4, 8, 10, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {14, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {5, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}}, - {{2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}}, - {{4, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{6, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 11, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {4, 8, 9, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{10, -1, -1, -1, -1, -1}, {3, 6, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, 4, 8, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {2, 4, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {5, 7, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}}, - {{4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{7, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 10, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}}, - {{4, 6, 9, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}}, - {{6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {8, 11, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {1, 5, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{2, 4, 7, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}}, - {{1, 5, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}}, - {{6, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {4, 8, 10, -1, -1, -1}, {14, -1, -1, -1, -1, -1}, {13, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {12, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{3, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{8, 11, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{7, 11, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {14, -1, -1, -1, -1, -1}, {13, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {4, 8, 10, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {12, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}}, - {{3, 6, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {1, 4, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{0, 3, 6, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 9, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 11, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}}, - {{0, 3, 6, -1, -1, -1}, {12, -1, -1, -1, -1, -1}, {13, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {4, 8, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {14, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}}, - {{7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 11, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 10, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{5, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{2, 4, 8, 11, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}}, - {{8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{8, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {8, 11, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}}, - {{7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, 3, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}}, - {{1, 5, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {4, 8, 9, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 10, -1, -1, -1, -1}, {11, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, 6, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {5, 7, 9, -1, -1, -1}, {8, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {1, 5, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 11, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}}, - {{0, 3, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}}, - {{0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {3, 6, 9, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {1, 4, 8, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{12, -1, -1, -1, -1, -1}, {13, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {4, 8, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {14, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, 4, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{7, 9, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 6, -1, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {1, 5, 8, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 8, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {4, 6, 9, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}}, - {{8, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 7, 10, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {2, 4, 11, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {11, 13, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, 6, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 4, 8, -1, -1, -1}}, - {{12, -1, -1, -1, -1, -1}, {7, 11, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {14, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, -1, -1, -1, -1}, {13, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 10, -1, -1, -1}}, - {{8, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, 6, -1, -1, -1}, {7, -1, -1, -1, -1, -1}, {10, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, 8, 9, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 5, 11, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{11, 13, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {9, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{11, 13, -1, -1, -1, -1}, {9, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {7, 12, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 10, 14, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{6, -1, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {1, 3, -1, -1, -1, -1}, {4, -1, -1, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}}, - {{0, 3, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {8, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}}, - {{1, -1, -1, -1, -1, -1}, {6, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}}, - {{0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 7, -1, -1, -1}, {8, -1, -1, -1, -1, -1}, {5, 6, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{6, -1, -1, -1, -1, -1}, {7, 9, -1, -1, -1, -1}, {0, 3, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 4, 8, 11, -1, -1}, {-1, -1, -1, -1, -1, -1}, {5, 10, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {0, 4, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, 5, -1, -1, -1, -1}, {3, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{0, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {2, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}, - {{-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1}}}; +const short tri_table_host[256][12][6] = {{{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}}, + {{0, 3, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, 3, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{4, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 10, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}}, + {{7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {8, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, 6, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, 7, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}}, + {{7, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 5, 10, -1, -1, -1}}, + {{4, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, 5, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {3, 6, 9, -1, -1, -1}, + {8, 10, -1, -1, -1, -1}, + {2, 5, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}}, + {{0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}}, + {{0, 3, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {2, 4, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{3, -1, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}}, + {{2, 4, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, 3, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{5, 8, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}}, + {{8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}}, + {{3, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {2, 3, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {6, 9, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}}, + {{2, 4, 8, 10, 14, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}}, + {{2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{7, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 10, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 11, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{0, 3, 6, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 10, -1, -1, -1}}, + {{3, 6, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, 10, -1, -1, -1, -1}, + {2, 5, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{7, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {6, 9, -1, -1, -1, -1}, + {8, 10, -1, -1, -1, -1}, + {2, 3, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{4, 6, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {12, -1, -1, -1, -1, -1}, + {14, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {13, -1, -1, -1, -1, -1}, + {4, 8, 10, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}}, + {{7, 12, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}}, + {{0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {13, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {4, 8, 10, -1, -1, -1}, + {14, -1, -1, -1, -1, -1}, + {12, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, 5, 8, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {2, 4, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, 9, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {8, 10, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}}, + {{3, 6, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 11, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}}, + {{5, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}}, + {{5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {1, 5, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {4, 8, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{11, -1, -1, -1, -1, -1}, + {3, 6, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, 5, 7, -1, -1, -1}, + {8, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 10, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{9, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {12, -1, -1, -1, -1, -1}, + {13, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {4, 8, 10, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {14, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {5, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}}, + {{2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}}, + {{4, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{6, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 11, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {4, 8, 9, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{10, -1, -1, -1, -1, -1}, + {3, 6, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, 4, 8, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {2, 4, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {5, 7, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}}, + {{4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{7, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 10, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}}, + {{4, 6, 9, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}}, + {{6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {8, 11, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {1, 5, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{2, 4, 7, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}}, + {{1, 5, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}}, + {{6, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {4, 8, 10, -1, -1, -1}, + {14, -1, -1, -1, -1, -1}, + {13, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {12, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{3, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{8, 11, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{7, 11, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {14, -1, -1, -1, -1, -1}, + {13, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {4, 8, 10, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {12, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}}, + {{3, 6, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {1, 4, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{0, 3, 6, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 9, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 11, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}}, + {{0, 3, 6, -1, -1, -1}, + {12, -1, -1, -1, -1, -1}, + {13, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {4, 8, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {14, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}}, + {{7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 11, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 10, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{5, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{2, 4, 8, 11, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}}, + {{8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{8, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {8, 11, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}}, + {{7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, 3, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}}, + {{1, 5, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {4, 8, 9, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 10, -1, -1, -1, -1}, + {11, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, 6, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {5, 7, 9, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {1, 5, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 11, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}}, + {{0, 3, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}}, + {{0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {3, 6, 9, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {1, 4, 8, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{12, -1, -1, -1, -1, -1}, + {13, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {4, 8, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {14, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, 4, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{7, 9, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 6, -1, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {1, 5, 8, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 8, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {4, 6, 9, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}}, + {{8, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 7, 10, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {2, 4, 11, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {11, 13, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, 6, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 4, 8, -1, -1, -1}}, + {{12, -1, -1, -1, -1, -1}, + {7, 11, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {14, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, -1, -1, -1, -1}, + {13, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 10, -1, -1, -1}}, + {{8, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, 6, -1, -1, -1}, + {7, -1, -1, -1, -1, -1}, + {10, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, 8, 9, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 5, 11, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{11, 13, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {9, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{11, 13, -1, -1, -1, -1}, + {9, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {7, 12, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 10, 14, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{6, -1, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {1, 3, -1, -1, -1, -1}, + {4, -1, -1, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}}, + {{0, 3, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}}, + {{1, -1, -1, -1, -1, -1}, + {6, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}}, + {{0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 7, -1, -1, -1}, + {8, -1, -1, -1, -1, -1}, + {5, 6, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{6, -1, -1, -1, -1, -1}, + {7, 9, -1, -1, -1, -1}, + {0, 3, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 4, 8, 11, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {5, 10, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {0, 4, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, 5, -1, -1, -1, -1}, + {3, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{0, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {2, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}, + {{-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1}}}; const int shift_host[8][3] = { - {0, 0, 0}, {1, 0, 0}, - {1, 1, 0}, {0, 1, 0}, - {0, 0, 1}, {1, 0, 1}, - {1, 1, 1}, {0, 1, 1}, + {0, 0, 0}, {1, 0, 0}, {1, 1, 0}, {0, 1, 0}, + {0, 0, 1}, {1, 0, 1}, {1, 1, 1}, {0, 1, 1}, }; // First 3 elements: edge start vertex coordinate (assume origin at (0, 0, 0)) @@ -329,12 +3143,17 @@ const int edge_to_vert_host[12][2] = { void SetConstants() { static std::once_flag setConstantsFlag; - std::call_once(setConstantsFlag, [] () { - cudaSafeCall(cudaMemcpyToSymbol(edge_table, edge_shift_host, sizeof(int) * 256)); - cudaSafeCall(cudaMemcpyToSymbol(tri_table, tri_table_host, sizeof(short) * 256 * 12 * 6)); - cudaSafeCall(cudaMemcpyToSymbol(shift, shift_host, sizeof(int) * 8 * 3)); - cudaSafeCall(cudaMemcpyToSymbol(edge_shift, edge_shift_host, sizeof(int) * 12 * 4)); - cudaSafeCall(cudaMemcpyToSymbol(edge_to_vert, edge_to_vert_host, sizeof(int) * 12 * 2)); + std::call_once(setConstantsFlag, []() { + cudaSafeCall(cudaMemcpyToSymbol(edge_table, edge_shift_host, + sizeof(int) * 256)); + cudaSafeCall(cudaMemcpyToSymbol(tri_table, tri_table_host, + sizeof(short) * 256 * 12 * 6)); + cudaSafeCall( + cudaMemcpyToSymbol(shift, shift_host, sizeof(int) * 8 * 3)); + cudaSafeCall(cudaMemcpyToSymbol(edge_shift, edge_shift_host, + sizeof(int) * 12 * 4)); + cudaSafeCall(cudaMemcpyToSymbol(edge_to_vert, edge_to_vert_host, + sizeof(int) * 12 * 2)); }); } diff --git a/src/cupoch/integration/uniform_tsdfvolume.cu b/src/cupoch/integration/uniform_tsdfvolume.cu index c040e925..4d2acc30 100644 --- a/src/cupoch/integration/uniform_tsdfvolume.cu +++ b/src/cupoch/integration/uniform_tsdfvolume.cu @@ -1,28 +1,26 @@ -#include "cupoch/integration/uniform_tsdfvolume.h" - #include "cupoch/geometry/voxelgrid.h" #include "cupoch/integration/marching_cubes_const.h" -#include "cupoch/utility/range.h" +#include "cupoch/integration/uniform_tsdfvolume.h" #include "cupoch/utility/helper.h" +#include "cupoch/utility/range.h" using namespace cupoch; using namespace cupoch::integration; namespace { -__device__ -int IndexOf(int x, int y, int z, int resolution) { +__device__ int IndexOf(int x, int y, int z, int resolution) { return x * resolution * resolution + y * resolution + z; } -__device__ -int IndexOf(const Eigen::Vector3i &xyz, int resolution) { +__device__ int IndexOf(const Eigen::Vector3i &xyz, int resolution) { return IndexOf(xyz(0), xyz(1), xyz(2), resolution); } -__device__ -float GetTSDFAt(const Eigen::Vector3f &p, const geometry::TSDFVoxel* voxels, - float voxel_length, int resolution) { +__device__ float GetTSDFAt(const Eigen::Vector3f &p, + const geometry::TSDFVoxel *voxels, + float voxel_length, + int resolution) { Eigen::Vector3i idx; Eigen::Vector3f p_grid = p / voxel_length - Eigen::Vector3f(0.5, 0.5, 0.5); for (int i = 0; i < 3; i++) { @@ -50,9 +48,10 @@ float GetTSDFAt(const Eigen::Vector3f &p, const geometry::TSDFVoxel* voxels, return tsdf; } -__device__ -Eigen::Vector3f GetNormalAt(const Eigen::Vector3f &p, const geometry::TSDFVoxel* voxels, - float voxel_length, int resolution) { +__device__ Eigen::Vector3f GetNormalAt(const Eigen::Vector3f &p, + const geometry::TSDFVoxel *voxels, + float voxel_length, + int resolution) { Eigen::Vector3f n; const double half_gap = 0.99 * voxel_length; for (int i = 0; i < 3; i++) { @@ -60,27 +59,32 @@ Eigen::Vector3f GetNormalAt(const Eigen::Vector3f &p, const geometry::TSDFVoxel* p0(i) -= half_gap; Eigen::Vector3f p1 = p; p1(i) += half_gap; - n(i) = GetTSDFAt(p1, voxels, voxel_length, resolution) - GetTSDFAt(p0, voxels, voxel_length, resolution); + n(i) = GetTSDFAt(p1, voxels, voxel_length, resolution) - + GetTSDFAt(p0, voxels, voxel_length, resolution); } return n.normalized(); } struct extract_pointcloud_functor { - extract_pointcloud_functor(const geometry::TSDFVoxel* voxels, - int resolution, float voxel_length, - const Eigen::Vector3f& origin, + extract_pointcloud_functor(const geometry::TSDFVoxel *voxels, + int resolution, + float voxel_length, + const Eigen::Vector3f &origin, TSDFVolumeColorType color_type) - : voxels_(voxels), resolution_(resolution), voxel_length_(voxel_length), - origin_(origin), half_voxel_length_(0.5 * voxel_length_), - color_type_(color_type) {}; - const geometry::TSDFVoxel* voxels_; + : voxels_(voxels), + resolution_(resolution), + voxel_length_(voxel_length), + origin_(origin), + half_voxel_length_(0.5 * voxel_length_), + color_type_(color_type){}; + const geometry::TSDFVoxel *voxels_; const int resolution_; const float voxel_length_; const Eigen::Vector3f origin_; const float half_voxel_length_; const TSDFVolumeColorType color_type_; - __device__ - thrust::tuple operator() (size_t idx) { + __device__ thrust::tuple + operator()(size_t idx) { int res2 = (resolution_ - 1) * (resolution_ - 1); int x = idx / (3 * res2); int yzi = idx % (3 * res2); @@ -93,8 +97,8 @@ struct extract_pointcloud_functor { std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); Eigen::Vector3f normal(std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()); + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); Eigen::Vector3f color(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); @@ -117,8 +121,7 @@ struct extract_pointcloud_functor { float f1 = voxels_[IndexOf(idx1, resolution_)].tsdf_; const Eigen::Vector3f &c1 = voxels_[IndexOf(idx1, resolution_)].color_; - if (w1 != 0.0f && f1 < 0.98f && f1 >= -0.98f && - f0 * f1 < 0) { + if (w1 != 0.0f && f1 < 0.98f && f1 >= -0.98f && f0 * f1 < 0) { float r0 = std::fabs(f0); float r1 = std::fabs(f1); Eigen::Vector3f p = p0; @@ -126,8 +129,7 @@ struct extract_pointcloud_functor { point = p + origin_; if (color_type_ == TSDFVolumeColorType::RGB8) { color = (c0 * r1 + c1 * r0) / (r0 + r1) / 255.0f; - } else if (color_type_ == - TSDFVolumeColorType::Gray32) { + } else if (color_type_ == TSDFVolumeColorType::Gray32) { color = (c0 * r1 + c1 * r0) / (r0 + r1); } // has_normal @@ -139,14 +141,15 @@ struct extract_pointcloud_functor { }; struct extract_mesh_phase1_functor { - extract_mesh_phase1_functor(const geometry::TSDFVoxel* voxels, int resolution, + extract_mesh_phase1_functor(const geometry::TSDFVoxel *voxels, + int resolution, TSDFVolumeColorType color_type) - : voxels_(voxels), resolution_(resolution), color_type_(color_type) {}; - const geometry::TSDFVoxel* voxels_; + : voxels_(voxels), resolution_(resolution), color_type_(color_type){}; + const geometry::TSDFVoxel *voxels_; const int resolution_; TSDFVolumeColorType color_type_; - __device__ - thrust::tuple operator() (size_t idx) { + __device__ thrust::tuple + operator()(size_t idx) { int res2 = (resolution_ - 1) * (resolution_ - 1); int x = idx / (8 * res2); int yzi = idx % (8 * res2); @@ -156,7 +159,8 @@ struct extract_mesh_phase1_functor { int i = zi % 8; Eigen::Vector3i key = Eigen::Vector3i(x, y, z); - Eigen::Vector3i idxs = key + Eigen::Vector3i(shift[i][0], shift[i][1], shift[i][2]); + Eigen::Vector3i idxs = + key + Eigen::Vector3i(shift[i][0], shift[i][1], shift[i][2]); Eigen::Vector3f c = Eigen::Vector3f::Zero(); if (voxels_[IndexOf(idxs, resolution_)].weight_ == 0.0f) { return thrust::make_tuple(key, -1, 0.0f, c); @@ -168,7 +172,7 @@ struct extract_mesh_phase1_functor { } if (color_type_ == TSDFVolumeColorType::RGB8) { c = voxels_[IndexOf(idxs, resolution_)].color_.cast() / - 255.0; + 255.0; } else if (color_type_ == TSDFVolumeColorType::Gray32) { c = voxels_[IndexOf(idxs, resolution_)].color_.cast(); } @@ -178,30 +182,39 @@ struct extract_mesh_phase1_functor { }; struct add_cube_index_functor { - __device__ - int operator() (int x, int y) { + __device__ int operator()(int x, int y) { return (x < 0 || y < 0) ? -1 : x + y; } }; struct extract_mesh_phase2_functor { - extract_mesh_phase2_functor(const Eigen::Vector3f& origin, + extract_mesh_phase2_functor(const Eigen::Vector3f &origin, int resolution, float voxel_length, - const float* fs, - const Eigen::Vector3f* cs, + const float *fs, + const Eigen::Vector3f *cs, TSDFVolumeColorType color_type) - : origin_(origin), resolution_(resolution), voxel_length_(voxel_length), - half_voxel_length_(0.5 * voxel_length_), fs_(fs), cs_(cs), color_type_(color_type) {}; + : origin_(origin), + resolution_(resolution), + voxel_length_(voxel_length), + half_voxel_length_(0.5 * voxel_length_), + fs_(fs), + cs_(cs), + color_type_(color_type){}; const Eigen::Vector3f origin_; const int resolution_; const float voxel_length_; const float half_voxel_length_; - const float* fs_; - const Eigen::Vector3f* cs_; + const float *fs_; + const Eigen::Vector3f *cs_; const TSDFVolumeColorType color_type_; - __device__ - thrust::tuple operator() (const thrust::tuple& idxs, int idx) const { + __device__ thrust::tuple + operator()(const thrust::tuple &idxs, int idx) const { Eigen::Vector3i xyz = thrust::get<0>(idxs); int x = xyz[0]; int y = xyz[1]; @@ -211,15 +224,13 @@ struct extract_mesh_phase2_functor { int i = idx % 12; if (edge_table[cube_index] & (1 << i)) { Eigen::Vector4i edge_index = - Eigen::Vector4i(x, y, z, 0) + Eigen::Vector4i(edge_shift[i][0], edge_shift[i][1], - edge_shift[i][2], edge_shift[i][3]); + Eigen::Vector4i(x, y, z, 0) + + Eigen::Vector4i(edge_shift[i][0], edge_shift[i][1], + edge_shift[i][2], edge_shift[i][3]); Eigen::Vector3f pt( - half_voxel_length_ + - voxel_length_ * edge_index(0), - half_voxel_length_ + - voxel_length_ * edge_index(1), - half_voxel_length_ + - voxel_length_ * edge_index(2)); + half_voxel_length_ + voxel_length_ * edge_index(0), + half_voxel_length_ + voxel_length_ * edge_index(1), + half_voxel_length_ + voxel_length_ * edge_index(2)); float f0 = std::abs(fs_[offset + edge_to_vert[i][0]]); float f1 = std::abs(fs_[offset + edge_to_vert[i][1]]); pt(edge_index(3)) += f0 * voxel_length_ / (f0 + f1); @@ -230,33 +241,38 @@ struct extract_mesh_phase2_functor { const auto &c1 = cs_[offset + edge_to_vert[i][1]]; vertex_color = (f1 * c0 + f0 * c1) / (f0 + f1); } - return thrust::make_tuple(edge_index, xyz, cube_index, i, vertex, vertex_color); + return thrust::make_tuple(edge_index, xyz, cube_index, i, vertex, + vertex_color); } else { Eigen::Vector4i edge_index = -Eigen::Vector4i::Ones(); Eigen::Vector3f vertex = Eigen::Vector3f::Zero(); Eigen::Vector3f vertex_color = Eigen::Vector3f::Zero(); - return thrust::make_tuple(edge_index, xyz, cube_index, i, vertex, vertex_color); + return thrust::make_tuple(edge_index, xyz, cube_index, i, vertex, + vertex_color); } } }; struct extract_mesh_phase3_functor { - extract_mesh_phase3_functor(const int* cube_index, const int* vert_no, - const int* key_index, - Eigen::Vector3i* triangles) - : cube_index_(cube_index), vert_no_(vert_no), - key_index_(key_index), triangles_(triangles) {}; - const int* cube_index_; - const int* vert_no_; - const int* key_index_; - Eigen::Vector3i* triangles_; - __device__ - void operator() (size_t idx) { + extract_mesh_phase3_functor(const int *cube_index, + const int *vert_no, + const int *key_index, + Eigen::Vector3i *triangles) + : cube_index_(cube_index), + vert_no_(vert_no), + key_index_(key_index), + triangles_(triangles){}; + const int *cube_index_; + const int *vert_no_; + const int *key_index_; + Eigen::Vector3i *triangles_; + __device__ void operator()(size_t idx) { int j[12] = {0}; int n = key_index_[idx + 1] - key_index_[idx]; for (int i = 0; i < n; ++i) { int k = key_index_[idx] + i; - int tri_idx = tri_table[cube_index_[k]][vert_no_[k]][j[vert_no_[k]]]; + int tri_idx = + tri_table[cube_index_[k]][vert_no_[k]][j[vert_no_[k]]]; if (tri_idx < 0) continue; triangles_[idx + tri_idx / 3][tri_idx % 3] = k; j[k]++; @@ -265,48 +281,54 @@ struct extract_mesh_phase3_functor { }; struct extract_voxel_pointcloud_functor { - extract_voxel_pointcloud_functor(const geometry::TSDFVoxel* voxels, - const Eigen::Vector3f& origin, int resolution, + extract_voxel_pointcloud_functor(const geometry::TSDFVoxel *voxels, + const Eigen::Vector3f &origin, + int resolution, float voxel_length) - : voxels_(voxels), origin_(origin), resolution_(resolution), - voxel_length_(voxel_length), half_voxel_length_(0.5 * voxel_length) {}; - const geometry::TSDFVoxel* voxels_; + : voxels_(voxels), + origin_(origin), + resolution_(resolution), + voxel_length_(voxel_length), + half_voxel_length_(0.5 * voxel_length){}; + const geometry::TSDFVoxel *voxels_; const Eigen::Vector3f origin_; const int resolution_; const float voxel_length_; const float half_voxel_length_; - __device__ - thrust::tuple operator() (size_t idx) { + __device__ thrust::tuple operator()( + size_t idx) { int res2 = resolution_ * resolution_; int x = idx / res2; int yz = idx % res2; int y = yz / resolution_; int z = yz % resolution_; Eigen::Vector3f pt(half_voxel_length_ + voxel_length_ * x, - half_voxel_length_ + voxel_length_ * y, - half_voxel_length_ + voxel_length_ * z); + half_voxel_length_ + voxel_length_ * y, + half_voxel_length_ + voxel_length_ * z); int ind = IndexOf(x, y, z, resolution_); - if (voxels_[ind].weight_ != 0.0f && - voxels_[ind].tsdf_ < 0.98f && + if (voxels_[ind].weight_ != 0.0f && voxels_[ind].tsdf_ < 0.98f && voxels_[ind].tsdf_ >= -0.98f) { float c = (voxels_[ind].tsdf_ + 1.0) * 0.5; return thrust::make_tuple(pt + origin_, Eigen::Vector3f(c, c, c)); } - return thrust::make_tuple(Eigen::Vector3f(std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()), - Eigen::Vector3f(std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN())); + return thrust::make_tuple( + Eigen::Vector3f(std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()), + Eigen::Vector3f(std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN())); } }; struct extract_voxel_grid_functor { - extract_voxel_grid_functor(const geometry::TSDFVoxel* voxels, int resolution) : voxels_(voxels), resolution_(resolution) {}; - const geometry::TSDFVoxel* voxels_; + extract_voxel_grid_functor(const geometry::TSDFVoxel *voxels, + int resolution) + : voxels_(voxels), resolution_(resolution){}; + const geometry::TSDFVoxel *voxels_; const int resolution_; - __device__ - thrust::tuple operator() (size_t idx) { + __device__ thrust::tuple operator()( + size_t idx) { int res2 = resolution_ * resolution_; int x = idx / res2; int yz = idx % res2; @@ -322,25 +344,52 @@ struct extract_voxel_grid_functor { Eigen::Vector3i index = Eigen::Vector3i(x, y, z); return thrust::make_tuple(index, geometry::Voxel(index, color)); } - return thrust::make_tuple(Eigen::Vector3i(-1, -1, -1), geometry::Voxel()); + return thrust::make_tuple(Eigen::Vector3i(-1, -1, -1), + geometry::Voxel()); } }; struct integrate_functor { - integrate_functor(const Eigen::Vector3f& origin, float fx, float fy, float cx, float cy, - const Eigen::Matrix4f& extrinsic, float voxel_length, float sdf_trunc, - float safe_width, float safe_height, int resolution, - const uint8_t* color, const uint8_t* depth, - const uint8_t* depth_to_camera_distance_multiplier, - int width, int num_of_channels, TSDFVolumeColorType color_type, - geometry::TSDFVoxel* voxels) - : origin_(origin), fx_(fx), fy_(fy), cx_(cx), cy_(cy), extrinsic_(extrinsic), - voxel_length_(voxel_length), half_voxel_length_(0.5 * voxel_length), - sdf_trunc_(sdf_trunc), sdf_trunc_inv_(1.0 / sdf_trunc), extrinsic_scaled_(voxel_length * extrinsic), - safe_width_(safe_width), safe_height_(safe_height), resolution_(resolution), color_(color), - depth_(depth), depth_to_camera_distance_multiplier_(depth_to_camera_distance_multiplier), - width_(width), num_of_channels_(num_of_channels), - color_type_(color_type), voxels_(voxels) {}; + integrate_functor(const Eigen::Vector3f &origin, + float fx, + float fy, + float cx, + float cy, + const Eigen::Matrix4f &extrinsic, + float voxel_length, + float sdf_trunc, + float safe_width, + float safe_height, + int resolution, + const uint8_t *color, + const uint8_t *depth, + const uint8_t *depth_to_camera_distance_multiplier, + int width, + int num_of_channels, + TSDFVolumeColorType color_type, + geometry::TSDFVoxel *voxels) + : origin_(origin), + fx_(fx), + fy_(fy), + cx_(cx), + cy_(cy), + extrinsic_(extrinsic), + voxel_length_(voxel_length), + half_voxel_length_(0.5 * voxel_length), + sdf_trunc_(sdf_trunc), + sdf_trunc_inv_(1.0 / sdf_trunc), + extrinsic_scaled_(voxel_length * extrinsic), + safe_width_(safe_width), + safe_height_(safe_height), + resolution_(resolution), + color_(color), + depth_(depth), + depth_to_camera_distance_multiplier_( + depth_to_camera_distance_multiplier), + width_(width), + num_of_channels_(num_of_channels), + color_type_(color_type), + voxels_(voxels){}; const Eigen::Vector3f origin_; const float fx_; const float fy_; @@ -355,27 +404,24 @@ struct integrate_functor { const float safe_width_; const float safe_height_; const int resolution_; - const uint8_t* color_; - const uint8_t* depth_; - const uint8_t* depth_to_camera_distance_multiplier_; + const uint8_t *color_; + const uint8_t *depth_; + const uint8_t *depth_to_camera_distance_multiplier_; const int width_; const int num_of_channels_; const TSDFVolumeColorType color_type_; - geometry::TSDFVoxel* voxels_; - __device__ - void operator() (size_t idx) { + geometry::TSDFVoxel *voxels_; + __device__ void operator()(size_t idx) { int res2 = resolution_ * resolution_; int x = idx / res2; int yz = idx % res2; int y = yz / resolution_; int z = yz % resolution_; - Eigen::Vector4f pt_3d_homo(float(half_voxel_length_ + - voxel_length_ * x + origin_(0)), - float(half_voxel_length_ + - voxel_length_ * y + origin_(1)), - float(half_voxel_length_ + origin_(2)), - 1.f); + Eigen::Vector4f pt_3d_homo( + float(half_voxel_length_ + voxel_length_ * x + origin_(0)), + float(half_voxel_length_ + voxel_length_ * y + origin_(1)), + float(half_voxel_length_ + origin_(2)), 1.f); Eigen::Vector4f pt_camera = extrinsic_ * pt_3d_homo; pt_camera(0) += z * extrinsic_scaled_(0, 2); pt_camera(1) += z * extrinsic_scaled_(1, 2); @@ -402,39 +448,36 @@ struct integrate_functor { int v_ind = IndexOf(x, y, z, resolution_); float sdf = (d - pt_camera(2)) * - (*geometry::PointerAt(depth_to_camera_distance_multiplier_, width_, - u, v)); + (*geometry::PointerAt( + depth_to_camera_distance_multiplier_, width_, u, v)); if (sdf > -sdf_trunc_) { // integrate float tsdf = std::min(1.0f, sdf * sdf_trunc_inv_); voxels_[v_ind].tsdf_ = - (voxels_[v_ind].tsdf_ * voxels_[v_ind].weight_ + - tsdf) / + (voxels_[v_ind].tsdf_ * voxels_[v_ind].weight_ + tsdf) / (voxels_[v_ind].weight_ + 1.0f); if (color_type_ == TSDFVolumeColorType::RGB8) { - const uint8_t *rgb = - geometry::PointerAt(color_, width_, num_of_channels_, u, v, 0); + const uint8_t *rgb = geometry::PointerAt( + color_, width_, num_of_channels_, u, v, 0); Eigen::Vector3f rgb_f(rgb[0], rgb[1], rgb[2]); voxels_[v_ind].color_ = - (voxels_[v_ind].color_ * - voxels_[v_ind].weight_ + + (voxels_[v_ind].color_ * voxels_[v_ind].weight_ + rgb_f) / (voxels_[v_ind].weight_ + 1.0f); } else if (color_type_ == TSDFVolumeColorType::Gray32) { - const float *intensity = - geometry::PointerAt(color_, width_, num_of_channels_, u, v, 0); - voxels_[v_ind].color_ = - (voxels_[v_ind].color_.array() * - voxels_[v_ind].weight_ + - (*intensity)) / - (voxels_[v_ind].weight_ + 1.0f); + const float *intensity = geometry::PointerAt( + color_, width_, num_of_channels_, u, v, 0); + voxels_[v_ind].color_ = (voxels_[v_ind].color_.array() * + voxels_[v_ind].weight_ + + (*intensity)) / + (voxels_[v_ind].weight_ + 1.0f); } voxels_[v_ind].weight_ += 1.0f; } } }; -} +} // namespace UniformTSDFVolume::UniformTSDFVolume( float length, @@ -491,8 +534,8 @@ void UniformTSDFVolume::Integrate( std::shared_ptr UniformTSDFVolume::ExtractPointCloud() { auto pointcloud = std::make_shared(); extract_pointcloud_functor func(thrust::raw_pointer_cast(voxels_.data()), - resolution_, voxel_length_, - origin_, color_type_); + resolution_, voxel_length_, origin_, + color_type_); size_t n_total = 3 * (resolution_ - 1) * (resolution_ - 1); pointcloud->points_.resize(n_total); pointcloud->normals_.resize(n_total); @@ -524,32 +567,34 @@ UniformTSDFVolume::ExtractTriangleMesh() { utility::device_vector cube_indices_out(res3); extract_mesh_phase1_functor func1(thrust::raw_pointer_cast(voxels_.data()), resolution_, color_type_); - thrust::transform(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(n_total), - make_tuple_iterator(repeat_keys.begin(), cube_indices.begin(), - fs.begin(), cs.begin()), - func1); - thrust::reduce_by_key(repeat_keys.begin(), repeat_keys.end(), - cube_indices.begin(), keys.begin(), - cube_indices_out.begin(), - thrust::equal_to(), - add_cube_index_functor()); + thrust::transform( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_total), + make_tuple_iterator(repeat_keys.begin(), cube_indices.begin(), + fs.begin(), cs.begin()), + func1); + thrust::reduce_by_key( + repeat_keys.begin(), repeat_keys.end(), cube_indices.begin(), + keys.begin(), cube_indices_out.begin(), + thrust::equal_to(), add_cube_index_functor()); auto begin1 = make_tuple_iterator(keys.begin(), cube_indices_out.begin()); - auto end1 = thrust::remove_if(begin1, - make_tuple_iterator(keys.end(), cube_indices_out.end()), - [] __device__ (const thrust::tuple& x) -> bool { - int cidx = thrust::get<1>(x); - return (cidx <= 0 || cidx == 255) ? true : false; - }); + auto end1 = thrust::remove_if( + begin1, make_tuple_iterator(keys.end(), cube_indices_out.end()), + [] __device__( + const thrust::tuple &x) -> bool { + int cidx = thrust::get<1>(x); + return (cidx <= 0 || cidx == 255) ? true : false; + }); size_t n_result1 = thrust::distance(begin1, end1); keys.resize(n_result1); cube_indices_out.resize(n_result1); // compute vertices and vertex_colors - thrust::repeated_range::iterator> range_keys(keys.begin(), - keys.end(), 12); - thrust::repeated_range::iterator> range_cube_indices(cube_indices_out.begin(), - cube_indices_out.end(), 12); + thrust::repeated_range::iterator> + range_keys(keys.begin(), keys.end(), 12); + thrust::repeated_range::iterator> + range_cube_indices(cube_indices_out.begin(), cube_indices_out.end(), + 12); size_t n_result2 = 12 * keys.size(); mesh->vertices_.resize(n_result2); mesh->vertex_colors_.resize(n_result2); @@ -557,30 +602,35 @@ UniformTSDFVolume::ExtractTriangleMesh() { cube_indices.resize(n_result2); repeat_keys.resize(n_result2); utility::device_vector vert_no(n_result2); - extract_mesh_phase2_functor func2(origin_, voxel_length_, - resolution_, + extract_mesh_phase2_functor func2(origin_, voxel_length_, resolution_, thrust::raw_pointer_cast(fs.data()), thrust::raw_pointer_cast(cs.data()), color_type_); - thrust::transform(make_tuple_iterator(range_keys.begin(), range_cube_indices.begin()), - make_tuple_iterator(range_keys.end(), range_cube_indices.end()), - thrust::make_counting_iterator(0), - make_tuple_iterator(edge_indices.begin(), repeat_keys.begin(), - cube_indices.begin(), vert_no.begin(), - mesh->vertices_.begin(), mesh->vertex_colors_.begin()), - func2); - auto begin2 = make_tuple_iterator(edge_indices.begin(), repeat_keys.begin(), cube_indices.begin(), - vert_no.begin(), mesh->vertices_.begin(), + thrust::transform( + make_tuple_iterator(range_keys.begin(), range_cube_indices.begin()), + make_tuple_iterator(range_keys.end(), range_cube_indices.end()), + thrust::make_counting_iterator(0), + make_tuple_iterator(edge_indices.begin(), repeat_keys.begin(), + cube_indices.begin(), vert_no.begin(), + mesh->vertices_.begin(), + mesh->vertex_colors_.begin()), + func2); + auto begin2 = make_tuple_iterator(edge_indices.begin(), repeat_keys.begin(), + cube_indices.begin(), vert_no.begin(), + mesh->vertices_.begin(), mesh->vertex_colors_.begin()); - auto end2 = thrust::remove_if(begin2, - make_tuple_iterator(edge_indices.end(), repeat_keys.end(), - cube_indices.end(), - vert_no.end(), mesh->vertices_.end(), - mesh->vertex_colors_.end()), - [] __device__ (const thrust::tuple& x) { - Eigen::Vector4i edge_index = thrust::get<0>(x); - return edge_index[0] < 0; - }); + auto end2 = thrust::remove_if( + begin2, + make_tuple_iterator(edge_indices.end(), repeat_keys.end(), + cube_indices.end(), vert_no.end(), + mesh->vertices_.end(), + mesh->vertex_colors_.end()), + [] __device__(const thrust::tuple &x) { + Eigen::Vector4i edge_index = thrust::get<0>(x); + return edge_index[0] < 0; + }); size_t n_result3 = thrust::distance(begin2, end2); edge_indices.resize(n_result3); repeat_keys.resize(n_result3); @@ -588,18 +638,16 @@ UniformTSDFVolume::ExtractTriangleMesh() { vert_no.resize(n_result3); mesh->vertices_.resize(n_result3); mesh->vertex_colors_.resize(n_result3); - thrust::sort_by_key(edge_indices.begin(), edge_indices.end(), - make_tuple_iterator(cube_indices.begin(), - repeat_keys.begin(), - vert_no.begin(), - mesh->vertices_.begin(), - mesh->vertex_colors_.begin())); - auto end3 = thrust::unique_by_key(edge_indices.begin(), edge_indices.end(), - make_tuple_iterator(cube_indices.begin(), - repeat_keys.begin(), - vert_no.begin(), - mesh->vertices_.begin(), - mesh->vertex_colors_.begin())); + thrust::sort_by_key( + edge_indices.begin(), edge_indices.end(), + make_tuple_iterator(cube_indices.begin(), repeat_keys.begin(), + vert_no.begin(), mesh->vertices_.begin(), + mesh->vertex_colors_.begin())); + auto end3 = thrust::unique_by_key( + edge_indices.begin(), edge_indices.end(), + make_tuple_iterator(cube_indices.begin(), repeat_keys.begin(), + vert_no.begin(), mesh->vertices_.begin(), + mesh->vertex_colors_.begin())); size_t n_result4 = thrust::distance(edge_indices.begin(), end3.first); edge_indices.resize(n_result4); repeat_keys.resize(n_result4); @@ -607,11 +655,11 @@ UniformTSDFVolume::ExtractTriangleMesh() { vert_no.resize(n_result4); mesh->vertices_.resize(n_result4); mesh->vertex_colors_.resize(n_result4); - thrust::sort_by_key(repeat_keys.begin(), repeat_keys.end(), - make_tuple_iterator(cube_indices.begin(), - vert_no.begin(), - mesh->vertices_.begin(), - mesh->vertex_colors_.begin())); + thrust::sort_by_key( + repeat_keys.begin(), repeat_keys.end(), + make_tuple_iterator(cube_indices.begin(), vert_no.begin(), + mesh->vertices_.begin(), + mesh->vertex_colors_.begin())); // compute triangles utility::device_vector seq(n_result4); @@ -623,14 +671,18 @@ UniformTSDFVolume::ExtractTriangleMesh() { seq.resize(n_result5); seq.push_back(n_result4); mesh->triangles_.resize(n_result5 * 5); - thrust::fill(mesh->triangles_.begin(), mesh->triangles_.end(), Eigen::Vector3i(-1, -1, -1)); - extract_mesh_phase3_functor func3(thrust::raw_pointer_cast(cube_indices.data()), - thrust::raw_pointer_cast(vert_no.data()), - thrust::raw_pointer_cast(seq.data()), - thrust::raw_pointer_cast(mesh->triangles_.data())); - thrust::for_each(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_result5), func3); - auto end5 = thrust::remove_if(mesh->triangles_.begin(), mesh->triangles_.end(), - [] __device__ (const Eigen::Vector3i& idxs) {return idxs[0] < 0;}); + thrust::fill(mesh->triangles_.begin(), mesh->triangles_.end(), + Eigen::Vector3i(-1, -1, -1)); + extract_mesh_phase3_functor func3( + thrust::raw_pointer_cast(cube_indices.data()), + thrust::raw_pointer_cast(vert_no.data()), + thrust::raw_pointer_cast(seq.data()), + thrust::raw_pointer_cast(mesh->triangles_.data())); + thrust::for_each(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_result5), func3); + auto end5 = thrust::remove_if( + mesh->triangles_.begin(), mesh->triangles_.end(), + [] __device__(const Eigen::Vector3i &idxs) { return idxs[0] < 0; }); mesh->triangles_.resize(thrust::distance(mesh->triangles_.begin(), end5)); return mesh; } @@ -641,13 +693,17 @@ UniformTSDFVolume::ExtractVoxelPointCloud() const { // const float *p_tsdf = (const float *)tsdf_.data(); // const float *p_weight = (const float *)weight_.data(); // const float *p_color = (const float *)color_.data(); - extract_voxel_pointcloud_functor func(thrust::raw_pointer_cast(voxels_.data()), - origin_, resolution_, voxel_length_); + extract_voxel_pointcloud_functor func( + thrust::raw_pointer_cast(voxels_.data()), origin_, resolution_, + voxel_length_); size_t n_total = resolution_ * resolution_ * resolution_; voxel->points_.resize(n_total); voxel->colors_.resize(n_total); - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_total), - make_tuple_iterator(voxel->points_.begin(), voxel->colors_.begin()), func); + thrust::transform( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_total), + make_tuple_iterator(voxel->points_.begin(), voxel->colors_.begin()), + func); voxel->RemoveNoneFinitePoints(true, false); return voxel; } @@ -660,17 +716,25 @@ std::shared_ptr UniformTSDFVolume::ExtractVoxelGrid() size_t n_total = resolution_ * resolution_ * resolution_; voxel_grid->voxels_keys_.resize(n_total); voxel_grid->voxels_values_.resize(n_total); - extract_voxel_grid_functor func(thrust::raw_pointer_cast(voxels_.data()), resolution_); - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_total), - make_tuple_iterator(voxel_grid->voxels_keys_.begin(), voxel_grid->voxels_values_.begin()), + extract_voxel_grid_functor func(thrust::raw_pointer_cast(voxels_.data()), + resolution_); + thrust::transform(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_total), + make_tuple_iterator(voxel_grid->voxels_keys_.begin(), + voxel_grid->voxels_values_.begin()), func); - auto begin = make_tuple_iterator(voxel_grid->voxels_keys_.begin(), voxel_grid->voxels_values_.begin()); - auto end = thrust::remove_if(begin, - make_tuple_iterator(voxel_grid->voxels_keys_.end(), voxel_grid->voxels_values_.end()), - [] __device__ (const thrust::tuple& x) -> bool { - Eigen::Vector3i index = thrust::get<0>(x); - return index[0] < 0; - }); + auto begin = make_tuple_iterator(voxel_grid->voxels_keys_.begin(), + voxel_grid->voxels_values_.begin()); + auto end = thrust::remove_if( + begin, + make_tuple_iterator(voxel_grid->voxels_keys_.end(), + voxel_grid->voxels_values_.end()), + [] __device__( + const thrust::tuple &x) + -> bool { + Eigen::Vector3i index = thrust::get<0>(x); + return index[0] < 0; + }); size_t n_out = thrust::distance(begin, end); voxel_grid->voxels_keys_.resize(n_out); voxel_grid->voxels_values_.resize(n_out); @@ -688,14 +752,17 @@ void UniformTSDFVolume::IntegrateWithDepthToCameraDistanceMultiplier( const float cy = intrinsic.GetPrincipalPoint().second; const float safe_width = intrinsic.width_ - 0.0001f; const float safe_height = intrinsic.height_ - 0.0001f; - integrate_functor func(origin_, fx, fy, cx, cy, extrinsic, voxel_length_, sdf_trunc_, - safe_width, safe_height, resolution_, - thrust::raw_pointer_cast(image.color_.data_.data()), - thrust::raw_pointer_cast(image.depth_.data_.data()), - thrust::raw_pointer_cast(depth_to_camera_distance_multiplier.data_.data()), - image.depth_.width_, image.color_.num_of_channels_, color_type_, - thrust::raw_pointer_cast(voxels_.data())); + integrate_functor func( + origin_, fx, fy, cx, cy, extrinsic, voxel_length_, sdf_trunc_, + safe_width, safe_height, resolution_, + thrust::raw_pointer_cast(image.color_.data_.data()), + thrust::raw_pointer_cast(image.depth_.data_.data()), + thrust::raw_pointer_cast( + depth_to_camera_distance_multiplier.data_.data()), + image.depth_.width_, image.color_.num_of_channels_, color_type_, + thrust::raw_pointer_cast(voxels_.data())); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(resolution_ * resolution_ * resolution_), + thrust::make_counting_iterator( + resolution_ * resolution_ * resolution_), func); } \ No newline at end of file diff --git a/src/cupoch/integration/uniform_tsdfvolume.h b/src/cupoch/integration/uniform_tsdfvolume.h index 1752f66d..6ac270cf 100644 --- a/src/cupoch/integration/uniform_tsdfvolume.h +++ b/src/cupoch/integration/uniform_tsdfvolume.h @@ -9,15 +9,13 @@ namespace geometry { class TSDFVoxel : public Voxel { public: - __host__ __device__ - TSDFVoxel() : Voxel() {} - __host__ __device__ - TSDFVoxel(const Eigen::Vector3i &grid_index) : Voxel(grid_index) {} - __host__ __device__ - TSDFVoxel(const Eigen::Vector3i &grid_index, const Eigen::Vector3f &color) + __host__ __device__ TSDFVoxel() : Voxel() {} + __host__ __device__ TSDFVoxel(const Eigen::Vector3i &grid_index) + : Voxel(grid_index) {} + __host__ __device__ TSDFVoxel(const Eigen::Vector3i &grid_index, + const Eigen::Vector3f &color) : Voxel(grid_index, color) {} - __host__ __device__ - ~TSDFVoxel() {} + __host__ __device__ ~TSDFVoxel() {} public: float tsdf_ = 0; diff --git a/src/cupoch/odometry/odometry.cu b/src/cupoch/odometry/odometry.cu index 97cfad85..b7baa434 100644 --- a/src/cupoch/odometry/odometry.cu +++ b/src/cupoch/odometry/odometry.cu @@ -1,10 +1,8 @@ -#include "cupoch/odometry/odometry.h" - -#include #include #include "cupoch/geometry/image.h" #include "cupoch/geometry/rgbdimage.h" +#include "cupoch/odometry/odometry.h" #include "cupoch/odometry/rgbdodometry_jacobian.h" namespace cupoch { @@ -13,19 +11,19 @@ namespace odometry { namespace { struct initialize_correspondence_map_functor { - initialize_correspondence_map_functor(uint8_t* correspondence_map, - uint8_t* depth_buffer, + initialize_correspondence_map_functor(uint8_t *correspondence_map, + uint8_t *depth_buffer, int width) - : correspondence_map_(correspondence_map), - depth_buffer_(depth_buffer), width_(width) {}; - uint8_t* correspondence_map_; - uint8_t* depth_buffer_; + : correspondence_map_(correspondence_map), + depth_buffer_(depth_buffer), + width_(width){}; + uint8_t *correspondence_map_; + uint8_t *depth_buffer_; int width_; - __device__ - void operator() (size_t idx) { - *(int*)(correspondence_map_ + idx * 2 * sizeof(int)) = -1; - *(int*)(correspondence_map_ + (idx * 2 + 1) * sizeof(int)) = -1; - *(float*)(depth_buffer_ + idx* sizeof(float)) = -1.0f; + __device__ void operator()(size_t idx) { + *(int *)(correspondence_map_ + idx * 2 * sizeof(int)) = -1; + *(int *)(correspondence_map_ + (idx * 2 + 1) * sizeof(int)) = -1; + *(float *)(depth_buffer_ + idx * sizeof(float)) = -1.0f; } }; @@ -36,65 +34,79 @@ InitializeCorrespondenceMap(int width, int height) { auto depth_buffer = std::make_shared(); correspondence_map->Prepare(width, height, 2, 4); depth_buffer->Prepare(width, height, 1, 4); - initialize_correspondence_map_functor func(thrust::raw_pointer_cast(correspondence_map->data_.data()), - thrust::raw_pointer_cast(depth_buffer->data_.data()), - width); + initialize_correspondence_map_functor func( + thrust::raw_pointer_cast(correspondence_map->data_.data()), + thrust::raw_pointer_cast(depth_buffer->data_.data()), width); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(width * height), func); + thrust::make_counting_iterator(width * height), + func); return std::make_tuple(correspondence_map, depth_buffer); } -__device__ -inline void AddElementToCorrespondenceMap(uint8_t* correspondence_map, - uint8_t* depth_buffer, - int width, - int u_s, - int v_s, - int u_t, - int v_t, - float transformed_d_t) { +__device__ inline void AddElementToCorrespondenceMap( + uint8_t *correspondence_map, + uint8_t *depth_buffer, + int width, + int u_s, + int v_s, + int u_t, + int v_t, + float transformed_d_t) { int exist_u_t, exist_v_t; float exist_d_t; - exist_u_t = *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, 0); - exist_v_t = *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, 1); + exist_u_t = *geometry::PointerAt(correspondence_map, width, 2, u_s, + v_s, 0); + exist_v_t = *geometry::PointerAt(correspondence_map, width, 2, u_s, + v_s, 1); if (exist_u_t != -1 && exist_v_t != -1) { exist_d_t = *geometry::PointerAt(depth_buffer, width, u_s, v_s); if (transformed_d_t < exist_d_t) { // update nearer point as correspondence - *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, 0) = u_t; - *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, 1) = v_t; - *geometry::PointerAt(depth_buffer, width, u_s, v_s) = transformed_d_t; + *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, + 0) = u_t; + *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, + 1) = v_t; + *geometry::PointerAt(depth_buffer, width, u_s, v_s) = + transformed_d_t; } } else { // register correspondence - *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, 0) = u_t; - *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, 1) = v_t; - *geometry::PointerAt(depth_buffer, width, u_s, v_s) = transformed_d_t; + *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, 0) = + u_t; + *geometry::PointerAt(correspondence_map, width, 2, u_s, v_s, 1) = + v_t; + *geometry::PointerAt(depth_buffer, width, u_s, v_s) = + transformed_d_t; } } struct merge_correspondence_maps_functor { - merge_correspondence_maps_functor(uint8_t* correspondence_map, uint8_t* depth_buffer, - uint8_t* correspondence_map_part, uint8_t* depth_buffer_part, + merge_correspondence_maps_functor(uint8_t *correspondence_map, + uint8_t *depth_buffer, + uint8_t *correspondence_map_part, + uint8_t *depth_buffer_part, int width) - : correspondence_map_(correspondence_map), depth_buffer_(depth_buffer), - correspondence_map_part_(correspondence_map_part), depth_buffer_part_(depth_buffer_part), - width_(width) {}; - uint8_t* correspondence_map_; - uint8_t* depth_buffer_; - uint8_t* correspondence_map_part_; - uint8_t* depth_buffer_part_; + : correspondence_map_(correspondence_map), + depth_buffer_(depth_buffer), + correspondence_map_part_(correspondence_map_part), + depth_buffer_part_(depth_buffer_part), + width_(width){}; + uint8_t *correspondence_map_; + uint8_t *depth_buffer_; + uint8_t *correspondence_map_part_; + uint8_t *depth_buffer_part_; int width_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { int v_s = idx / width_; int u_s = idx % width_; - int u_t = *geometry::PointerAt(correspondence_map_part_, width_, 2, u_s, v_s, 0); - int v_t = *geometry::PointerAt(correspondence_map_part_, width_, 2, u_s, v_s, 1); + int u_t = *geometry::PointerAt(correspondence_map_part_, width_, 2, + u_s, v_s, 0); + int v_t = *geometry::PointerAt(correspondence_map_part_, width_, 2, + u_s, v_s, 1); if (u_t != -1 && v_t != -1) { - float transformed_d_t = - *geometry::PointerAt(depth_buffer_part_, width_, u_s, v_s); - AddElementToCorrespondenceMap(correspondence_map_, depth_buffer_, width_, - u_s, v_s, u_t, v_t, + float transformed_d_t = *geometry::PointerAt( + depth_buffer_part_, width_, u_s, v_s); + AddElementToCorrespondenceMap(correspondence_map_, depth_buffer_, + width_, u_s, v_s, u_t, v_t, transformed_d_t); } } @@ -104,87 +116,97 @@ void MergeCorrespondenceMaps(geometry::Image &correspondence_map, geometry::Image &depth_buffer, geometry::Image &correspondence_map_part, geometry::Image &depth_buffer_part) { - merge_correspondence_maps_functor func(thrust::raw_pointer_cast(correspondence_map.data_.data()), - thrust::raw_pointer_cast(depth_buffer.data_.data()), - thrust::raw_pointer_cast(correspondence_map_part.data_.data()), - thrust::raw_pointer_cast(depth_buffer_part.data_.data()), - correspondence_map.width_); - thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(correspondence_map.width_ * correspondence_map.height_), func); + merge_correspondence_maps_functor func( + thrust::raw_pointer_cast(correspondence_map.data_.data()), + thrust::raw_pointer_cast(depth_buffer.data_.data()), + thrust::raw_pointer_cast(correspondence_map_part.data_.data()), + thrust::raw_pointer_cast(depth_buffer_part.data_.data()), + correspondence_map.width_); + thrust::for_each( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(correspondence_map.width_ * + correspondence_map.height_), + func); } - struct compute_correspondence_map { - compute_correspondence_map(const uint8_t* depth_s, const uint8_t* depth_t, - int width, int height, uint8_t* correspondence_map, uint8_t* depth_buffer, - const Eigen::Vector3f& Kt, const Eigen::Matrix3f& KRK_inv, + compute_correspondence_map(const uint8_t *depth_s, + const uint8_t *depth_t, + int width, + int height, + uint8_t *correspondence_map, + uint8_t *depth_buffer, + const Eigen::Vector3f &Kt, + const Eigen::Matrix3f &KRK_inv, float max_depth_diff) - : depth_s_(depth_s), depth_t_(depth_t), width_(width), height_(height), - correspondence_map_(correspondence_map), depth_buffer_(depth_buffer), - Kt_(Kt), KRK_inv_(KRK_inv), max_depth_diff_(max_depth_diff) {}; - const uint8_t* depth_s_; - const uint8_t* depth_t_; + : depth_s_(depth_s), + depth_t_(depth_t), + width_(width), + height_(height), + correspondence_map_(correspondence_map), + depth_buffer_(depth_buffer), + Kt_(Kt), + KRK_inv_(KRK_inv), + max_depth_diff_(max_depth_diff){}; + const uint8_t *depth_s_; + const uint8_t *depth_t_; int width_; int height_; - uint8_t* correspondence_map_; - uint8_t* depth_buffer_; + uint8_t *correspondence_map_; + uint8_t *depth_buffer_; const Eigen::Vector3f Kt_; const Eigen::Matrix3f KRK_inv_; const float max_depth_diff_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { int v_s = idx / width_; int u_s = idx % width_; float d_s = *geometry::PointerAt(depth_s_, width_, u_s, v_s); - if (!std::isnan(d_s)) { + if (!isnan(d_s)) { Eigen::Vector3f uv_in_s = d_s * KRK_inv_ * Eigen::Vector3f(u_s, v_s, 1.0) + Kt_; float transformed_d_s = uv_in_s(2); int u_t = (int)(uv_in_s(0) / transformed_d_s + 0.5); int v_t = (int)(uv_in_s(1) / transformed_d_s + 0.5); - if (u_t >= 0 && u_t < width_ && v_t >= 0 && - v_t < height_) { - float d_t = *geometry::PointerAt(depth_t_, width_, u_t, v_t); - if (!std::isnan(d_t) && + if (u_t >= 0 && u_t < width_ && v_t >= 0 && v_t < height_) { + float d_t = + *geometry::PointerAt(depth_t_, width_, u_t, v_t); + if (!isnan(d_t) && std::abs(transformed_d_s - d_t) <= max_depth_diff_) { - AddElementToCorrespondenceMap( - correspondence_map_, - depth_buffer_, width_, - u_s, v_s, u_t, v_t, - (float)d_s); + AddElementToCorrespondenceMap(correspondence_map_, + depth_buffer_, width_, u_s, + v_s, u_t, v_t, (float)d_s); } } } } }; - struct compute_correspondence_functor { - compute_correspondence_functor(const uint8_t* correspondence_map, int width, - Eigen::Vector4i* correspondence) - : correspondence_map_(correspondence_map), width_(width), - correspondence_(correspondence) {}; - const uint8_t* correspondence_map_; + compute_correspondence_functor(const uint8_t *correspondence_map, + int width, + Eigen::Vector4i *correspondence) + : correspondence_map_(correspondence_map), + width_(width), + correspondence_(correspondence){}; + const uint8_t *correspondence_map_; const int width_; - Eigen::Vector4i* correspondence_; - __device__ - void operator() (size_t idx) { + Eigen::Vector4i *correspondence_; + __device__ void operator()(size_t idx) { int v_s = idx / width_; int u_s = idx % width_; - int u_t = *(int*)(correspondence_map_ + idx * 2 * sizeof(int)); - int v_t = *(int*)(correspondence_map_ + (idx * 2 + 1) * sizeof(int)); + int u_t = *(int *)(correspondence_map_ + idx * 2 * sizeof(int)); + int v_t = *(int *)(correspondence_map_ + (idx * 2 + 1) * sizeof(int)); Eigen::Vector4i pixel_correspondence(u_s, v_s, u_t, v_t); correspondence_[idx] = pixel_correspondence; } }; -void ComputeCorrespondence( - const Eigen::Matrix3f intrinsic_matrix, - const Eigen::Matrix4f &extrinsic, - const geometry::Image &depth_s, - const geometry::Image &depth_t, - const OdometryOption &option, - CorrespondenceSetPixelWise& correspondence) { +void ComputeCorrespondence(const Eigen::Matrix3f intrinsic_matrix, + const Eigen::Matrix4f &extrinsic, + const geometry::Image &depth_s, + const geometry::Image &depth_t, + const OdometryOption &option, + CorrespondenceSetPixelWise &correspondence) { const Eigen::Matrix3f K = intrinsic_matrix; const Eigen::Matrix3f K_inv = K.inverse(); const Eigen::Matrix3f R = extrinsic.block<3, 3>(0, 0); @@ -201,48 +223,62 @@ void ComputeCorrespondence( std::tie(correspondence_map_private, depth_buffer_private) = InitializeCorrespondenceMap(depth_t.width_, depth_t.height_); - compute_correspondence_map func_cm(thrust::raw_pointer_cast(depth_s.data_.data()), - thrust::raw_pointer_cast(depth_t.data_.data()), - depth_s.width_, depth_s.height_, - thrust::raw_pointer_cast(correspondence_map_private->data_.data()), - thrust::raw_pointer_cast(depth_buffer_private->data_.data()), - Kt, KRK_inv, option.max_depth_diff_); + compute_correspondence_map func_cm( + thrust::raw_pointer_cast(depth_s.data_.data()), + thrust::raw_pointer_cast(depth_t.data_.data()), depth_s.width_, + depth_s.height_, + thrust::raw_pointer_cast(correspondence_map_private->data_.data()), + thrust::raw_pointer_cast(depth_buffer_private->data_.data()), Kt, + KRK_inv, option.max_depth_diff_); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(depth_s.width_ * depth_s.height_), func_cm); + thrust::make_counting_iterator(depth_s.width_ * + depth_s.height_), + func_cm); MergeCorrespondenceMaps(*correspondence_map, *depth_buffer, - *correspondence_map_private, - *depth_buffer_private); - - correspondence.resize(correspondence_map->width_* correspondence_map->height_); - compute_correspondence_functor func_cc(thrust::raw_pointer_cast(correspondence_map->data_.data()), - correspondence_map->width_, - thrust::raw_pointer_cast(correspondence.data())); - thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(correspondence_map->width_ * correspondence_map->height_), func_cc); + *correspondence_map_private, *depth_buffer_private); + + correspondence.resize(correspondence_map->width_ * + correspondence_map->height_); + compute_correspondence_functor func_cc( + thrust::raw_pointer_cast(correspondence_map->data_.data()), + correspondence_map->width_, + thrust::raw_pointer_cast(correspondence.data())); + thrust::for_each( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(correspondence_map->width_ * + correspondence_map->height_), + func_cc); auto end = thrust::remove_if(correspondence.begin(), correspondence.end(), - [] __device__ (const Eigen::Vector4i& pc) {return (pc[2] == -1 || pc[3] == -1);}); + [] __device__(const Eigen::Vector4i &pc) { + return (pc[2] == -1 || pc[3] == -1); + }); correspondence.resize(thrust::distance(correspondence.begin(), end)); } struct convert_depth_to_xyz_image_functor { - convert_depth_to_xyz_image_functor(const uint8_t* depth, int width, - uint8_t* image_xyz, - float ox, float oy, - float inv_fx, float inv_fy) - : depth_(depth), width_(width), - image_xyz_(image_xyz), - ox_(ox), oy_(oy), - inv_fx_(inv_fx), inv_fy_(inv_fy) {}; - const uint8_t* depth_; + convert_depth_to_xyz_image_functor(const uint8_t *depth, + int width, + uint8_t *image_xyz, + float ox, + float oy, + float inv_fx, + float inv_fy) + : depth_(depth), + width_(width), + image_xyz_(image_xyz), + ox_(ox), + oy_(oy), + inv_fx_(inv_fx), + inv_fy_(inv_fy){}; + const uint8_t *depth_; const int width_; - uint8_t* image_xyz_; + uint8_t *image_xyz_; const float ox_; const float oy_; const float inv_fx_; const float inv_fy_; - __device__ - void operator() (size_t idx) { + __device__ void operator()(size_t idx) { int y = idx / width_; int x = idx % width_; float *px = geometry::PointerAt(image_xyz_, width_, 3, x, y, 0); @@ -268,12 +304,14 @@ std::shared_ptr ConvertDepthImageToXYZImage( const float oy = intrinsic_matrix(1, 2); image_xyz->Prepare(depth.width_, depth.height_, 3, 4); - convert_depth_to_xyz_image_functor func(thrust::raw_pointer_cast(depth.data_.data()), - depth.width_, - thrust::raw_pointer_cast(image_xyz->data_.data()), - ox, oy, inv_fx, inv_fy); + convert_depth_to_xyz_image_functor func( + thrust::raw_pointer_cast(depth.data_.data()), depth.width_, + thrust::raw_pointer_cast(image_xyz->data_.data()), ox, oy, inv_fx, + inv_fy); thrust::for_each(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(image_xyz->width_ * image_xyz->height_), func); + thrust::make_counting_iterator(image_xyz->width_ * + image_xyz->height_), + func); return image_xyz; } @@ -295,23 +333,27 @@ std::vector CreateCameraMatrixPyramid( } struct compute_gtg_functor { - compute_gtg_functor(const Eigen::Vector4i* correspondences, - const uint8_t* xyz_t, int width) - : correspondences_(correspondences), xyz_t_(xyz_t), width_(width) {}; - const Eigen::Vector4i* correspondences_; - const uint8_t* xyz_t_; + compute_gtg_functor(const Eigen::Vector4i *correspondences, + const uint8_t *xyz_t, + int width) + : correspondences_(correspondences), xyz_t_(xyz_t), width_(width){}; + const Eigen::Vector4i *correspondences_; + const uint8_t *xyz_t_; const int width_; - __device__ - Eigen::Matrix6f operator() (size_t idx) const { + __device__ Eigen::Matrix6f operator()(size_t idx) const { int u_t = correspondences_[idx](2); int v_t = correspondences_[idx](3); float x = *geometry::PointerAt(xyz_t_, width_, 3, u_t, v_t, 0); float y = *geometry::PointerAt(xyz_t_, width_, 3, u_t, v_t, 1); float z = *geometry::PointerAt(xyz_t_, width_, 3, u_t, v_t, 2); - Eigen::Vector6f g_r_1 = (Eigen::Vector6f() << 0.0, z, -y, 1.0, 0.0, 0.0).finished(); - Eigen::Vector6f g_r_2 = (Eigen::Vector6f() << -z, 0.0, x, 0.0, 1.0, 0.0).finished(); - Eigen::Vector6f g_r_3 = (Eigen::Vector6f() << y, -x, 0.0, 0.0, 0.0, 1.0).finished(); - return g_r_1 * g_r_1.transpose() + g_r_2 * g_r_2.transpose() + g_r_3 * g_r_3.transpose(); + Eigen::Vector6f g_r_1 = + (Eigen::Vector6f() << 0.0, z, -y, 1.0, 0.0, 0.0).finished(); + Eigen::Vector6f g_r_2 = + (Eigen::Vector6f() << -z, 0.0, x, 0.0, 1.0, 0.0).finished(); + Eigen::Vector6f g_r_3 = + (Eigen::Vector6f() << y, -x, 0.0, 0.0, 0.0, 1.0).finished(); + return g_r_1 * g_r_1.transpose() + g_r_2 * g_r_2.transpose() + + g_r_3 * g_r_3.transpose(); } }; @@ -322,8 +364,8 @@ Eigen::Matrix6f CreateInformationMatrix( const geometry::Image &depth_t, const OdometryOption &option) { CorrespondenceSetPixelWise correspondence; - ComputeCorrespondence(pinhole_camera_intrinsic.intrinsic_matrix_, - extrinsic, depth_s, depth_t, option, correspondence); + ComputeCorrespondence(pinhole_camera_intrinsic.intrinsic_matrix_, extrinsic, + depth_s, depth_t, option, correspondence); auto xyz_t = ConvertDepthImageToXYZImage( depth_t, pinhole_camera_intrinsic.intrinsic_matrix_); @@ -335,37 +377,43 @@ Eigen::Matrix6f CreateInformationMatrix( thrust::raw_pointer_cast(xyz_t->data_.data()), xyz_t->width_); Eigen::Matrix6f init = Eigen::Matrix6f::Identity(); - Eigen::Matrix6f GTG = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(correspondence.size()), - func, init, thrust::plus()); + Eigen::Matrix6f GTG = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(correspondence.size()), func, init, + thrust::plus()); return GTG; } struct compute_mean_functor { - compute_mean_functor(const Eigen::Vector4i* corres, - const uint8_t* image_s, - const uint8_t* image_t, + compute_mean_functor(const Eigen::Vector4i *corres, + const uint8_t *image_s, + const uint8_t *image_t, int width) - : corres_(corres), image_s_(image_s), image_t_(image_t), width_(width) {}; - const Eigen::Vector4i* corres_; - const uint8_t* image_s_; - const uint8_t* image_t_; + : corres_(corres), + image_s_(image_s), + image_t_(image_t), + width_(width){}; + const Eigen::Vector4i *corres_; + const uint8_t *image_s_; + const uint8_t *image_t_; int width_; - __device__ - thrust::tuple operator() (size_t idx) const { + __device__ thrust::tuple operator()(size_t idx) const { int u_s = corres_[idx](0); int v_s = corres_[idx](1); int u_t = corres_[idx](2); int v_t = corres_[idx](3); - return thrust::make_tuple(*geometry::PointerAt(image_s_, width_, u_s, v_s), - *geometry::PointerAt(image_t_, width_, u_t, v_t)); + return thrust::make_tuple( + *geometry::PointerAt(image_s_, width_, u_s, v_s), + *geometry::PointerAt(image_t_, width_, u_t, v_t)); } }; struct add_tuple2f_functor { - __device__ - thrust::tuple operator()(const thrust::tuple& lhs, const thrust::tuple& rhs) const { - return thrust::make_tuple(thrust::get<0>(lhs) + thrust::get<0>(rhs), thrust::get<1>(lhs) + thrust::get<1>(rhs)); + __device__ thrust::tuple operator()( + const thrust::tuple &lhs, + const thrust::tuple &rhs) const { + return thrust::make_tuple(thrust::get<0>(lhs) + thrust::get<0>(rhs), + thrust::get<1>(lhs) + thrust::get<1>(rhs)); } }; @@ -378,13 +426,14 @@ void NormalizeIntensity(geometry::Image &image_s, "[NormalizeIntensity] Size of two input images should be " "same"); } - compute_mean_functor func_tf(thrust::raw_pointer_cast(correspondence.data()), - thrust::raw_pointer_cast(image_s.data_.data()), - thrust::raw_pointer_cast(image_t.data_.data()), - image_s.width_); - auto means = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(correspondence.size()), - func_tf, thrust::make_tuple(0.0f, 0.0f), add_tuple2f_functor()); + compute_mean_functor func_tf( + thrust::raw_pointer_cast(correspondence.data()), + thrust::raw_pointer_cast(image_s.data_.data()), + thrust::raw_pointer_cast(image_t.data_.data()), image_s.width_); + auto means = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(correspondence.size()), func_tf, + thrust::make_tuple(0.0f, 0.0f), add_tuple2f_functor()); float mean_s = thrust::get<0>(means) / (float)correspondence.size(); float mean_t = thrust::get<1>(means) / (float)correspondence.size(); image_s.LinearTransform(0.5 / mean_s, 0.0); @@ -398,29 +447,34 @@ inline std::shared_ptr PackRGBDImage( } struct preprocess_depth_functor { - preprocess_depth_functor(uint8_t* depth, float min_depth, float max_depth) - : depth_(depth), min_depth_(min_depth), max_depth_(max_depth) {}; - uint8_t* depth_; + preprocess_depth_functor(uint8_t *depth, float min_depth, float max_depth) + : depth_(depth), min_depth_(min_depth), max_depth_(max_depth){}; + uint8_t *depth_; const float min_depth_; const float max_depth_; - __device__ - void operator() (size_t idx) { - float *p = (float*)(depth_ + idx * sizeof(float)); + __device__ void operator()(size_t idx) { + float *p = (float *)(depth_ + idx * sizeof(float)); if ((*p < min_depth_ || *p > max_depth_ || *p <= 0)) *p = std::numeric_limits::quiet_NaN(); } }; -std::shared_ptr PreprocessDepth(cudaStream_t stream, - const geometry::Image &depth_orig, const OdometryOption &option) { +std::shared_ptr PreprocessDepth( + cudaStream_t stream, + const geometry::Image &depth_orig, + const OdometryOption &option) { std::shared_ptr depth_processed = std::make_shared(); *depth_processed = depth_orig; - preprocess_depth_functor func(thrust::raw_pointer_cast(depth_processed->data_.data()), - option.min_depth_, option.max_depth_); - thrust::for_each(utility::exec_policy(stream)->on(stream), - thrust::make_counting_iterator(0), - thrust::make_counting_iterator(depth_processed->width_ * depth_processed->height_), func); + preprocess_depth_functor func( + thrust::raw_pointer_cast(depth_processed->data_.data()), + option.min_depth_, option.max_depth_); + thrust::for_each( + utility::exec_policy(stream)->on(stream), + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(depth_processed->width_ * + depth_processed->height_), + func); return depth_processed; } @@ -458,8 +512,10 @@ InitializeRGBDOdometry( source.color_.Filter(geometry::Image::FilterType::Gaussian3); auto target_gray = target.color_.Filter(geometry::Image::FilterType::Gaussian3); - auto source_depth_preprocessed = PreprocessDepth(utility::GetStream(0), source.depth_, option); - auto target_depth_preprocessed = PreprocessDepth(utility::GetStream(1), target.depth_, option); + auto source_depth_preprocessed = + PreprocessDepth(utility::GetStream(0), source.depth_, option); + auto target_depth_preprocessed = + PreprocessDepth(utility::GetStream(1), target.depth_, option); cudaSafeCall(cudaDeviceSynchronize()); auto source_depth = source_depth_preprocessed->Filter( geometry::Image::FilterType::Gaussian3); @@ -467,9 +523,8 @@ InitializeRGBDOdometry( geometry::Image::FilterType::Gaussian3); CorrespondenceSetPixelWise correspondence; - ComputeCorrespondence(pinhole_camera_intrinsic.intrinsic_matrix_, - odo_init, *source_depth, - *target_depth, option, correspondence); + ComputeCorrespondence(pinhole_camera_intrinsic.intrinsic_matrix_, odo_init, + *source_depth, *target_depth, option, correspondence); NormalizeIntensity(*source_gray, *target_gray, correspondence); auto source_out = PackRGBDImage(*source_gray, *source_depth); @@ -478,44 +533,56 @@ InitializeRGBDOdometry( } template -struct compute_jacobian_and_residual_functor : public utility::multiple_jacobians_residuals_functor { - compute_jacobian_and_residual_functor(const uint8_t* source_color, const uint8_t* source_depth, - const uint8_t* target_color, const uint8_t* target_depth, - const uint8_t* source_xyz, const uint8_t* target_dx_color, - const uint8_t* target_dx_depth, const uint8_t* target_dy_color, - const uint8_t* target_dy_depth, int width, - const Eigen::Matrix3f& intrinsic, const Eigen::Matrix4f& extrinsic, - const Eigen::Vector4i* corresps) - : source_color_(source_color), source_depth_(source_depth), - target_color_(target_color), target_depth_(target_depth), - source_xyz_(source_xyz), target_dx_color_(target_dx_color), - target_dx_depth_(target_dx_depth), target_dy_color_(target_dy_color), - target_dy_depth_(target_dy_depth), width_(width), - intrinsic_(intrinsic), extrinsic_(extrinsic), corresps_(corresps) {}; - const uint8_t* source_color_; - const uint8_t* source_depth_; - const uint8_t* target_color_; - const uint8_t* target_depth_; - const uint8_t* source_xyz_; - const uint8_t* target_dx_color_; - const uint8_t* target_dx_depth_; - const uint8_t* target_dy_color_; - const uint8_t* target_dy_depth_; +struct compute_jacobian_and_residual_functor + : public utility::multiple_jacobians_residuals_functor { + compute_jacobian_and_residual_functor(const uint8_t *source_color, + const uint8_t *source_depth, + const uint8_t *target_color, + const uint8_t *target_depth, + const uint8_t *source_xyz, + const uint8_t *target_dx_color, + const uint8_t *target_dx_depth, + const uint8_t *target_dy_color, + const uint8_t *target_dy_depth, + int width, + const Eigen::Matrix3f &intrinsic, + const Eigen::Matrix4f &extrinsic, + const Eigen::Vector4i *corresps) + : source_color_(source_color), + source_depth_(source_depth), + target_color_(target_color), + target_depth_(target_depth), + source_xyz_(source_xyz), + target_dx_color_(target_dx_color), + target_dx_depth_(target_dx_depth), + target_dy_color_(target_dy_color), + target_dy_depth_(target_dy_depth), + width_(width), + intrinsic_(intrinsic), + extrinsic_(extrinsic), + corresps_(corresps){}; + const uint8_t *source_color_; + const uint8_t *source_depth_; + const uint8_t *target_color_; + const uint8_t *target_depth_; + const uint8_t *source_xyz_; + const uint8_t *target_dx_color_; + const uint8_t *target_dx_depth_; + const uint8_t *target_dy_color_; + const uint8_t *target_dy_depth_; const int width_; const Eigen::Matrix3f intrinsic_; const Eigen::Matrix4f extrinsic_; - const Eigen::Vector4i* corresps_; + const Eigen::Vector4i *corresps_; JacobianType jacobian_; - __device__ - void operator() (int i, Eigen::Vector6f J_r[2], float r[2]) const { + __device__ void operator()(int i, + Eigen::Vector6f J_r[2], + float r[2]) const { jacobian_.ComputeJacobianAndResidual( - i, J_r, r, - source_color_, source_depth_, - target_color_, target_depth_, - source_xyz_, target_dx_color_, - target_dx_depth_, target_dy_color_, - target_dy_depth_, width_, - intrinsic_, extrinsic_, corresps_); + i, J_r, r, source_color_, source_depth_, target_color_, + target_depth_, source_xyz_, target_dx_color_, target_dx_depth_, + target_dy_color_, target_dy_depth_, width_, intrinsic_, + extrinsic_, corresps_); } }; @@ -532,23 +599,22 @@ std::tuple DoSingleIteration( const Eigen::Matrix4f &extrinsic_initial, const OdometryOption &option) { CorrespondenceSetPixelWise correspondence; - ComputeCorrespondence(intrinsic, extrinsic_initial, - source.depth_, target.depth_, - option, correspondence); + ComputeCorrespondence(intrinsic, extrinsic_initial, source.depth_, + target.depth_, option, correspondence); int corresps_count = (int)correspondence.size(); - compute_jacobian_and_residual_functor func(thrust::raw_pointer_cast(source.color_.data_.data()), - thrust::raw_pointer_cast(source.depth_.data_.data()), - thrust::raw_pointer_cast(target.color_.data_.data()), - thrust::raw_pointer_cast(target.depth_.data_.data()), - thrust::raw_pointer_cast(source_xyz.data_.data()), - thrust::raw_pointer_cast(target_dx.color_.data_.data()), - thrust::raw_pointer_cast(target_dx.depth_.data_.data()), - thrust::raw_pointer_cast(target_dy.color_.data_.data()), - thrust::raw_pointer_cast(target_dy.depth_.data_.data()), - source.color_.width_, - intrinsic, extrinsic_initial, - thrust::raw_pointer_cast(correspondence.data())); + compute_jacobian_and_residual_functor func( + thrust::raw_pointer_cast(source.color_.data_.data()), + thrust::raw_pointer_cast(source.depth_.data_.data()), + thrust::raw_pointer_cast(target.color_.data_.data()), + thrust::raw_pointer_cast(target.depth_.data_.data()), + thrust::raw_pointer_cast(source_xyz.data_.data()), + thrust::raw_pointer_cast(target_dx.color_.data_.data()), + thrust::raw_pointer_cast(target_dx.depth_.data_.data()), + thrust::raw_pointer_cast(target_dy.color_.data_.data()), + thrust::raw_pointer_cast(target_dy.depth_.data_.data()), + source.color_.width_, intrinsic, extrinsic_initial, + thrust::raw_pointer_cast(correspondence.data())); utility::LogDebug("Iter : {:d}, Level : {:d}, ", iter, level); Eigen::Matrix6f JTJ; Eigen::Vector6f JTr; @@ -569,7 +635,7 @@ std::tuple DoSingleIteration( } } -template +template std::tuple ComputeMultiscale( const geometry::RGBDImage &source, const geometry::RGBDImage &target, @@ -664,43 +730,43 @@ std::tuple ComputeRGBDOdometryT( } } -template std::tuple ComputeRGBDOdometryT( - const geometry::RGBDImage &source, - const geometry::RGBDImage &target, - const camera::PinholeCameraIntrinsic &pinhole_camera_intrinsic - /*= camera::PinholeCameraIntrinsic()*/, - const Eigen::Matrix4f &odo_init /*= Eigen::Matrix4f::Identity()*/, - const OdometryOption &option /*= OdometryOption()*/); - -template std::tuple ComputeRGBDOdometryT( - const geometry::RGBDImage &source, - const geometry::RGBDImage &target, - const camera::PinholeCameraIntrinsic &pinhole_camera_intrinsic - /*= camera::PinholeCameraIntrinsic()*/, - const Eigen::Matrix4f &odo_init /*= Eigen::Matrix4f::Identity()*/, - const OdometryOption &option /*= OdometryOption()*/); +template std::tuple +ComputeRGBDOdometryT( + const geometry::RGBDImage &source, + const geometry::RGBDImage &target, + const camera::PinholeCameraIntrinsic &pinhole_camera_intrinsic + /*= camera::PinholeCameraIntrinsic()*/, + const Eigen::Matrix4f &odo_init /*= Eigen::Matrix4f::Identity()*/, + const OdometryOption &option /*= OdometryOption()*/); + +template std::tuple +ComputeRGBDOdometryT( + const geometry::RGBDImage &source, + const geometry::RGBDImage &target, + const camera::PinholeCameraIntrinsic &pinhole_camera_intrinsic + /*= camera::PinholeCameraIntrinsic()*/, + const Eigen::Matrix4f &odo_init /*= Eigen::Matrix4f::Identity()*/, + const OdometryOption &option /*= OdometryOption()*/); } // unnamed namespace std::tuple ComputeRGBDOdometry( - const geometry::RGBDImage &source, - const geometry::RGBDImage &target, - const camera::PinholeCameraIntrinsic &pinhole_camera_intrinsic - /*= camera::PinholeCameraIntrinsic()*/, - const Eigen::Matrix4f &odo_init /*= Eigen::Matrix4f::Identity()*/, - const RGBDOdometryJacobian &jacobian_method - /*=RGBDOdometryJacobianFromHybridTerm*/, - const OdometryOption &option /*= OdometryOption()*/) { + const geometry::RGBDImage &source, + const geometry::RGBDImage &target, + const camera::PinholeCameraIntrinsic &pinhole_camera_intrinsic + /*= camera::PinholeCameraIntrinsic()*/, + const Eigen::Matrix4f &odo_init /*= Eigen::Matrix4f::Identity()*/, + const RGBDOdometryJacobian &jacobian_method + /*=RGBDOdometryJacobianFromHybridTerm*/, + const OdometryOption &option /*= OdometryOption()*/) { if (jacobian_method.jacobian_type_ == RGBDOdometryJacobian::COLOR_TERM) { - return ComputeRGBDOdometryT(source, target, - pinhole_camera_intrinsic, - odo_init, option); + return ComputeRGBDOdometryT( + source, target, pinhole_camera_intrinsic, odo_init, option); } else { - return ComputeRGBDOdometryT(source, target, - pinhole_camera_intrinsic, - odo_init, option); + return ComputeRGBDOdometryT( + source, target, pinhole_camera_intrinsic, odo_init, option); } } -} -} \ No newline at end of file +} // namespace odometry +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/odometry/rgbdodometry_jacobian.h b/src/cupoch/odometry/rgbdodometry_jacobian.h index d452df35..e3aa8669 100644 --- a/src/cupoch/odometry/rgbdodometry_jacobian.h +++ b/src/cupoch/odometry/rgbdodometry_jacobian.h @@ -1,8 +1,8 @@ #pragma once #include "cupoch/odometry/odometry_option.h" -#include "cupoch/utility/eigen.h" #include "cupoch/utility/device_vector.h" +#include "cupoch/utility/eigen.h" namespace cupoch { @@ -18,34 +18,32 @@ class RGBDOdometryJacobian { HYBRID_TERM = 1, }; - __host__ __device__ - RGBDOdometryJacobian(OdometryJacobianType jacobian_type) : jacobian_type_(jacobian_type) {} - __host__ __device__ - virtual ~RGBDOdometryJacobian() {} + __host__ __device__ RGBDOdometryJacobian(OdometryJacobianType jacobian_type) + : jacobian_type_(jacobian_type) {} + __host__ __device__ virtual ~RGBDOdometryJacobian() {} public: /// Function to compute i-th row of J and r /// the vector form of J_r is basically 6x1 matrix, but it can be /// easily extendable to 6xn matrix. /// See RGBDOdometryJacobianFromHybridTerm for this case. - __host__ __device__ - virtual void ComputeJacobianAndResidual( + __host__ __device__ virtual void ComputeJacobianAndResidual( int row, Eigen::Vector6f J_r[2], float r[2], - const uint8_t* source_color, - const uint8_t* source_depth, - const uint8_t* target_color, - const uint8_t* target_depth, - const uint8_t* source_xyz, - const uint8_t* target_dx_color, - const uint8_t* target_dx_depth, - const uint8_t* target_dy_color, - const uint8_t* target_dy_depth, + const uint8_t *source_color, + const uint8_t *source_depth, + const uint8_t *target_color, + const uint8_t *target_depth, + const uint8_t *source_xyz, + const uint8_t *target_dx_color, + const uint8_t *target_dx_depth, + const uint8_t *target_dy_color, + const uint8_t *target_dy_depth, int width, const Eigen::Matrix3f &intrinsic, const Eigen::Matrix4f &extrinsic, - const Eigen::Vector4i* corresps) const {}; + const Eigen::Vector4i *corresps) const {}; OdometryJacobianType jacobian_type_; }; @@ -57,30 +55,28 @@ class RGBDOdometryJacobian { /// In ICCV Workshops, 2011. class RGBDOdometryJacobianFromColorTerm : public RGBDOdometryJacobian { public: - __host__ __device__ - RGBDOdometryJacobianFromColorTerm() : RGBDOdometryJacobian(COLOR_TERM) {} - __host__ __device__ - ~RGBDOdometryJacobianFromColorTerm() override {} + __host__ __device__ RGBDOdometryJacobianFromColorTerm() + : RGBDOdometryJacobian(COLOR_TERM) {} + __host__ __device__ ~RGBDOdometryJacobianFromColorTerm() override {} public: - __host__ __device__ - void ComputeJacobianAndResidual( + __host__ __device__ void ComputeJacobianAndResidual( int row, Eigen::Vector6f J_r[2], float r[2], - const uint8_t* source_color, - const uint8_t* source_depth, - const uint8_t* target_color, - const uint8_t* target_depth, - const uint8_t* source_xyz, - const uint8_t* target_dx_color, - const uint8_t* target_dx_depth, - const uint8_t* target_dy_color, - const uint8_t* target_dy_depth, + const uint8_t *source_color, + const uint8_t *source_depth, + const uint8_t *target_color, + const uint8_t *target_depth, + const uint8_t *source_xyz, + const uint8_t *target_dx_color, + const uint8_t *target_dx_depth, + const uint8_t *target_dy_color, + const uint8_t *target_dy_depth, int width, const Eigen::Matrix3f &intrinsic, const Eigen::Matrix4f &extrinsic, - const Eigen::Vector4i* corresps) const override; + const Eigen::Vector4i *corresps) const override; }; /// Class to compute Jacobian using hybrid term @@ -90,30 +86,28 @@ class RGBDOdometryJacobianFromColorTerm : public RGBDOdometryJacobian { /// anonymous submission class RGBDOdometryJacobianFromHybridTerm : public RGBDOdometryJacobian { public: - __host__ __device__ - RGBDOdometryJacobianFromHybridTerm() : RGBDOdometryJacobian(HYBRID_TERM) {} - __host__ __device__ - ~RGBDOdometryJacobianFromHybridTerm() override {} + __host__ __device__ RGBDOdometryJacobianFromHybridTerm() + : RGBDOdometryJacobian(HYBRID_TERM) {} + __host__ __device__ ~RGBDOdometryJacobianFromHybridTerm() override {} public: - __host__ __device__ - void ComputeJacobianAndResidual( + __host__ __device__ void ComputeJacobianAndResidual( int row, Eigen::Vector6f J_r[2], float r[2], - const uint8_t* source_color, - const uint8_t* source_depth, - const uint8_t* target_color, - const uint8_t* target_depth, - const uint8_t* source_xyz, - const uint8_t* target_dx_color, - const uint8_t* target_dx_depth, - const uint8_t* target_dy_color, - const uint8_t* target_dy_depth, + const uint8_t *source_color, + const uint8_t *source_depth, + const uint8_t *target_color, + const uint8_t *target_depth, + const uint8_t *source_xyz, + const uint8_t *target_dx_color, + const uint8_t *target_dx_depth, + const uint8_t *target_dy_color, + const uint8_t *target_dy_depth, int width, const Eigen::Matrix3f &intrinsic, const Eigen::Matrix4f &extrinsic, - const Eigen::Vector4i* corresps) const override; + const Eigen::Vector4i *corresps) const override; }; } // namespace odometry diff --git a/src/cupoch/odometry/rgbdodometry_jacobian.inl b/src/cupoch/odometry/rgbdodometry_jacobian.inl index ad2606e0..1d9dfb47 100644 --- a/src/cupoch/odometry/rgbdodometry_jacobian.inl +++ b/src/cupoch/odometry/rgbdodometry_jacobian.inl @@ -1,36 +1,39 @@ -#include "cupoch/odometry/rgbdodometry_jacobian.h" - #include "cupoch/geometry/image.h" #include "cupoch/geometry/rgbdimage.h" +#include "cupoch/odometry/rgbdodometry_jacobian.h" + +#ifndef __CUDACC__ +using std::isnan; +#endif namespace cupoch { namespace odometry { namespace { -const float SOBEL_SCALE = 0.125; -const float LAMBDA_HYBRID_DEPTH = 0.968; +__device__ const float SOBEL_SCALE = 0.125; +__device__ const float LAMBDA_HYBRID_DEPTH = 0.968; } // unnamed namespace -__host__ __device__ -inline void RGBDOdometryJacobianFromColorTerm::ComputeJacobianAndResidual( - int row, - Eigen::Vector6f J_r[2], - float r[2], - const uint8_t* source_color, - const uint8_t* source_depth, - const uint8_t* target_color, - const uint8_t* target_depth, - const uint8_t* source_xyz, - const uint8_t* target_dx_color, - const uint8_t* target_dx_depth, - const uint8_t* target_dy_color, - const uint8_t* target_dy_depth, - int width, - const Eigen::Matrix3f &intrinsic, - const Eigen::Matrix4f &extrinsic, - const Eigen::Vector4i* corresps) const { +__host__ __device__ inline void +RGBDOdometryJacobianFromColorTerm::ComputeJacobianAndResidual( + int row, + Eigen::Vector6f J_r[2], + float r[2], + const uint8_t *source_color, + const uint8_t *source_depth, + const uint8_t *target_color, + const uint8_t *target_depth, + const uint8_t *source_xyz, + const uint8_t *target_dx_color, + const uint8_t *target_dx_depth, + const uint8_t *target_dy_color, + const uint8_t *target_dy_depth, + int width, + const Eigen::Matrix3f &intrinsic, + const Eigen::Matrix4f &extrinsic, + const Eigen::Vector4i *corresps) const { Eigen::Matrix3f R = extrinsic.block<3, 3>(0, 0); Eigen::Vector3f t = extrinsic.block<3, 1>(0, 3); @@ -40,11 +43,14 @@ inline void RGBDOdometryJacobianFromColorTerm::ComputeJacobianAndResidual( int v_t = corresps[row](3); float diff = *geometry::PointerAt(target_color, width, u_t, v_t) - *geometry::PointerAt(source_color, width, u_s, v_s); - float dIdx = SOBEL_SCALE * (*geometry::PointerAt(target_dx_color, width, u_t, v_t)); - float dIdy = SOBEL_SCALE * (*geometry::PointerAt(target_dy_color, width, u_t, v_t)); - Eigen::Vector3f p3d_mat(*geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 0), - *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 1), - *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 2)); + float dIdx = SOBEL_SCALE * (*geometry::PointerAt(target_dx_color, + width, u_t, v_t)); + float dIdy = SOBEL_SCALE * (*geometry::PointerAt(target_dy_color, + width, u_t, v_t)); + Eigen::Vector3f p3d_mat( + *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 0), + *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 1), + *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 2)); Eigen::Vector3f p3d_trans = R * p3d_mat + t; float invz = 1. / p3d_trans(2); float c0 = dIdx * intrinsic(0, 0) * invz; @@ -62,24 +68,24 @@ inline void RGBDOdometryJacobianFromColorTerm::ComputeJacobianAndResidual( r[1] = 0.0; } -__host__ __device__ -inline void RGBDOdometryJacobianFromHybridTerm::ComputeJacobianAndResidual( - int row, - Eigen::Vector6f J_r[2], - float r[2], - const uint8_t* source_color, - const uint8_t* source_depth, - const uint8_t* target_color, - const uint8_t* target_depth, - const uint8_t* source_xyz, - const uint8_t* target_dx_color, - const uint8_t* target_dx_depth, - const uint8_t* target_dy_color, - const uint8_t* target_dy_depth, - int width, - const Eigen::Matrix3f &intrinsic, - const Eigen::Matrix4f &extrinsic, - const Eigen::Vector4i* corresps) const { +__host__ __device__ inline void +RGBDOdometryJacobianFromHybridTerm::ComputeJacobianAndResidual( + int row, + Eigen::Vector6f J_r[2], + float r[2], + const uint8_t *source_color, + const uint8_t *source_depth, + const uint8_t *target_color, + const uint8_t *target_depth, + const uint8_t *source_xyz, + const uint8_t *target_dx_color, + const uint8_t *target_dx_depth, + const uint8_t *target_dy_color, + const uint8_t *target_dy_depth, + int width, + const Eigen::Matrix3f &intrinsic, + const Eigen::Matrix4f &extrinsic, + const Eigen::Vector4i *corresps) const { float sqrt_lamba_dep, sqrt_lambda_img; sqrt_lamba_dep = sqrt(LAMBDA_HYBRID_DEPTH); sqrt_lambda_img = sqrt(1.0 - LAMBDA_HYBRID_DEPTH); @@ -93,20 +99,28 @@ inline void RGBDOdometryJacobianFromHybridTerm::ComputeJacobianAndResidual( int v_s = corresps[row](1); int u_t = corresps[row](2); int v_t = corresps[row](3); - float diff_photo = (*geometry::PointerAt(target_color, width, u_t, v_t) - - *geometry::PointerAt(source_color, width, u_s, v_s)); - float dIdx = SOBEL_SCALE * (*geometry::PointerAt(target_dx_color, width, u_t, v_t)); - float dIdy = SOBEL_SCALE * (*geometry::PointerAt(target_dy_color, width, u_t, v_t)); - float dDdx = SOBEL_SCALE * (*geometry::PointerAt(target_dx_depth, width, u_t, v_t)); - float dDdy = SOBEL_SCALE * (*geometry::PointerAt(target_dy_depth, width, u_t, v_t)); - if (std::isnan(dDdx)) dDdx = 0; - if (std::isnan(dDdy)) dDdy = 0; - Eigen::Vector3f p3d_mat(*geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 0), - *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 1), - *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 2)); + float diff_photo = + (*geometry::PointerAt(target_color, width, u_t, v_t) - + *geometry::PointerAt(source_color, width, u_s, v_s)); + float dIdx = SOBEL_SCALE * (*geometry::PointerAt(target_dx_color, + width, u_t, v_t)); + float dIdy = SOBEL_SCALE * (*geometry::PointerAt(target_dy_color, + width, u_t, v_t)); + float dDdx = SOBEL_SCALE * (*geometry::PointerAt(target_dx_depth, + width, u_t, v_t)); + float dDdy = SOBEL_SCALE * (*geometry::PointerAt(target_dy_depth, + width, u_t, v_t)); + if (isnan(dDdx)) dDdx = 0; + if (isnan(dDdy)) dDdy = 0; + Eigen::Vector3f p3d_mat( + *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 0), + *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 1), + *geometry::PointerAt(source_xyz, width, 3, u_s, v_s, 2)); Eigen::Vector3f p3d_trans = R * p3d_mat + t; - float diff_geo = *geometry::PointerAt(target_depth, width, u_t, v_t) - p3d_trans(2); + float diff_geo = + *geometry::PointerAt(target_depth, width, u_t, v_t) - + p3d_trans(2); float invz = 1. / p3d_trans(2); float c0 = dIdx * fx * invz; float c1 = dIdy * fy * invz; @@ -136,5 +150,5 @@ inline void RGBDOdometryJacobianFromHybridTerm::ComputeJacobianAndResidual( r[1] = r_geo; } -} -} \ No newline at end of file +} // namespace odometry +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/registration/colored_icp.cu b/src/cupoch/registration/colored_icp.cu index 320ea8e4..b8f4ff95 100644 --- a/src/cupoch/registration/colored_icp.cu +++ b/src/cupoch/registration/colored_icp.cu @@ -1,11 +1,10 @@ -#include "cupoch/registration/colored_icp.h" - #include #include #include "cupoch/geometry/kdtree_flann.h" #include "cupoch/geometry/kdtree_search_param.h" #include "cupoch/geometry/pointcloud.h" +#include "cupoch/registration/colored_icp.h" #include "cupoch/utility/console.h" #include "cupoch/utility/eigen.h" @@ -34,8 +33,8 @@ public: public: float ComputeRMSE(const geometry::PointCloud &source, - const geometry::PointCloud &target, - const CorrespondenceSet &corres) const override; + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const override; Eigen::Matrix4f ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, @@ -50,23 +49,28 @@ private: }; struct compute_color_gradient_functor { - compute_color_gradient_functor(const Eigen::Vector3f* points, const Eigen::Vector3f* normals, - const Eigen::Vector3f* colors, - const int* indices, const float* distances2, int knn) - : points_(points), normals_(normals), colors_(colors), - indices_(indices), distances2_(distances2), knn_(knn) {}; - const Eigen::Vector3f* points_; - const Eigen::Vector3f* normals_; - const Eigen::Vector3f* colors_; - const int* indices_; - const float* distances2_; + compute_color_gradient_functor(const Eigen::Vector3f *points, + const Eigen::Vector3f *normals, + const Eigen::Vector3f *colors, + const int *indices, + const float *distances2, + int knn) + : points_(points), + normals_(normals), + colors_(colors), + indices_(indices), + distances2_(distances2), + knn_(knn){}; + const Eigen::Vector3f *points_; + const Eigen::Vector3f *normals_; + const Eigen::Vector3f *colors_; + const int *indices_; + const float *distances2_; const int knn_; - __device__ - Eigen::Vector3f operator()(size_t idx) const { + __device__ Eigen::Vector3f operator()(size_t idx) const { const Eigen::Vector3f &vt = points_[idx]; const Eigen::Vector3f &nt = normals_[idx]; - float it = (colors_[idx](0) + colors_[idx](1) + - colors_[idx](2)) / 3.0; + float it = (colors_[idx](0) + colors_[idx](1) + colors_[idx](2)) / 3.0; Eigen::Matrix A; Eigen::Matrix b; A.setZero(); @@ -77,8 +81,7 @@ struct compute_color_gradient_functor { int P_adj_idx = indices_[idx * knn_ + i]; Eigen::Vector3f vt_adj = points_[P_adj_idx]; Eigen::Vector3f vt_proj = vt_adj - (vt_adj - vt).dot(nt) * nt; - float it_adj = (colors_[P_adj_idx](0) + - colors_[P_adj_idx](1) + + float it_adj = (colors_[P_adj_idx](0) + colors_[P_adj_idx](1) + colors_[P_adj_idx](2)) / 3.0; A(nn - 1, 0) = (vt_proj(0) - vt(0)); @@ -118,42 +121,54 @@ std::shared_ptr InitializePointCloudForColoredICP( output->color_gradient_.resize(n_points, Eigen::Vector3f::Zero()); utility::device_vector point_idx; utility::device_vector point_squared_distance; - tree.SearchHybrid(output->points_, search_param.radius_, search_param.max_nn_, - point_idx, point_squared_distance); - compute_color_gradient_functor func(thrust::raw_pointer_cast(output->points_.data()), - thrust::raw_pointer_cast(output->normals_.data()), - thrust::raw_pointer_cast(output->colors_.data()), - thrust::raw_pointer_cast(point_idx.data()), - thrust::raw_pointer_cast(point_squared_distance.data()), - search_param.max_nn_); - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_points), + tree.SearchHybrid(output->points_, search_param.radius_, + search_param.max_nn_, point_idx, point_squared_distance); + compute_color_gradient_functor func( + thrust::raw_pointer_cast(output->points_.data()), + thrust::raw_pointer_cast(output->normals_.data()), + thrust::raw_pointer_cast(output->colors_.data()), + thrust::raw_pointer_cast(point_idx.data()), + thrust::raw_pointer_cast(point_squared_distance.data()), + search_param.max_nn_); + thrust::transform(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_points), output->color_gradient_.begin(), func); return output; } -struct compute_jacobian_and_residual_functor : public utility::multiple_jacobians_residuals_functor { - compute_jacobian_and_residual_functor(const Eigen::Vector3f* source_points, const Eigen::Vector3f* source_colors, - const Eigen::Vector3f* target_points, const Eigen::Vector3f* target_normals, - const Eigen::Vector3f* target_colors, const Eigen::Vector3f* target_color_gradient, - const Eigen::Vector2i* corres, - float sqrt_lambda_geometric, float sqrt_lambda_photometric) - : source_points_(source_points), source_colors_(source_colors), - target_points_(target_points), target_normals_(target_normals), - target_colors_(target_colors), target_color_gradient_(target_color_gradient), - corres_(corres), - sqrt_lambda_geometric_(sqrt_lambda_geometric), - sqrt_lambda_photometric_(sqrt_lambda_photometric) {}; - const Eigen::Vector3f* source_points_; - const Eigen::Vector3f* source_colors_; - const Eigen::Vector3f* target_points_; - const Eigen::Vector3f* target_normals_; - const Eigen::Vector3f* target_colors_; - const Eigen::Vector3f* target_color_gradient_; - const Eigen::Vector2i* corres_; +struct compute_jacobian_and_residual_functor + : public utility::multiple_jacobians_residuals_functor { + compute_jacobian_and_residual_functor( + const Eigen::Vector3f *source_points, + const Eigen::Vector3f *source_colors, + const Eigen::Vector3f *target_points, + const Eigen::Vector3f *target_normals, + const Eigen::Vector3f *target_colors, + const Eigen::Vector3f *target_color_gradient, + const Eigen::Vector2i *corres, + float sqrt_lambda_geometric, + float sqrt_lambda_photometric) + : source_points_(source_points), + source_colors_(source_colors), + target_points_(target_points), + target_normals_(target_normals), + target_colors_(target_colors), + target_color_gradient_(target_color_gradient), + corres_(corres), + sqrt_lambda_geometric_(sqrt_lambda_geometric), + sqrt_lambda_photometric_(sqrt_lambda_photometric){}; + const Eigen::Vector3f *source_points_; + const Eigen::Vector3f *source_colors_; + const Eigen::Vector3f *target_points_; + const Eigen::Vector3f *target_normals_; + const Eigen::Vector3f *target_colors_; + const Eigen::Vector3f *target_color_gradient_; + const Eigen::Vector2i *corres_; const float sqrt_lambda_geometric_; const float sqrt_lambda_photometric_; - __device__ - void operator() (int i, Eigen::Vector6f J_r[2], float r[2]) const { + __device__ void operator()(int i, + Eigen::Vector6f J_r[2], + float r[2]) const { size_t cs = corres_[i][0]; size_t ct = corres_[i][1]; const Eigen::Vector3f &vs = source_points_[cs]; @@ -176,15 +191,14 @@ struct compute_jacobian_and_residual_functor : public utility::multiple_jacobian float is0_proj = (dit.dot(vs_proj - vt)) + it; const Eigen::Matrix3f M = - (Eigen::Matrix3f() << 1.0 - nt(0) * nt(0), - -nt(0) * nt(1), -nt(0) * nt(2), -nt(0) * nt(1), - 1.0 - nt(1) * nt(1), -nt(1) * nt(2), -nt(0) * nt(2), - -nt(1) * nt(2), 1.0 - nt(2) * nt(2)) + (Eigen::Matrix3f() << 1.0 - nt(0) * nt(0), -nt(0) * nt(1), + -nt(0) * nt(2), -nt(0) * nt(1), 1.0 - nt(1) * nt(1), + -nt(1) * nt(2), -nt(0) * nt(2), -nt(1) * nt(2), + 1.0 - nt(2) * nt(2)) .finished(); const Eigen::Vector3f &ditM = -dit.transpose() * M; - J_r[1].block<3, 1>(0, 0) = - sqrt_lambda_photometric_ * vs.cross(ditM); + J_r[1].block<3, 1>(0, 0) = sqrt_lambda_photometric_ * vs.cross(ditM); J_r[1].block<3, 1>(3, 0) = sqrt_lambda_photometric_ * ditM; r[1] = sqrt_lambda_photometric_ * (is - is0_proj); } @@ -204,19 +218,21 @@ Eigen::Matrix4f TransformationEstimationForColoredICP::ComputeTransformation( const auto &target_c = (const PointCloudForColoredICP &)target; - compute_jacobian_and_residual_functor func(thrust::raw_pointer_cast(source.points_.data()), - thrust::raw_pointer_cast(source.colors_.data()), - thrust::raw_pointer_cast(target.points_.data()), - thrust::raw_pointer_cast(target.normals_.data()), - thrust::raw_pointer_cast(target.colors_.data()), - thrust::raw_pointer_cast(target_c.color_gradient_.data()), - thrust::raw_pointer_cast(corres.data()), - sqrt_lambda_geometric, sqrt_lambda_photometric); + compute_jacobian_and_residual_functor func( + thrust::raw_pointer_cast(source.points_.data()), + thrust::raw_pointer_cast(source.colors_.data()), + thrust::raw_pointer_cast(target.points_.data()), + thrust::raw_pointer_cast(target.normals_.data()), + thrust::raw_pointer_cast(target.colors_.data()), + thrust::raw_pointer_cast(target_c.color_gradient_.data()), + thrust::raw_pointer_cast(corres.data()), sqrt_lambda_geometric, + sqrt_lambda_photometric); Eigen::Matrix6f JTJ; Eigen::Vector6f JTr; float r2; thrust::tie(JTJ, JTr, r2) = - utility::ComputeJTJandJTr( + utility::ComputeJTJandJTr( func, (int)corres.size()); bool is_success; @@ -228,28 +244,34 @@ Eigen::Matrix4f TransformationEstimationForColoredICP::ComputeTransformation( } struct diff_square_colored_functor { - diff_square_colored_functor(const Eigen::Vector3f* source_points, const Eigen::Vector3f* source_colors, - const Eigen::Vector3f* target_points, const Eigen::Vector3f* target_normals, - const Eigen::Vector3f* target_colors, const Eigen::Vector3f* target_color_gradient, - const Eigen::Vector2i* corres, - float sqrt_lambda_geometric, float sqrt_lambda_photometric) - : source_points_(source_points), source_colors_(source_colors), - target_points_(target_points), target_normals_(target_normals), - target_colors_(target_colors), target_color_gradient_(target_color_gradient), - corres_(corres), - sqrt_lambda_geometric_(sqrt_lambda_geometric), - sqrt_lambda_photometric_(sqrt_lambda_photometric) {}; - const Eigen::Vector3f* source_points_; - const Eigen::Vector3f* source_colors_; - const Eigen::Vector3f* target_points_; - const Eigen::Vector3f* target_normals_; - const Eigen::Vector3f* target_colors_; - const Eigen::Vector3f* target_color_gradient_; - const Eigen::Vector2i* corres_; + diff_square_colored_functor(const Eigen::Vector3f *source_points, + const Eigen::Vector3f *source_colors, + const Eigen::Vector3f *target_points, + const Eigen::Vector3f *target_normals, + const Eigen::Vector3f *target_colors, + const Eigen::Vector3f *target_color_gradient, + const Eigen::Vector2i *corres, + float sqrt_lambda_geometric, + float sqrt_lambda_photometric) + : source_points_(source_points), + source_colors_(source_colors), + target_points_(target_points), + target_normals_(target_normals), + target_colors_(target_colors), + target_color_gradient_(target_color_gradient), + corres_(corres), + sqrt_lambda_geometric_(sqrt_lambda_geometric), + sqrt_lambda_photometric_(sqrt_lambda_photometric){}; + const Eigen::Vector3f *source_points_; + const Eigen::Vector3f *source_colors_; + const Eigen::Vector3f *target_points_; + const Eigen::Vector3f *target_normals_; + const Eigen::Vector3f *target_colors_; + const Eigen::Vector3f *target_color_gradient_; + const Eigen::Vector2i *corres_; const float sqrt_lambda_geometric_; const float sqrt_lambda_photometric_; - __device__ - float operator()(size_t idx) const { + __device__ float operator()(size_t idx) const { size_t cs = corres_[idx][0]; size_t ct = corres_[idx][1]; const Eigen::Vector3f &vs = source_points_[cs]; @@ -267,7 +289,7 @@ struct diff_square_colored_functor { float residual_geometric = sqrt_lambda_geometric_ * (vs - vt).dot(nt); float residual_photometric = sqrt_lambda_photometric_ * (is - is0_proj); return residual_geometric * residual_geometric + - residual_photometric * residual_photometric; + residual_photometric * residual_photometric; } }; @@ -280,21 +302,23 @@ float TransformationEstimationForColoredICP::ComputeRMSE( float sqrt_lambda_photometric = sqrt(lambda_photometric); const auto &target_c = (const PointCloudForColoredICP &)target; - diff_square_colored_functor func(thrust::raw_pointer_cast(source.points_.data()), - thrust::raw_pointer_cast(source.colors_.data()), - thrust::raw_pointer_cast(target.points_.data()), - thrust::raw_pointer_cast(target.normals_.data()), - thrust::raw_pointer_cast(target.colors_.data()), - thrust::raw_pointer_cast(target_c.color_gradient_.data()), - thrust::raw_pointer_cast(corres.data()), - sqrt_lambda_geometric, sqrt_lambda_photometric); - const auto err = thrust::transform_reduce(thrust::make_counting_iterator(0), thrust::make_counting_iterator(corres.size()), - func, 0.0f, thrust::plus()); + diff_square_colored_functor func( + thrust::raw_pointer_cast(source.points_.data()), + thrust::raw_pointer_cast(source.colors_.data()), + thrust::raw_pointer_cast(target.points_.data()), + thrust::raw_pointer_cast(target.normals_.data()), + thrust::raw_pointer_cast(target.colors_.data()), + thrust::raw_pointer_cast(target_c.color_gradient_.data()), + thrust::raw_pointer_cast(corres.data()), sqrt_lambda_geometric, + sqrt_lambda_photometric); + const auto err = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(corres.size()), func, 0.0f, + thrust::plus()); return err; }; -} - +} // namespace RegistrationResult cupoch::registration::RegistrationColoredICP( const geometry::PointCloud &source, diff --git a/src/cupoch/registration/colored_icp.h b/src/cupoch/registration/colored_icp.h index d330ced3..f1dc4a87 100644 --- a/src/cupoch/registration/colored_icp.h +++ b/src/cupoch/registration/colored_icp.h @@ -26,4 +26,4 @@ RegistrationResult RegistrationColoredICP( float lambda_geometric = 0.968); } // namespace registration -} // namespace open3d \ No newline at end of file +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/registration/feature.cu b/src/cupoch/registration/feature.cu index 98d904ad..1efad971 100644 --- a/src/cupoch/registration/feature.cu +++ b/src/cupoch/registration/feature.cu @@ -1,7 +1,8 @@ #include -#include "cupoch/registration/feature.h" -#include "cupoch/geometry/pointcloud.h" + #include "cupoch/geometry/kdtree_flann.h" +#include "cupoch/geometry/pointcloud.h" +#include "cupoch/registration/feature.h" #include "cupoch/utility/console.h" using namespace cupoch; @@ -9,11 +10,10 @@ using namespace cupoch::registration; namespace { -__device__ -Eigen::Vector4f ComputePairFeatures(const Eigen::Vector3f &p1, - const Eigen::Vector3f &n1, - const Eigen::Vector3f &p2, - const Eigen::Vector3f &n2) { +__device__ Eigen::Vector4f ComputePairFeatures(const Eigen::Vector3f &p1, + const Eigen::Vector3f &n1, + const Eigen::Vector3f &p2, + const Eigen::Vector3f &n2) { Eigen::Vector4f result; Eigen::Vector3f dp2p1 = p2 - p1; result(3) = dp2p1.norm(); @@ -46,17 +46,22 @@ Eigen::Vector4f ComputePairFeatures(const Eigen::Vector3f &p1, } struct compute_spfh_functor { - compute_spfh_functor(const Eigen::Vector3f* points, - const Eigen::Vector3f* normals, - const int* indices, int knn, float hist_incr) - : points_(points), normals_(normals), indices_(indices), knn_(knn), hist_incr_(hist_incr) {}; - const Eigen::Vector3f* points_; - const Eigen::Vector3f* normals_; - const int* indices_; + compute_spfh_functor(const Eigen::Vector3f *points, + const Eigen::Vector3f *normals, + const int *indices, + int knn, + float hist_incr) + : points_(points), + normals_(normals), + indices_(indices), + knn_(knn), + hist_incr_(hist_incr){}; + const Eigen::Vector3f *points_; + const Eigen::Vector3f *normals_; + const int *indices_; const int knn_; const float hist_incr_; - __device__ - Feature<33>::FeatureType operator()(size_t idx) const { + __device__ Feature<33>::FeatureType operator()(size_t idx) const { Feature<33>::FeatureType ft; for (size_t k = 1; k < knn_; k++) { // skip the point itself, compute histogram @@ -81,22 +86,21 @@ struct compute_spfh_functor { }; std::shared_ptr> ComputeSPFHFeature( - const geometry::PointCloud &input, - const geometry::KDTreeFlann &kdtree, - const geometry::KDTreeSearchParam &search_param) { + const geometry::PointCloud &input, + const geometry::KDTreeFlann &kdtree, + const geometry::KDTreeSearchParam &search_param) { auto feature = std::make_shared>(); feature->Resize((int)input.points_.size()); utility::device_vector indices; utility::device_vector distance2; auto knn = ((const geometry::KDTreeSearchParamKNN &)search_param).knn_; - kdtree.SearchKNN(input.points_, knn, - indices, distance2); + kdtree.SearchKNN(input.points_, knn, indices, distance2); float hist_incr = 100.0 / (float)(knn - 1); compute_spfh_functor func(thrust::raw_pointer_cast(input.points_.data()), thrust::raw_pointer_cast(input.normals_.data()), - thrust::raw_pointer_cast(indices.data()), - knn, hist_incr); + thrust::raw_pointer_cast(indices.data()), knn, + hist_incr); thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(input.points_.size()), feature->data_.begin(), func); @@ -104,15 +108,19 @@ std::shared_ptr> ComputeSPFHFeature( } struct compute_fpfh_functor { - compute_fpfh_functor(const Feature<33>::FeatureType* spfh_data, - const int* indices, const float* distance2, int knn) - : spfh_data_(spfh_data), indices_(indices), distance2_(distance2), knn_(knn) {}; - const Feature<33>::FeatureType* spfh_data_; - const int* indices_; - const float* distance2_; + compute_fpfh_functor(const Feature<33>::FeatureType *spfh_data, + const int *indices, + const float *distance2, + int knn) + : spfh_data_(spfh_data), + indices_(indices), + distance2_(distance2), + knn_(knn){}; + const Feature<33>::FeatureType *spfh_data_; + const int *indices_; + const float *distance2_; const int knn_; - __device__ - Feature<33>::FeatureType operator() (size_t idx) const { + __device__ Feature<33>::FeatureType operator()(size_t idx) const { Feature<33>::FeatureType ft; float sum[3] = {0.0, 0.0, 0.0}; for (size_t k = 1; k < knn_; k++) { @@ -140,12 +148,12 @@ struct compute_fpfh_functor { } }; -} +} // namespace std::shared_ptr> ComputeFPFHFeature( - const geometry::PointCloud &input, - const geometry::KDTreeSearchParam - &search_param /* = geometry::KDTreeSearchParamKNN()*/) { + const geometry::PointCloud &input, + const geometry::KDTreeSearchParam + &search_param /* = geometry::KDTreeSearchParamKNN()*/) { auto feature = std::make_shared>(); feature->Resize((int)input.points_.size()); @@ -159,13 +167,15 @@ std::shared_ptr> ComputeFPFHFeature( auto spfh = ComputeSPFHFeature(input, kdtree, search_param); utility::device_vector indices; utility::device_vector distance2; - kdtree.SearchKNN(input.points_, - ((const geometry::KDTreeSearchParamKNN &)search_param).knn_, - indices, distance2); - compute_fpfh_functor func(thrust::raw_pointer_cast(spfh->data_.data()), - thrust::raw_pointer_cast(indices.data()), - thrust::raw_pointer_cast(distance2.data()), - ((const geometry::KDTreeSearchParamKNN &)search_param).knn_); + kdtree.SearchKNN( + input.points_, + ((const geometry::KDTreeSearchParamKNN &)search_param).knn_, + indices, distance2); + compute_fpfh_functor func( + thrust::raw_pointer_cast(spfh->data_.data()), + thrust::raw_pointer_cast(indices.data()), + thrust::raw_pointer_cast(distance2.data()), + ((const geometry::KDTreeSearchParamKNN &)search_param).knn_); thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(input.points_.size()), feature->data_.begin(), func); diff --git a/src/cupoch/registration/feature.h b/src/cupoch/registration/feature.h index e287c2c5..b17e29ca 100644 --- a/src/cupoch/registration/feature.h +++ b/src/cupoch/registration/feature.h @@ -2,9 +2,9 @@ #include #include -#include "cupoch/utility/device_vector.h" #include "cupoch/geometry/kdtree_search_param.h" +#include "cupoch/utility/device_vector.h" namespace cupoch { @@ -14,12 +14,13 @@ class PointCloud; namespace registration { -template +template class Feature { public: void Resize(int n) { data_.resize(n); }; size_t Dimension() const { return Dim; } size_t Num() const { return data_.size(); }; + public: typedef Eigen::Matrix FeatureType; utility::device_vector data_; diff --git a/src/cupoch/registration/kabsch.cu b/src/cupoch/registration/kabsch.cu index b6928dd4..822ac944 100644 --- a/src/cupoch/registration/kabsch.cu +++ b/src/cupoch/registration/kabsch.cu @@ -1,108 +1,125 @@ -#include "cupoch/registration/kabsch.h" -#include "cupoch/utility/svd3_cuda.h" -#include #include #include +#include + +#include "cupoch/registration/kabsch.h" +#include "cupoch/utility/svd3_cuda.h" + using namespace cupoch; using namespace cupoch::registration; namespace { -template +template struct extract_correspondence_functor { - extract_correspondence_functor(const Eigen::Vector3f* points, const Eigen::Vector2i* corres) - : points_(points), corres_(corres) {}; - const Eigen::Vector3f* points_; - const Eigen::Vector2i* corres_; - __device__ - Eigen::Vector3f operator() (size_t idx) const { + extract_correspondence_functor(const Eigen::Vector3f *points, + const Eigen::Vector2i *corres) + : points_(points), corres_(corres){}; + const Eigen::Vector3f *points_; + const Eigen::Vector2i *corres_; + __device__ Eigen::Vector3f operator()(size_t idx) const { return points_[corres_[idx][Index]]; } }; struct outer_product_functor { - outer_product_functor(const Eigen::Vector3f* source, const Eigen::Vector3f* target, - const Eigen::Vector2i* corres, const Eigen::Vector3f& x_offset, - const Eigen::Vector3f& y_offset) - : source_(source), target_(target), corres_(corres), x_offset_(x_offset), y_offset_(y_offset) {}; - const Eigen::Vector3f* source_; - const Eigen::Vector3f* target_; - const Eigen::Vector2i* corres_; + outer_product_functor(const Eigen::Vector3f *source, + const Eigen::Vector3f *target, + const Eigen::Vector2i *corres, + const Eigen::Vector3f &x_offset, + const Eigen::Vector3f &y_offset) + : source_(source), + target_(target), + corres_(corres), + x_offset_(x_offset), + y_offset_(y_offset){}; + const Eigen::Vector3f *source_; + const Eigen::Vector3f *target_; + const Eigen::Vector2i *corres_; const Eigen::Vector3f x_offset_; const Eigen::Vector3f y_offset_; - __device__ - Eigen::Matrix3f operator() (size_t idx) const { - const Eigen::Vector3f centralized_x = source_[corres_[idx][0]] - x_offset_; - const Eigen::Vector3f centralized_y = target_[corres_[idx][1]] - y_offset_; + __device__ Eigen::Matrix3f operator()(size_t idx) const { + const Eigen::Vector3f centralized_x = + source_[corres_[idx][0]] - x_offset_; + const Eigen::Vector3f centralized_y = + target_[corres_[idx][1]] - y_offset_; Eigen::Matrix3f ans = centralized_x * centralized_y.transpose(); return ans; } }; struct set_correspondence_functor { - __device__ - Eigen::Vector2i operator() (size_t idx) { + __device__ Eigen::Vector2i operator()(size_t idx) { return Eigen::Vector2i(idx, idx); } }; -} +} // namespace -Eigen::Matrix4f_u cupoch::registration::Kabsch(const utility::device_vector& model, - const utility::device_vector& target, - const CorrespondenceSet& corres) { - //Compute the center - extract_correspondence_functor<0> ex_func0(thrust::raw_pointer_cast(model.data()), - thrust::raw_pointer_cast(corres.data())); - extract_correspondence_functor<1> ex_func1(thrust::raw_pointer_cast(target.data()), - thrust::raw_pointer_cast(corres.data())); - Eigen::Vector3f model_center = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(corres.size()), - ex_func0, Eigen::Vector3f(0.0, 0.0, 0.0), - thrust::plus()); - Eigen::Vector3f target_center = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(corres.size()), - ex_func1, Eigen::Vector3f(0.0, 0.0, 0.0), - thrust::plus()); +Eigen::Matrix4f_u cupoch::registration::Kabsch( + const utility::device_vector &model, + const utility::device_vector &target, + const CorrespondenceSet &corres) { + // Compute the center + extract_correspondence_functor<0> ex_func0( + thrust::raw_pointer_cast(model.data()), + thrust::raw_pointer_cast(corres.data())); + extract_correspondence_functor<1> ex_func1( + thrust::raw_pointer_cast(target.data()), + thrust::raw_pointer_cast(corres.data())); + Eigen::Vector3f model_center = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(corres.size()), ex_func0, + Eigen::Vector3f(0.0, 0.0, 0.0), thrust::plus()); + Eigen::Vector3f target_center = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(corres.size()), ex_func1, + Eigen::Vector3f(0.0, 0.0, 0.0), thrust::plus()); float divided_by = 1.0f / model.size(); model_center *= divided_by; target_center *= divided_by; - //Centralize them - //Compute the H matrix + // Centralize them + // Compute the H matrix outer_product_functor func(thrust::raw_pointer_cast(model.data()), thrust::raw_pointer_cast(target.data()), thrust::raw_pointer_cast(corres.data()), model_center, target_center); const Eigen::Matrix3f init = Eigen::Matrix3f::Zero(); - Eigen::Matrix3f hh = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(corres.size()), - func, init, thrust::plus()); + Eigen::Matrix3f hh = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(corres.size()), func, init, + thrust::plus()); - //Do svd + // Do svd hh /= model.size(); Eigen::Matrix3f uu, ss, vv; - svd(hh(0, 0), hh(0, 1), hh(0, 2), hh(1, 0), hh(1, 1), hh(1, 2), hh(2, 0), hh(2, 1), hh(2, 2), - uu(0, 0), uu(0, 1), uu(0, 2), uu(1, 0), uu(1, 1), uu(1, 2), uu(2, 0), uu(2, 1), uu(2, 2), - ss(0, 0), ss(0, 1), ss(0, 2), ss(1, 0), ss(1, 1), ss(1, 2), ss(2, 0), ss(2, 1), ss(2, 2), - vv(0, 0), vv(0, 1), vv(0, 2), vv(1, 0), vv(1, 1), vv(1, 2), vv(2, 0), vv(2, 1), vv(2, 2)); + svd(hh(0, 0), hh(0, 1), hh(0, 2), hh(1, 0), hh(1, 1), hh(1, 2), hh(2, 0), + hh(2, 1), hh(2, 2), uu(0, 0), uu(0, 1), uu(0, 2), uu(1, 0), uu(1, 1), + uu(1, 2), uu(2, 0), uu(2, 1), uu(2, 2), ss(0, 0), ss(0, 1), ss(0, 2), + ss(1, 0), ss(1, 1), ss(1, 2), ss(2, 0), ss(2, 1), ss(2, 2), vv(0, 0), + vv(0, 1), vv(0, 2), vv(1, 0), vv(1, 1), vv(1, 2), vv(2, 0), vv(2, 1), + vv(2, 2)); ss = Eigen::Matrix3f::Identity(); ss(2, 2) = (uu * vv).determinant(); Eigen::Matrix4f_u tr = Eigen::Matrix4f_u::Identity(); tr.block<3, 3>(0, 0) = vv * ss * uu.transpose(); - //The translation + // The translation tr.block<3, 1>(0, 3) = target_center; tr.block<3, 1>(0, 3) -= tr.block<3, 3>(0, 0) * model_center; return tr; } -Eigen::Matrix4f_u cupoch::registration::Kabsch(const utility::device_vector& model, - const utility::device_vector& target) { +Eigen::Matrix4f_u cupoch::registration::Kabsch( + const utility::device_vector &model, + const utility::device_vector &target) { CorrespondenceSet corres(model.size()); set_correspondence_functor func; - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(model.size()), corres.begin(), func); + thrust::transform(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(model.size()), + corres.begin(), func); return Kabsch(model, target, corres); } \ No newline at end of file diff --git a/src/cupoch/registration/kabsch.h b/src/cupoch/registration/kabsch.h index a3fe4c84..7c3024cd 100644 --- a/src/cupoch/registration/kabsch.h +++ b/src/cupoch/registration/kabsch.h @@ -1,18 +1,18 @@ #pragma once -#include "cupoch/utility/eigen.h" #include "cupoch/registration/transformation_estimation.h" #include "cupoch/utility/device_vector.h" +#include "cupoch/utility/eigen.h" namespace cupoch { namespace registration { -Eigen::Matrix4f_u Kabsch(const utility::device_vector& model, - const utility::device_vector& target, - const CorrespondenceSet& corres); +Eigen::Matrix4f_u Kabsch(const utility::device_vector &model, + const utility::device_vector &target, + const CorrespondenceSet &corres); -Eigen::Matrix4f_u Kabsch(const utility::device_vector& model, - const utility::device_vector& target); +Eigen::Matrix4f_u Kabsch(const utility::device_vector &model, + const utility::device_vector &target); -} -} \ No newline at end of file +} // namespace registration +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/registration/registration.cu b/src/cupoch/registration/registration.cu index b6d58ac8..748d8863 100644 --- a/src/cupoch/registration/registration.cu +++ b/src/cupoch/registration/registration.cu @@ -1,8 +1,8 @@ -#include "cupoch/registration/registration.h" -#include "cupoch/geometry/pointcloud.h" #include "cupoch/geometry/kdtree_flann.h" -#include "cupoch/utility/helper.h" +#include "cupoch/geometry/pointcloud.h" +#include "cupoch/registration/registration.h" #include "cupoch/utility/console.h" +#include "cupoch/utility/helper.h" using namespace cupoch; using namespace cupoch::registration; @@ -10,29 +10,29 @@ using namespace cupoch::registration; namespace { struct extact_knn_distance_functor { - extact_knn_distance_functor(const float* distances) : distances_(distances) {}; - const float* distances_; - __device__ - float operator() (int idx) const { + extact_knn_distance_functor(const float *distances) + : distances_(distances){}; + const float *distances_; + __device__ float operator()(int idx) const { return (std::isinf(distances_[idx])) ? 0.0 : distances_[idx]; } }; struct make_correspondence_pair_functor { - make_correspondence_pair_functor(const int* indices) : indices_(indices) {}; - const int* indices_; - __device__ - Eigen::Vector2i operator() (int i) const { - return (indices_[i] < 0) ? Eigen::Vector2i(-1, -1) : Eigen::Vector2i(i, indices_[i]); - } + make_correspondence_pair_functor(const int *indices) : indices_(indices){}; + const int *indices_; + __device__ Eigen::Vector2i operator()(int i) const { + return (indices_[i] < 0) ? Eigen::Vector2i(-1, -1) + : Eigen::Vector2i(i, indices_[i]); + } }; RegistrationResult GetRegistrationResultAndCorrespondences( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - const geometry::KDTreeFlann &target_kdtree, - float max_correspondence_distance, - const Eigen::Matrix4f &transformation) { + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const geometry::KDTreeFlann &target_kdtree, + float max_correspondence_distance, + const Eigen::Matrix4f &transformation) { RegistrationResult result(transformation); if (max_correspondence_distance <= 0.0) { return result; @@ -41,18 +41,25 @@ RegistrationResult GetRegistrationResultAndCorrespondences( const int n_pt = source.points_.size(); utility::device_vector indices(n_pt); utility::device_vector dists(n_pt); - target_kdtree.SearchHybrid(source.points_, max_correspondence_distance, - 1, indices, dists); + target_kdtree.SearchHybrid(source.points_, max_correspondence_distance, 1, + indices, dists); extact_knn_distance_functor func(thrust::raw_pointer_cast(dists.data())); result.correspondence_set_.resize(n_pt); - const float error2 = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(n_pt), - func, 0.0f, thrust::plus()); - thrust::transform(thrust::make_counting_iterator(0), thrust::make_counting_iterator(n_pt), + const float error2 = + thrust::transform_reduce(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_pt), func, + 0.0f, thrust::plus()); + thrust::transform(thrust::make_counting_iterator(0), + thrust::make_counting_iterator(n_pt), result.correspondence_set_.begin(), - make_correspondence_pair_functor(thrust::raw_pointer_cast(indices.data()))); - auto end = thrust::remove_if(result.correspondence_set_.begin(), result.correspondence_set_.end(), - [] __device__ (const Eigen::Vector2i& x) -> bool {return (x[0] < 0);}); + make_correspondence_pair_functor( + thrust::raw_pointer_cast(indices.data()))); + auto end = + thrust::remove_if(result.correspondence_set_.begin(), + result.correspondence_set_.end(), + [] __device__(const Eigen::Vector2i &x) -> bool { + return (x[0] < 0); + }); int n_out = thrust::distance(result.correspondence_set_.begin(), end); result.correspondence_set_.resize(n_out); @@ -67,44 +74,47 @@ RegistrationResult GetRegistrationResultAndCorrespondences( return result; } -} +} // namespace RegistrationResult::RegistrationResult(const Eigen::Matrix4f &transformation) : transformation_(transformation), inlier_rmse_(0.0), fitness_(0.0) {} -RegistrationResult::RegistrationResult(const RegistrationResult& other) - : transformation_(other.transformation_), correspondence_set_(other.correspondence_set_), inlier_rmse_(other.inlier_rmse_), fitness_(other.fitness_) -{} +RegistrationResult::RegistrationResult(const RegistrationResult &other) + : transformation_(other.transformation_), + correspondence_set_(other.correspondence_set_), + inlier_rmse_(other.inlier_rmse_), + fitness_(other.fitness_) {} RegistrationResult::~RegistrationResult() {} -void RegistrationResult::SetCorrespondenceSet(const thrust::host_vector& corres) { +void RegistrationResult::SetCorrespondenceSet( + const thrust::host_vector &corres) { correspondence_set_ = corres; } -thrust::host_vector RegistrationResult::GetCorrespondenceSet() const { +thrust::host_vector RegistrationResult::GetCorrespondenceSet() + const { thrust::host_vector corres = correspondence_set_; return corres; } - RegistrationResult cupoch::registration::RegistrationICP( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - float max_correspondence_distance, - const Eigen::Matrix4f &init /* = Eigen::Matrix4f::Identity()*/, - const TransformationEstimation &estimation - /* = TransformationEstimationPointToPoint(false)*/, - const ICPConvergenceCriteria - &criteria /* = ICPConvergenceCriteria()*/) { + const geometry::PointCloud &source, + const geometry::PointCloud &target, + float max_correspondence_distance, + const Eigen::Matrix4f &init /* = Eigen::Matrix4f::Identity()*/, + const TransformationEstimation &estimation + /* = TransformationEstimationPointToPoint(false)*/, + const ICPConvergenceCriteria + &criteria /* = ICPConvergenceCriteria()*/) { if (max_correspondence_distance <= 0.0) { utility::LogError("Invalid max_correspondence_distance."); } if ((estimation.GetTransformationEstimationType() == - TransformationEstimationType::PointToPlane || + TransformationEstimationType::PointToPlane || estimation.GetTransformationEstimationType() == - TransformationEstimationType::ColoredICP) && + TransformationEstimationType::ColoredICP) && (!source.HasNormals() || !target.HasNormals())) { utility::LogError( "TransformationEstimationPointToPlane and " diff --git a/src/cupoch/registration/registration.h b/src/cupoch/registration/registration.h index cffbc5f2..c9d9fd4d 100644 --- a/src/cupoch/registration/registration.h +++ b/src/cupoch/registration/registration.h @@ -1,8 +1,9 @@ #pragma once -#include "cupoch/utility/eigen.h" -#include "cupoch/registration/transformation_estimation.h" #include +#include "cupoch/registration/transformation_estimation.h" +#include "cupoch/utility/eigen.h" + namespace cupoch { namespace geometry { @@ -27,15 +28,15 @@ class ICPConvergenceCriteria { int max_iteration_; }; - class RegistrationResult { public: - RegistrationResult( - const Eigen::Matrix4f &transformation = Eigen::Matrix4f::Identity()); - RegistrationResult(const RegistrationResult& other); + RegistrationResult(const Eigen::Matrix4f &transformation = + Eigen::Matrix4f::Identity()); + RegistrationResult(const RegistrationResult &other); ~RegistrationResult(); - void SetCorrespondenceSet(const thrust::host_vector& corres); + void SetCorrespondenceSet( + const thrust::host_vector &corres); thrust::host_vector GetCorrespondenceSet() const; public: @@ -55,5 +56,5 @@ RegistrationResult RegistrationICP( TransformationEstimationPointToPoint(), const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria()); -} -} \ No newline at end of file +} // namespace registration +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/registration/transformation_estimation.cu b/src/cupoch/registration/transformation_estimation.cu index 7cf3b60a..17520c65 100644 --- a/src/cupoch/registration/transformation_estimation.cu +++ b/src/cupoch/registration/transformation_estimation.cu @@ -1,57 +1,65 @@ -#include "cupoch/registration/transformation_estimation.h" -#include "cupoch/registration/kabsch.h" -#include "cupoch/geometry/pointcloud.h" -#include #include +#include + +#include "cupoch/geometry/pointcloud.h" +#include "cupoch/registration/kabsch.h" +#include "cupoch/registration/transformation_estimation.h" + using namespace cupoch; using namespace cupoch::registration; -namespace{ +namespace { struct diff_square_pt2pt_functor { - diff_square_pt2pt_functor(const Eigen::Vector3f* source, - const Eigen::Vector3f* target, - const Eigen::Vector2i* corres) - : source_(source), target_(target), corres_(corres) {}; - const Eigen::Vector3f* source_; - const Eigen::Vector3f* target_; - const Eigen::Vector2i* corres_; - __device__ - float operator()(size_t idx) const { - return (source_[corres_[idx][0]] - target_[corres_[idx][1]]).squaredNorm(); + diff_square_pt2pt_functor(const Eigen::Vector3f *source, + const Eigen::Vector3f *target, + const Eigen::Vector2i *corres) + : source_(source), target_(target), corres_(corres){}; + const Eigen::Vector3f *source_; + const Eigen::Vector3f *target_; + const Eigen::Vector2i *corres_; + __device__ float operator()(size_t idx) const { + return (source_[corres_[idx][0]] - target_[corres_[idx][1]]) + .squaredNorm(); } }; struct diff_square_pt2pl_functor { - diff_square_pt2pl_functor(const Eigen::Vector3f* source, - const Eigen::Vector3f* target_points, - const Eigen::Vector3f* target_normals, - const Eigen::Vector2i* corres) - : source_(source), target_points_(target_points), target_normals_(target_normals), corres_(corres) {}; - const Eigen::Vector3f* source_; - const Eigen::Vector3f* target_points_; - const Eigen::Vector3f* target_normals_; - const Eigen::Vector2i* corres_; - __device__ - float operator()(size_t idx) const { - float r = (source_[corres_[idx][0]] - target_points_[corres_[idx][1]]).dot(target_normals_[corres_[idx][1]]); + diff_square_pt2pl_functor(const Eigen::Vector3f *source, + const Eigen::Vector3f *target_points, + const Eigen::Vector3f *target_normals, + const Eigen::Vector2i *corres) + : source_(source), + target_points_(target_points), + target_normals_(target_normals), + corres_(corres){}; + const Eigen::Vector3f *source_; + const Eigen::Vector3f *target_points_; + const Eigen::Vector3f *target_normals_; + const Eigen::Vector2i *corres_; + __device__ float operator()(size_t idx) const { + float r = (source_[corres_[idx][0]] - target_points_[corres_[idx][1]]) + .dot(target_normals_[corres_[idx][1]]); return r * r; } }; -struct pt2pl_jacobian_residual_functor : public utility::jacobian_residual_functor { - pt2pl_jacobian_residual_functor(const Eigen::Vector3f* source, - const Eigen::Vector3f* target_points, - const Eigen::Vector3f* target_normals, - const Eigen::Vector2i* corres) - : source_(source), target_points_(target_points), target_normals_(target_normals), corres_(corres) {}; - const Eigen::Vector3f* source_; - const Eigen::Vector3f* target_points_; - const Eigen::Vector3f* target_normals_; - const Eigen::Vector2i* corres_; - __device__ - void operator() (int idx, Eigen::Vector6f& vec, float& r) const { +struct pt2pl_jacobian_residual_functor + : public utility::jacobian_residual_functor { + pt2pl_jacobian_residual_functor(const Eigen::Vector3f *source, + const Eigen::Vector3f *target_points, + const Eigen::Vector3f *target_normals, + const Eigen::Vector2i *corres) + : source_(source), + target_points_(target_points), + target_normals_(target_normals), + corres_(corres){}; + const Eigen::Vector3f *source_; + const Eigen::Vector3f *target_points_; + const Eigen::Vector3f *target_normals_; + const Eigen::Vector2i *corres_; + __device__ void operator()(int idx, Eigen::Vector6f &vec, float &r) const { const Eigen::Vector3f &vs = source_[corres_[idx][0]]; const Eigen::Vector3f &vt = target_points_[corres_[idx][1]]; const Eigen::Vector3f &nt = target_normals_[corres_[idx][1]]; @@ -61,59 +69,66 @@ struct pt2pl_jacobian_residual_functor : public utility::jacobian_residual_funct } }; -} +} // namespace float TransformationEstimationPointToPoint::ComputeRMSE( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - const CorrespondenceSet &corres) const { - diff_square_pt2pt_functor func(thrust::raw_pointer_cast(source.points_.data()), - thrust::raw_pointer_cast(target.points_.data()), - thrust::raw_pointer_cast(corres.data())); - const float err = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(corres.size()), - func, 0.0f, thrust::plus()); + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { + diff_square_pt2pt_functor func( + thrust::raw_pointer_cast(source.points_.data()), + thrust::raw_pointer_cast(target.points_.data()), + thrust::raw_pointer_cast(corres.data())); + const float err = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(corres.size()), func, 0.0f, + thrust::plus()); return std::sqrt(err / (float)corres.size()); } Eigen::Matrix4f TransformationEstimationPointToPoint::ComputeTransformation( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - const CorrespondenceSet &corres) const { + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { return Kabsch(source.points_, target.points_, corres); } float TransformationEstimationPointToPlane::ComputeRMSE( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - const CorrespondenceSet &corres) const { + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { if (corres.empty() || !target.HasNormals()) return 0.0; - diff_square_pt2pl_functor func(thrust::raw_pointer_cast(source.points_.data()), - thrust::raw_pointer_cast(target.points_.data()), - thrust::raw_pointer_cast(target.normals_.data()), - thrust::raw_pointer_cast(corres.data())); - const float err = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(corres.size()), - func, 0.0f, thrust::plus()); + diff_square_pt2pl_functor func( + thrust::raw_pointer_cast(source.points_.data()), + thrust::raw_pointer_cast(target.points_.data()), + thrust::raw_pointer_cast(target.normals_.data()), + thrust::raw_pointer_cast(corres.data())); + const float err = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(corres.size()), func, 0.0f, + thrust::plus()); return std::sqrt(err / (float)corres.size()); } Eigen::Matrix4f TransformationEstimationPointToPlane::ComputeTransformation( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - const CorrespondenceSet &corres) const { - if (corres.empty() || !target.HasNormals()) return Eigen::Matrix4f::Identity(); + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { + if (corres.empty() || !target.HasNormals()) + return Eigen::Matrix4f::Identity(); Eigen::Matrix6f JTJ; Eigen::Vector6f JTr; float r2; - pt2pl_jacobian_residual_functor func(thrust::raw_pointer_cast(source.points_.data()), - thrust::raw_pointer_cast(target.points_.data()), - thrust::raw_pointer_cast(target.normals_.data()), - thrust::raw_pointer_cast(corres.data())); + pt2pl_jacobian_residual_functor func( + thrust::raw_pointer_cast(source.points_.data()), + thrust::raw_pointer_cast(target.points_.data()), + thrust::raw_pointer_cast(target.normals_.data()), + thrust::raw_pointer_cast(corres.data())); thrust::tie(JTJ, JTr, r2) = - utility::ComputeJTJandJTr( - func, (int)corres.size()); + utility::ComputeJTJandJTr( + func, (int)corres.size()); bool is_success; Eigen::Matrix4f extrinsic; diff --git a/src/cupoch/registration/transformation_estimation.h b/src/cupoch/registration/transformation_estimation.h index 573486bd..012d0e5e 100644 --- a/src/cupoch/registration/transformation_estimation.h +++ b/src/cupoch/registration/transformation_estimation.h @@ -2,6 +2,7 @@ #include #include + #include "cupoch/utility/device_vector.h" namespace cupoch { @@ -89,5 +90,5 @@ class TransformationEstimationPointToPlane : public TransformationEstimation { TransformationEstimationType::PointToPlane; }; -} -} \ No newline at end of file +} // namespace registration +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/utility/console.cpp b/src/cupoch/utility/console.cpp index 13033c17..ed356a30 100644 --- a/src/cupoch/utility/console.cpp +++ b/src/cupoch/utility/console.cpp @@ -1,10 +1,10 @@ #include "cupoch/utility/console.h" +#include + #include #include -#include - #ifdef _WIN32 #include #endif diff --git a/src/cupoch/utility/console.h b/src/cupoch/utility/console.h index 7b03e7ed..a865bd8d 100644 --- a/src/cupoch/utility/console.h +++ b/src/cupoch/utility/console.h @@ -3,10 +3,9 @@ #include #undef __SIZEOF_INT128__ +#include #include #include -#include - #define DEFAULT_IO_BUFFER_SIZE 1024 diff --git a/src/cupoch/utility/device_vector.h b/src/cupoch/utility/device_vector.h index 690e4c00..55c7f923 100644 --- a/src/cupoch/utility/device_vector.h +++ b/src/cupoch/utility/device_vector.h @@ -10,17 +10,18 @@ namespace cupoch { namespace utility { #ifdef USE_RMM -template +template using device_vector = rmm::device_vector; inline decltype(auto) exec_policy(cudaStream_t stream = 0) { return rmm::exec_policy(stream); } -inline void InitializeAllocator(rmmAllocationMode_t mode = CudaDefaultAllocation, - size_t initial_pool_size = 0, - bool logging = false, - const std::vector& devices = {}) { +inline void InitializeAllocator( + rmmAllocationMode_t mode = CudaDefaultAllocation, + size_t initial_pool_size = 0, + bool logging = false, + const std::vector &devices = {}) { static bool is_initialized = false; if (is_initialized) rmmFinalize(); rmmOptions_t options = {mode, initial_pool_size, logging, devices}; @@ -29,19 +30,20 @@ inline void InitializeAllocator(rmmAllocationMode_t mode = CudaDefaultAllocation } #else -template +template using device_vector = thrust::device_vector; inline decltype(auto) exec_policy(cudaStream_t stream = 0) { return &thrust::cuda::par; } -inline void InitializeAllocator(rmmAllocationMode_t mode = CudaDefaultAllocation, - size_t initial_pool_size = 0, - bool logging = false, - const std::vector& devices = {}) {} +inline void InitializeAllocator( + rmmAllocationMode_t mode = CudaDefaultAllocation, + size_t initial_pool_size = 0, + bool logging = false, + const std::vector &devices = {}) {} #endif -} -} \ No newline at end of file +} // namespace utility +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/utility/dl_converter.cu b/src/cupoch/utility/dl_converter.cu index fb1cb464..41638f7e 100644 --- a/src/cupoch/utility/dl_converter.cu +++ b/src/cupoch/utility/dl_converter.cu @@ -1,13 +1,14 @@ +#include + #include "cupoch/utility/dl_converter.h" #include "cupoch/utility/platform.h" -#include using namespace cupoch; using namespace cupoch::utility; namespace { -template +template DLDataTypeCode GetDLDataTypeCode() { if (std::is_same::value) { return DLDataTypeCode::kDLInt; @@ -22,27 +23,29 @@ DLDataTypeCode GetDLDataTypeCode() { } } -template +template struct DeviceVectorDLMTensor { thrust::device_vector> handle; DLManagedTensor tensor; }; -template -void deleter(DLManagedTensor* arg) { +template +void deleter(DLManagedTensor *arg) { delete[] arg->dl_tensor.shape; - delete static_cast*>(arg->manager_ctx); + delete static_cast *>(arg->manager_ctx); } -} +} // namespace -template -DLManagedTensor* cupoch::utility::ToDLPack(const utility::device_vector>& src) { - DeviceVectorDLMTensor* dvdl(new DeviceVectorDLMTensor); +template +DLManagedTensor *cupoch::utility::ToDLPack( + const utility::device_vector> &src) { + DeviceVectorDLMTensor *dvdl(new DeviceVectorDLMTensor); dvdl->handle = src; dvdl->tensor.manager_ctx = dvdl; dvdl->tensor.deleter = &deleter; - dvdl->tensor.dl_tensor.data = const_cast((const void*)(thrust::raw_pointer_cast(src.data()))); + dvdl->tensor.dl_tensor.data = const_cast( + (const void *)(thrust::raw_pointer_cast(src.data()))); int64_t device_id = GetDevice(); DLContext ctx; ctx.device_id = device_id; @@ -54,7 +57,7 @@ DLManagedTensor* cupoch::utility::ToDLPack(const utility::device_vector(); dvdl->tensor.dl_tensor.dtype = dtype; - int64_t* shape = new int64_t[2]; + int64_t *shape = new int64_t[2]; shape[0] = src.size(); shape[1] = Dim; dvdl->tensor.dl_tensor.shape = shape; @@ -63,27 +66,39 @@ DLManagedTensor* cupoch::utility::ToDLPack(const utility::device_vectortensor); } -template DLManagedTensor* cupoch::utility::ToDLPack(const utility::device_vector>& src); -template DLManagedTensor* cupoch::utility::ToDLPack(const utility::device_vector>& src); -template DLManagedTensor* cupoch::utility::ToDLPack(const utility::device_vector>& src); +template DLManagedTensor *cupoch::utility::ToDLPack( + const utility::device_vector> &src); +template DLManagedTensor *cupoch::utility::ToDLPack( + const utility::device_vector> &src); +template DLManagedTensor *cupoch::utility::ToDLPack( + const utility::device_vector> &src); -template<> -void cupoch::utility::FromDLPack(const DLManagedTensor* src, utility::device_vector>& dst) { +template <> +void cupoch::utility::FromDLPack( + const DLManagedTensor *src, + utility::device_vector> &dst) { dst.resize(src->dl_tensor.shape[0]); - auto base_ptr = thrust::device_pointer_cast((Eigen::Matrix*)src->dl_tensor.data); + auto base_ptr = thrust::device_pointer_cast( + (Eigen::Matrix *)src->dl_tensor.data); thrust::copy(base_ptr, base_ptr + src->dl_tensor.shape[0], dst.begin()); } -template<> -void cupoch::utility::FromDLPack(const DLManagedTensor* src, utility::device_vector>& dst) { +template <> +void cupoch::utility::FromDLPack( + const DLManagedTensor *src, + utility::device_vector> &dst) { dst.resize(src->dl_tensor.shape[0]); - auto base_ptr = thrust::device_pointer_cast((Eigen::Matrix*)src->dl_tensor.data); + auto base_ptr = thrust::device_pointer_cast( + (Eigen::Matrix *)src->dl_tensor.data); thrust::copy(base_ptr, base_ptr + src->dl_tensor.shape[0], dst.begin()); } -template<> -void cupoch::utility::FromDLPack(const DLManagedTensor* src, utility::device_vector>& dst) { +template <> +void cupoch::utility::FromDLPack( + const DLManagedTensor *src, + utility::device_vector> &dst) { dst.resize(src->dl_tensor.shape[0]); - auto base_ptr = thrust::device_pointer_cast((Eigen::Matrix*)src->dl_tensor.data); + auto base_ptr = thrust::device_pointer_cast( + (Eigen::Matrix *)src->dl_tensor.data); thrust::copy(base_ptr, base_ptr + src->dl_tensor.shape[0], dst.begin()); } \ No newline at end of file diff --git a/src/cupoch/utility/dl_converter.h b/src/cupoch/utility/dl_converter.h index 0ac6b47b..75960445 100644 --- a/src/cupoch/utility/dl_converter.h +++ b/src/cupoch/utility/dl_converter.h @@ -1,17 +1,21 @@ #pragma once +#include + #include + #include "cupoch/utility/device_vector.h" -#include namespace cupoch { namespace utility { -template -DLManagedTensor* ToDLPack(const utility::device_vector>& src); +template +DLManagedTensor *ToDLPack( + const utility::device_vector> &src); -template -void FromDLPack(const DLManagedTensor* src, utility::device_vector>& dst); +template +void FromDLPack(const DLManagedTensor *src, + utility::device_vector> &dst); -} -} \ No newline at end of file +} // namespace utility +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/utility/eigen.cu b/src/cupoch/utility/eigen.cu index f15d0b09..01274b45 100644 --- a/src/cupoch/utility/eigen.cu +++ b/src/cupoch/utility/eigen.cu @@ -1,10 +1,12 @@ -#include "cupoch/utility/eigen.h" #include +#include "cupoch/utility/eigen.h" + using namespace cupoch; using namespace cupoch::utility; -Eigen::Matrix4f cupoch::utility::TransformVector6fToMatrix4f(const Eigen::Vector6f &input) { +Eigen::Matrix4f cupoch::utility::TransformVector6fToMatrix4f( + const Eigen::Vector6f &input) { Eigen::Matrix4f output = Eigen::Matrix4f::Identity(); output.block<3, 3>(0, 0) = (Eigen::AngleAxisf(input(2), Eigen::Vector3f::UnitZ()) * @@ -15,12 +17,12 @@ Eigen::Matrix4f cupoch::utility::TransformVector6fToMatrix4f(const Eigen::Vector return output; } -template -thrust::tuple> cupoch::utility::SolveLinearSystemPSD( - const Eigen::Matrix &A, - const Eigen::Matrix &b, - bool check_symmetric, - bool check_det) { +template +thrust::tuple> +cupoch::utility::SolveLinearSystemPSD(const Eigen::Matrix &A, + const Eigen::Matrix &b, + bool check_symmetric, + bool check_det) { // PSD implies symmetric if (check_symmetric && !A.isApprox(A.transpose())) { LogWarning("check_symmetric failed, empty vector will be returned"); @@ -31,7 +33,8 @@ thrust::tuple> cupoch::utility::SolveLinearSy float det = A.determinant(); if (fabs(det) < 1e-6 || std::isnan(det) || std::isinf(det)) { LogWarning("check_det failed, empty vector will be returned"); - return thrust::make_tuple(false, Eigen::Matrix::Zero()); + return thrust::make_tuple(false, + Eigen::Matrix::Zero()); } } @@ -41,11 +44,13 @@ thrust::tuple> cupoch::utility::SolveLinearSy return thrust::make_tuple(true, std::move(x)); } -thrust::tuple cupoch::utility::SolveJacobianSystemAndObtainExtrinsicMatrix( - const Eigen::Matrix6f &JTJ, const Eigen::Vector6f &JTr) { +thrust::tuple +cupoch::utility::SolveJacobianSystemAndObtainExtrinsicMatrix( + const Eigen::Matrix6f &JTJ, const Eigen::Vector6f &JTr) { bool solution_exist; Eigen::Vector6f x; - thrust::tie(solution_exist, x) = SolveLinearSystemPSD(JTJ, Eigen::Vector6f(-JTr)); + thrust::tie(solution_exist, x) = + SolveLinearSystemPSD(JTJ, Eigen::Vector6f(-JTr)); if (solution_exist) { Eigen::Matrix4f extrinsic = TransformVector6fToMatrix4f(x); @@ -55,12 +60,11 @@ thrust::tuple cupoch::utility::SolveJacobianSystemAndObta } } -template thrust::tuple cupoch::utility::SolveLinearSystemPSD( - const Eigen::Matrix6f &A, - const Eigen::Vector6f &b, - bool check_symmetric, - bool check_det); - +template thrust::tuple +cupoch::utility::SolveLinearSystemPSD(const Eigen::Matrix6f &A, + const Eigen::Vector6f &b, + bool check_symmetric, + bool check_det); Eigen::Matrix3f cupoch::utility::RotationMatrixX(float radians) { Eigen::Matrix3f rot; diff --git a/src/cupoch/utility/eigen.h b/src/cupoch/utility/eigen.h index c5c25320..d780c3ca 100644 --- a/src/cupoch/utility/eigen.h +++ b/src/cupoch/utility/eigen.h @@ -1,7 +1,8 @@ #pragma once -#include #include +#include + namespace Eigen { /// Extending Eigen namespace by adding frequently used matrix type @@ -18,14 +19,14 @@ namespace utility { template struct jacobian_residual_functor { - __device__ - virtual void operator() (int i, VecType& vec, float& r) const = 0; + __device__ virtual void operator()(int i, VecType &vec, float &r) const = 0; }; template struct multiple_jacobians_residuals_functor { - __device__ - virtual void operator() (int i, VecType J_r[NumJ], float r[NumJ]) const = 0; + __device__ virtual void operator()(int i, + VecType J_r[NumJ], + float r[NumJ]) const = 0; }; /// Function to transform 6D motion vector to 4D motion matrix @@ -34,7 +35,7 @@ struct multiple_jacobians_residuals_functor { Eigen::Matrix4f TransformVector6fToMatrix4f(const Eigen::Vector6f &input); /// Function to solve Ax=b -template +template thrust::tuple> SolveLinearSystemPSD( const Eigen::Matrix &A, const Eigen::Matrix &b, @@ -44,8 +45,9 @@ thrust::tuple> SolveLinearSystemPSD( /// Function to solve Jacobian system /// Input: 6x6 Jacobian matrix and 6-dim residual vector. /// Output: tuple of is_success, 4x4 extrinsic matrices. -thrust::tuple SolveJacobianSystemAndObtainExtrinsicMatrix( - const Eigen::Matrix6f &JTJ, const Eigen::Vector6f &JTr); +thrust::tuple +SolveJacobianSystemAndObtainExtrinsicMatrix(const Eigen::Matrix6f &JTJ, + const Eigen::Vector6f &JTr); /// Function to compute JTJ and Jtr /// Input: function pointer f and total number of rows of Jacobian matrix @@ -53,10 +55,9 @@ thrust::tuple SolveJacobianSystemAndObtainExtrinsicMatrix /// Note: f takes index of row, and outputs corresponding residual and row /// vector. template -thrust::tuple ComputeJTJandJTr( - const FuncType& f, - int iteration_num, - bool verbose = true); +thrust::tuple ComputeJTJandJTr(const FuncType &f, + int iteration_num, + bool verbose = true); /// Function to compute JTJ and Jtr /// Input: function pointer f and total number of rows of Jacobian matrix @@ -64,10 +65,9 @@ thrust::tuple ComputeJTJandJTr( /// Note: f takes index of row, and outputs corresponding residual and row /// vector. template -thrust::tuple ComputeJTJandJTr( - const FuncType& f, - int iteration_num, - bool verbose = true); +thrust::tuple ComputeJTJandJTr(const FuncType &f, + int iteration_num, + bool verbose = true); Eigen::Matrix3f RotationMatrixX(float radians); Eigen::Matrix3f RotationMatrixY(float radians); diff --git a/src/cupoch/utility/eigen.inl b/src/cupoch/utility/eigen.inl index 9e86ecf3..9dcbe283 100644 --- a/src/cupoch/utility/eigen.inl +++ b/src/cupoch/utility/eigen.inl @@ -1,20 +1,21 @@ -#include "cupoch/utility/eigen.h" -#include "cupoch/utility/helper.h" -#include "cupoch/utility/console.h" #include #include +#include "cupoch/utility/console.h" +#include "cupoch/utility/eigen.h" +#include "cupoch/utility/helper.h" + namespace cupoch { namespace utility { namespace { -template +template struct jtj_jtr_reduce_functor { - jtj_jtr_reduce_functor(const FuncType& f) : f_(f) {}; + jtj_jtr_reduce_functor(const FuncType &f) : f_(f){}; const FuncType f_; - __device__ - thrust::tuple operator() (int idx) const { + __device__ thrust::tuple operator()( + int idx) const { VecType J_r; float r; f_(idx, J_r, r); @@ -24,12 +25,12 @@ struct jtj_jtr_reduce_functor { } }; -template +template struct multiple_jtj_jtr_reduce_functor { - multiple_jtj_jtr_reduce_functor(const FuncType& f) : f_(f) {}; + multiple_jtj_jtr_reduce_functor(const FuncType &f) : f_(f){}; const FuncType f_; - __device__ - thrust::tuple operator() (int idx) const { + __device__ thrust::tuple operator()( + int idx) const { MatType JTJ_private; VecType JTr_private; float r2_sum_private = 0.0; @@ -47,23 +48,23 @@ struct multiple_jtj_jtr_reduce_functor { } }; -} +} // namespace template -thrust::tuple ComputeJTJandJTr( - const FuncType& f, - int iteration_num, - bool verbose) { +thrust::tuple ComputeJTJandJTr(const FuncType &f, + int iteration_num, + bool verbose) { MatType JTJ; VecType JTr; float r2_sum = 0.0; JTJ.setZero(); JTr.setZero(); jtj_jtr_reduce_functor func(f); - auto jtj_jtr_r2 = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(iteration_num), - func, thrust::make_tuple(JTJ, JTr, r2_sum), - thrust::plus>()); + auto jtj_jtr_r2 = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(iteration_num), func, + thrust::make_tuple(JTJ, JTr, r2_sum), + thrust::plus>()); r2_sum = thrust::get<2>(jtj_jtr_r2); if (verbose) { LogDebug("Residual : {:.2e} (# of elements : {:d})", @@ -74,19 +75,18 @@ thrust::tuple ComputeJTJandJTr( template thrust::tuple ComputeJTJandJTr( - const FuncType& f, - int iteration_num, - bool verbose /*=true*/) { + const FuncType &f, int iteration_num, bool verbose /*=true*/) { MatType JTJ; VecType JTr; float r2_sum = 0.0; JTJ.setZero(); JTr.setZero(); multiple_jtj_jtr_reduce_functor func(f); - auto jtj_jtr_r2 = thrust::transform_reduce(thrust::make_counting_iterator(0), - thrust::make_counting_iterator(iteration_num), - func, thrust::make_tuple(JTJ, JTr, r2_sum), - thrust::plus>()); + auto jtj_jtr_r2 = thrust::transform_reduce( + thrust::make_counting_iterator(0), + thrust::make_counting_iterator(iteration_num), func, + thrust::make_tuple(JTJ, JTr, r2_sum), + thrust::plus>()); r2_sum = thrust::get<2>(jtj_jtr_r2); if (verbose) { LogDebug("Residual : {:.2e} (# of elements : {:d})", diff --git a/src/cupoch/utility/filesystem.cpp b/src/cupoch/utility/filesystem.cpp index 258c5808..de382b24 100644 --- a/src/cupoch/utility/filesystem.cpp +++ b/src/cupoch/utility/filesystem.cpp @@ -84,6 +84,6 @@ FILE *FOpen(const std::string &filename, const std::string &mode) { return fp; } -} -} -} \ No newline at end of file +} // namespace filesystem +} // namespace utility +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/utility/filesystem.h b/src/cupoch/utility/filesystem.h index 85bf6898..5822f1d7 100644 --- a/src/cupoch/utility/filesystem.h +++ b/src/cupoch/utility/filesystem.h @@ -22,6 +22,6 @@ bool ChangeWorkingDirectory(const std::string &directory); // wrapper for fopen that enables unicode paths on Windows FILE *FOpen(const std::string &filename, const std::string &mode); -} -} -} \ No newline at end of file +} // namespace filesystem +} // namespace utility +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/utility/helper.cpp b/src/cupoch/utility/helper.cpp index f30d26d5..33082717 100644 --- a/src/cupoch/utility/helper.cpp +++ b/src/cupoch/utility/helper.cpp @@ -5,9 +5,9 @@ using namespace cupoch; using namespace cupoch::utility; -void cupoch::utility::SplitString(std::vector& tokens, - const std::string& str, - const std::string& delimiters /* = " "*/, +void cupoch::utility::SplitString(std::vector &tokens, + const std::string &str, + const std::string &delimiters /* = " "*/, bool trim_empty_str /* = true*/) { std::string::size_type pos = 0, new_pos = 0, last_pos = 0; while (pos != std::string::npos) { @@ -20,29 +20,32 @@ void cupoch::utility::SplitString(std::vector& tokens, } } -std::string& cupoch::utility::LeftStripString(std::string& str, const std::string& chars) { +std::string &cupoch::utility::LeftStripString(std::string &str, + const std::string &chars) { str.erase(0, str.find_first_not_of(chars)); return str; } -std::string& cupoch::utility::RightStripString(std::string& str, const std::string& chars) { +std::string &cupoch::utility::RightStripString(std::string &str, + const std::string &chars) { str.erase(str.find_last_not_of(chars) + 1); return str; } -std::string& cupoch::utility::StripString(std::string& str, const std::string& chars) { +std::string &cupoch::utility::StripString(std::string &str, + const std::string &chars) { return LeftStripString(RightStripString(str, chars), chars); } // Count the length of current word starting from start_pos -size_t cupoch::utility::WordLength(const std::string& doc, +size_t cupoch::utility::WordLength(const std::string &doc, size_t start_pos, - const std::string& valid_chars) { + const std::string &valid_chars) { std::unordered_set valid_chars_set; - for (const char& c : valid_chars) { + for (const char &c : valid_chars) { valid_chars_set.insert(c); } - auto is_word_char = [&valid_chars_set](const char& c) { + auto is_word_char = [&valid_chars_set](const char &c) { return std::isalnum(c) || valid_chars_set.find(c) != valid_chars_set.end(); }; diff --git a/src/cupoch/utility/helper.h b/src/cupoch/utility/helper.h index 0fd6b26f..7352403a 100644 --- a/src/cupoch/utility/helper.h +++ b/src/cupoch/utility/helper.h @@ -1,34 +1,40 @@ #pragma once -#include -#include -#include #include -#include #include +#include + +#include +#include +#include + #include "cupoch/utility/device_vector.h" #include "cupoch/utility/platform.h" namespace thrust { -template +template struct equal_to> { typedef Eigen::Matrix first_argument_type; typedef Eigen::Matrix second_argument_type; typedef bool result_type; + // clang-format off __thrust_exec_check_disable__ - __host__ __device__ bool operator()(const Eigen::Matrix &lhs, const Eigen::Matrix &rhs) const { + __host__ __device__ bool operator()( + const Eigen::Matrix &lhs, + const Eigen::Matrix &rhs) const { for (int i = 0; i < Dim; ++i) { if (lhs[i] != rhs[i]) return false; } return true; } + // clang-format on }; -template +template struct plus> { - __host__ __device__ - thrust::tuple operator() (const thrust::tuple& x, - const thrust::tuple& y) const { + __host__ __device__ thrust::tuple operator()( + const thrust::tuple &x, + const thrust::tuple &y) const { MatType mat = thrust::get<0>(x) + thrust::get<0>(y); VecType vec = thrust::get<1>(x) + thrust::get<1>(y); float r = thrust::get<2>(x) + thrust::get<2>(y); @@ -36,73 +42,86 @@ struct plus> { } }; -} +} // namespace thrust namespace Eigen { -template -__host__ __device__ -bool operator<(const Eigen::Matrix &lhs, const Eigen::Matrix &rhs) { +template +__host__ __device__ bool operator<(const Eigen::Matrix &lhs, + const Eigen::Matrix &rhs) { for (int i = 0; i < Dim; ++i) { if (lhs[i] != rhs[i]) return lhs[i] < rhs[i]; } return false; } -__host__ __device__ -inline bool operator==(const Eigen::Vector3i &lhs, const Eigen::Vector3i &rhs) { +__host__ __device__ inline bool operator==(const Eigen::Vector3i &lhs, + const Eigen::Vector3i &rhs) { return (lhs[0] == rhs[0] && lhs[1] == rhs[1] && lhs[2] == rhs[2]); } -__host__ __device__ -inline bool operator!=(const Eigen::Vector3i &lhs, const Eigen::Vector3i &rhs) { +__host__ __device__ inline bool operator!=(const Eigen::Vector3i &lhs, + const Eigen::Vector3i &rhs) { return (lhs[0] != rhs[0] || lhs[1] != rhs[1] || lhs[2] != rhs[2]); } -__host__ __device__ -inline bool operator!=(const Eigen::Vector3f &lhs, const Eigen::Vector3f &rhs) { +__host__ __device__ inline bool operator!=(const Eigen::Vector3f &lhs, + const Eigen::Vector3f &rhs) { return (lhs[0] != rhs[0] || lhs[1] != rhs[1] || lhs[2] != rhs[2]); } -template -__host__ __device__ -bool device_any(const ArrayType& array) { +template +__host__ __device__ bool device_any(const ArrayType &array) { for (int i = 0; i < array.size(); ++i) { if (array[i]) return true; } return false; } -} +} // namespace Eigen namespace cupoch { -template -struct add_tuple_functor : public thrust::binary_function, const thrust::tuple, thrust::tuple> { - __host__ __device__ - thrust::tuple operator()(const thrust::tuple& x, const thrust::tuple& y) const; +template +struct add_tuple_functor + : public thrust::binary_function, + const thrust::tuple, + thrust::tuple> { + __host__ __device__ thrust::tuple operator()( + const thrust::tuple &x, + const thrust::tuple &y) const; }; -template -struct devided_tuple_functor : public thrust::binary_function, const int, thrust::tuple> { - __host__ __device__ - thrust::tuple operator()(const thrust::tuple& x, const int& y) const; +template +struct devided_tuple_functor + : public thrust::binary_function, + const int, + thrust::tuple> { + __host__ __device__ thrust::tuple operator()( + const thrust::tuple &x, const int &y) const; }; -template -struct add_tuple_functor : public thrust::binary_function, const thrust::tuple, thrust::tuple> { - __host__ __device__ - thrust::tuple operator()(const thrust::tuple& x, const thrust::tuple& y) const { +template +struct add_tuple_functor + : public thrust::binary_function, + const thrust::tuple, + thrust::tuple> { + __host__ __device__ thrust::tuple operator()( + const thrust::tuple &x, const thrust::tuple &y) const { thrust::tuple ans; thrust::get<0>(ans) = thrust::get<0>(x) + thrust::get<0>(y); return ans; } }; -template -struct add_tuple_functor : public thrust::binary_function, const thrust::tuple, thrust::tuple> { - __host__ __device__ - thrust::tuple operator()(const thrust::tuple& x, const thrust::tuple& y) const { +template +struct add_tuple_functor + : public thrust::binary_function, + const thrust::tuple, + thrust::tuple> { + __host__ __device__ thrust::tuple operator()( + const thrust::tuple &x, + const thrust::tuple &y) const { thrust::tuple ans; thrust::get<0>(ans) = thrust::get<0>(x) + thrust::get<0>(y); thrust::get<1>(ans) = thrust::get<1>(x) + thrust::get<1>(y); @@ -110,10 +129,14 @@ struct add_tuple_functor : public thrust::binary_function -struct add_tuple_functor : public thrust::binary_function, const thrust::tuple, thrust::tuple> { - __host__ __device__ - thrust::tuple operator()(const thrust::tuple& x, const thrust::tuple& y) const { +template +struct add_tuple_functor + : public thrust::binary_function, + const thrust::tuple, + thrust::tuple> { + __host__ __device__ thrust::tuple operator()( + const thrust::tuple &x, + const thrust::tuple &y) const { thrust::tuple ans; thrust::get<0>(ans) = thrust::get<0>(x) + thrust::get<0>(y); thrust::get<1>(ans) = thrust::get<1>(x) + thrust::get<1>(y); @@ -122,20 +145,26 @@ struct add_tuple_functor : public thrust::binary_function -struct devided_tuple_functor : public thrust::binary_function, const int, thrust::tuple> { - __host__ __device__ - thrust::tuple operator()(const thrust::tuple& x, const int& y) const { +template +struct devided_tuple_functor + : public thrust::binary_function, + const int, + thrust::tuple> { + __host__ __device__ thrust::tuple operator()(const thrust::tuple &x, + const int &y) const { thrust::tuple ans; thrust::get<0>(ans) = thrust::get<0>(x) / static_cast(y); return ans; } }; -template -struct devided_tuple_functor : public thrust::binary_function, const int, thrust::tuple> { - __host__ __device__ - thrust::tuple operator()(const thrust::tuple& x, const int& y) const { +template +struct devided_tuple_functor + : public thrust::binary_function, + const int, + thrust::tuple> { + __host__ __device__ thrust::tuple operator()( + const thrust::tuple &x, const int &y) const { thrust::tuple ans; thrust::get<0>(ans) = thrust::get<0>(x) / static_cast(y); thrust::get<1>(ans) = thrust::get<1>(x) / static_cast(y); @@ -143,10 +172,13 @@ struct devided_tuple_functor : public thrust::binary_function -struct devided_tuple_functor : public thrust::binary_function, const int, thrust::tuple> { - __host__ __device__ - thrust::tuple operator()(const thrust::tuple& x, const int& y) const { +template +struct devided_tuple_functor + : public thrust::binary_function, + const int, + thrust::tuple> { + __host__ __device__ thrust::tuple operator()( + const thrust::tuple &x, const int &y) const { thrust::tuple ans; thrust::get<0>(ans) = thrust::get<0>(x) / static_cast(y); thrust::get<1>(ans) = thrust::get<1>(x) / static_cast(y); @@ -155,13 +187,15 @@ struct devided_tuple_functor : public thrust::binary_function -thrust::zip_iterator > make_tuple_iterator(const Args&... args) { +template +thrust::zip_iterator> make_tuple_iterator( + const Args &... args) { return thrust::make_zip_iterator(thrust::make_tuple(args...)); } -template -thrust::zip_iterator > make_tuple_iterator(Args&... args) { +template +thrust::zip_iterator> make_tuple_iterator( + Args &... args) { return thrust::make_zip_iterator(thrust::make_tuple(args...)); } @@ -169,54 +203,60 @@ namespace utility { /// Function to split a string, mimics boost::split /// http://stackoverflow.com/questions/236129/split-a-string-in-c -void SplitString(std::vector& tokens, - const std::string& str, - const std::string& delimiters = " ", +void SplitString(std::vector &tokens, + const std::string &str, + const std::string &delimiters = " ", bool trim_empty_str = true); /// String util: find length of current word staring from a position /// By default, alpha numeric chars and chars in valid_chars are considered /// as valid charactors in a word -size_t WordLength(const std::string& doc, +size_t WordLength(const std::string &doc, size_t start_pos, - const std::string& valid_chars = "_"); + const std::string &valid_chars = "_"); -std::string& LeftStripString(std::string& str, - const std::string& chars = "\t\n\v\f\r "); +std::string &LeftStripString(std::string &str, + const std::string &chars = "\t\n\v\f\r "); -std::string& RightStripString(std::string& str, - const std::string& chars = "\t\n\v\f\r "); +std::string &RightStripString(std::string &str, + const std::string &chars = "\t\n\v\f\r "); /// Strip empty charactors in front and after string. Similar to Python's /// str.strip() -std::string& StripString(std::string& str, - const std::string& chars = "\t\n\v\f\r "); +std::string &StripString(std::string &str, + const std::string &chars = "\t\n\v\f\r "); -template -void CopyToDeviceMultiStream(const thrust::host_vector& src, utility::device_vector& dst, +template +void CopyToDeviceMultiStream(const thrust::host_vector &src, + utility::device_vector &dst, int n_stream = MAX_NUM_STREAMS) { const int step = src.size() / n_stream; int step_size = step * sizeof(T); for (int i = 0; i < n_stream; ++i) { const int offset = i * step; - if (i == n_stream - 1) step_size = (src.size() - step * (n_stream - 1)) * sizeof(T); - cudaMemcpyAsync(thrust::raw_pointer_cast(&dst[offset]), thrust::raw_pointer_cast(&src[offset]), - step_size, cudaMemcpyHostToDevice, GetStream(i)); + if (i == n_stream - 1) + step_size = (src.size() - step * (n_stream - 1)) * sizeof(T); + cudaMemcpyAsync(thrust::raw_pointer_cast(&dst[offset]), + thrust::raw_pointer_cast(&src[offset]), step_size, + cudaMemcpyHostToDevice, GetStream(i)); } } -template -void CopyFromDeviceMultiStream(const utility::device_vector& src, thrust::host_vector& dst, +template +void CopyFromDeviceMultiStream(const utility::device_vector &src, + thrust::host_vector &dst, int n_stream = MAX_NUM_STREAMS) { const int step = src.size() / n_stream; int step_size = step * sizeof(T); for (int i = 0; i < n_stream; ++i) { const int offset = i * step; - if (i == n_stream - 1) step_size = (src.size() - step * (n_stream - 1)) * sizeof(T); - cudaMemcpyAsync(thrust::raw_pointer_cast(&dst[offset]), thrust::raw_pointer_cast(&src[offset]), - step_size, cudaMemcpyDeviceToHost, GetStream(i)); + if (i == n_stream - 1) + step_size = (src.size() - step * (n_stream - 1)) * sizeof(T); + cudaMemcpyAsync(thrust::raw_pointer_cast(&dst[offset]), + thrust::raw_pointer_cast(&src[offset]), step_size, + cudaMemcpyDeviceToHost, GetStream(i)); } } -} -} \ No newline at end of file +} // namespace utility +} // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/utility/platform.cu b/src/cupoch/utility/platform.cu index 08f1d2bf..e324d6f9 100644 --- a/src/cupoch/utility/platform.cu +++ b/src/cupoch/utility/platform.cu @@ -3,6 +3,7 @@ #include #endif #include + #include using namespace cupoch; @@ -11,9 +12,8 @@ using namespace cupoch::utility; cudaStream_t cupoch::utility::GetStream(size_t i) { static std::once_flag streamInitFlags[MAX_NUM_STREAMS]; static cudaStream_t streams[MAX_NUM_STREAMS]; - std::call_once(streamInitFlags[i], [i]() { - cudaStreamCreate(&(streams[i])); - }); + std::call_once(streamInitFlags[i], + [i]() { cudaStreamCreate(&(streams[i])); }); return streams[i]; } @@ -23,11 +23,11 @@ int cupoch::utility::GetDevice() { return device_no; } -void cupoch::utility::SetDevice(int device_no) { - cudaSetDevice(device_no); -} +void cupoch::utility::SetDevice(int device_no) { cudaSetDevice(device_no); } -void cupoch::utility::Error(const char *error_string, const char *file, const int line, +void cupoch::utility::Error(const char *error_string, + const char *file, + const int line, const char *func) { std::cout << "Error: " << error_string << "\t" << file << ":" << line << std::endl; diff --git a/src/cupoch/utility/platform.h b/src/cupoch/utility/platform.h index ea5ebb59..00d6b913 100644 --- a/src/cupoch/utility/platform.h +++ b/src/cupoch/utility/platform.h @@ -1,16 +1,20 @@ #pragma once #include + #include #ifdef _WIN32 -#define __FILENAME__ (strrchr(__FILE__, '\\') ? strrchr(__FILE__, '\\') + 1 : __FILE__) +#define __FILENAME__ \ + (strrchr(__FILE__, '\\') ? strrchr(__FILE__, '\\') + 1 : __FILE__) #else -#define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) +#define __FILENAME__ \ + (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) #endif #if defined(__GNUC__) - #define cudaSafeCall(expr) ___cudaSafeCall(expr, __FILENAME__, __LINE__, __func__) +#define cudaSafeCall(expr) \ + ___cudaSafeCall(expr, __FILENAME__, __LINE__, __func__) #else /* defined(__CUDACC__) || defined(__MSVC__) */ - #define cudaSafeCall(expr) ___cudaSafeCall(expr, __FILENAME__, __LINE__) +#define cudaSafeCall(expr) ___cudaSafeCall(expr, __FILENAME__, __LINE__) #endif namespace cupoch { @@ -24,12 +28,17 @@ int GetDevice(); void SetDevice(int device_no); -void Error(const char *error_string, const char *file, const int line, +void Error(const char *error_string, + const char *file, + const int line, const char *func); -} -} +} // namespace utility +} // namespace cupoch -static inline void ___cudaSafeCall(cudaError_t err, const char *file, const int line, const char *func = "") { +static inline void ___cudaSafeCall(cudaError_t err, + const char *file, + const int line, + const char *func = "") { if (cudaSuccess != err) cupoch::utility::Error(cudaGetErrorString(err), file, line, func); } diff --git a/src/cupoch/utility/range.h b/src/cupoch/utility/range.h index efcdaceb..bb353926 100644 --- a/src/cupoch/utility/range.h +++ b/src/cupoch/utility/range.h @@ -1,40 +1,41 @@ +#include #include -#include #include -#include +#include // examples: -// repeated_range([0, 1, 2, 3], 1) -> [0, 1, 2, 3] +// repeated_range([0, 1, 2, 3], 1) -> [0, 1, 2, 3] // repeated_range([0, 1, 2, 3], 2) -> [0, 0, 1, 1, 2, 2, 3, 3] -// repeated_range([0, 1, 2, 3], 3) -> [0, 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3] +// repeated_range([0, 1, 2, 3], 3) -> [0, 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3] // ... namespace thrust { template -class repeated_range -{ - public: - - typedef typename thrust::iterator_difference::type difference_type; +class repeated_range { +public: + typedef typename thrust::iterator_difference::type + difference_type; - struct repeat_functor : public thrust::unary_function - { + struct repeat_functor + : public thrust::unary_function { difference_type repeats; - repeat_functor(difference_type repeats) - : repeats(repeats) {} + repeat_functor(difference_type repeats) : repeats(repeats) {} - __host__ __device__ - difference_type operator()(const difference_type& i) const - { + __host__ __device__ difference_type + operator()(const difference_type &i) const { return i / repeats; } }; - typedef typename thrust::counting_iterator CountingIterator; - typedef typename thrust::transform_iterator TransformIterator; - typedef typename thrust::permutation_iterator PermutationIterator; + typedef typename thrust::counting_iterator + CountingIterator; + typedef typename thrust::transform_iterator + TransformIterator; + typedef typename thrust::permutation_iterator + PermutationIterator; // type of the repeated_range iterator typedef PermutationIterator iterator; @@ -42,18 +43,16 @@ class repeated_range // construct repeated_range for the range [first,last) repeated_range(Iterator first, Iterator last, difference_type repeats) : first(first), last(last), repeats(repeats) {} - - iterator begin(void) const - { - return PermutationIterator(first, TransformIterator(CountingIterator(0), repeat_functor(repeats))); - } - iterator end(void) const - { - return begin() + repeats * (last - first); + iterator begin(void) const { + return PermutationIterator(first, + TransformIterator(CountingIterator(0), + repeat_functor(repeats))); } - - protected: + + iterator end(void) const { return begin() + repeats * (last - first); } + +protected: Iterator first; Iterator last; difference_type repeats; @@ -67,29 +66,30 @@ class repeated_range // ... template -class strided_range -{ - public: - - typedef typename thrust::iterator_difference::type difference_type; +class strided_range { +public: + typedef typename thrust::iterator_difference::type + difference_type; - struct stride_functor : public thrust::unary_function - { + struct stride_functor + : public thrust::unary_function { difference_type stride; - stride_functor(difference_type stride) - : stride(stride) {} + stride_functor(difference_type stride) : stride(stride) {} - __host__ __device__ - difference_type operator()(const difference_type& i) const - { + __host__ __device__ difference_type + operator()(const difference_type &i) const { return stride * i; } }; - typedef typename thrust::counting_iterator CountingIterator; - typedef typename thrust::transform_iterator TransformIterator; - typedef typename thrust::permutation_iterator PermutationIterator; + typedef typename thrust::counting_iterator + CountingIterator; + typedef typename thrust::transform_iterator + TransformIterator; + typedef typename thrust::permutation_iterator + PermutationIterator; // type of the strided_range iterator typedef PermutationIterator iterator; @@ -97,21 +97,21 @@ class strided_range // construct strided_range for the range [first,last) strided_range(Iterator first, Iterator last, difference_type stride) : first(first), last(last), stride(stride) {} - - iterator begin(void) const - { - return PermutationIterator(first, TransformIterator(CountingIterator(0), stride_functor(stride))); + + iterator begin(void) const { + return PermutationIterator( + first, + TransformIterator(CountingIterator(0), stride_functor(stride))); } - iterator end(void) const - { + iterator end(void) const { return begin() + ((last - first) + (stride - 1)) / stride; } - - protected: + +protected: Iterator first; Iterator last; difference_type stride; }; -} \ No newline at end of file +} // namespace thrust \ No newline at end of file diff --git a/src/cupoch/utility/svd3_cuda.h b/src/cupoch/utility/svd3_cuda.h index b0565c86..6b11c5da 100644 --- a/src/cupoch/utility/svd3_cuda.h +++ b/src/cupoch/utility/svd3_cuda.h @@ -18,35 +18,31 @@ #ifndef SVD3_CUDA_H #define SVD3_CUDA_H -#define _gamma 5.828427124 // FOUR_GAMMA_SQUARED = sqrt(8)+3; -#define _cstar 0.923879532 // cos(pi/8) -#define _sstar 0.3826834323 // sin(p/8) +#define _gamma 5.828427124 // FOUR_GAMMA_SQUARED = sqrt(8)+3; +#define _cstar 0.923879532 // cos(pi/8) +#define _sstar 0.3826834323 // sin(p/8) #define EPSILON 1e-6 #include -#include "math.h" // CUDA math library +#include "math.h" // CUDA math library // CUDA's rsqrt seems to be faster than the inlined approximation? -__host__ __device__ __forceinline__ -float accurateSqrt(float x) -{ +__host__ __device__ __forceinline__ float accurateSqrt(float x) { return x * rsqrt(x); } -__host__ __device__ __forceinline__ -void condSwap(bool c, float &X, float &Y) -{ +__host__ __device__ __forceinline__ void condSwap(bool c, float &X, float &Y) { // used in step 2 float Z = X; X = c ? Y : X; Y = c ? Z : Y; } -__host__ __device__ __forceinline__ -void condNegSwap(bool c, float &X, float &Y) -{ +__host__ __device__ __forceinline__ void condNegSwap(bool c, + float &X, + float &Y) { // used in step 2 and 3 float Z = -X; X = c ? Y : X; @@ -54,116 +50,176 @@ void condNegSwap(bool c, float &X, float &Y) } // matrix multiplication M = A * B -__host__ __device__ __forceinline__ -void multAB(float a11, float a12, float a13, - float a21, float a22, float a23, - float a31, float a32, float a33, - // - float b11, float b12, float b13, - float b21, float b22, float b23, - float b31, float b32, float b33, - // - float &m11, float &m12, float &m13, - float &m21, float &m22, float &m23, - float &m31, float &m32, float &m33) -{ - - m11=a11*b11 + a12*b21 + a13*b31; m12=a11*b12 + a12*b22 + a13*b32; m13=a11*b13 + a12*b23 + a13*b33; - m21=a21*b11 + a22*b21 + a23*b31; m22=a21*b12 + a22*b22 + a23*b32; m23=a21*b13 + a22*b23 + a23*b33; - m31=a31*b11 + a32*b21 + a33*b31; m32=a31*b12 + a32*b22 + a33*b32; m33=a31*b13 + a32*b23 + a33*b33; +__host__ __device__ __forceinline__ void multAB(float a11, + float a12, + float a13, + float a21, + float a22, + float a23, + float a31, + float a32, + float a33, + // + float b11, + float b12, + float b13, + float b21, + float b22, + float b23, + float b31, + float b32, + float b33, + // + float &m11, + float &m12, + float &m13, + float &m21, + float &m22, + float &m23, + float &m31, + float &m32, + float &m33) { + m11 = a11 * b11 + a12 * b21 + a13 * b31; + m12 = a11 * b12 + a12 * b22 + a13 * b32; + m13 = a11 * b13 + a12 * b23 + a13 * b33; + m21 = a21 * b11 + a22 * b21 + a23 * b31; + m22 = a21 * b12 + a22 * b22 + a23 * b32; + m23 = a21 * b13 + a22 * b23 + a23 * b33; + m31 = a31 * b11 + a32 * b21 + a33 * b31; + m32 = a31 * b12 + a32 * b22 + a33 * b32; + m33 = a31 * b13 + a32 * b23 + a33 * b33; } // matrix multiplication M = Transpose[A] * B -__host__ __device__ __forceinline__ -void multAtB(float a11, float a12, float a13, - float a21, float a22, float a23, - float a31, float a32, float a33, - // - float b11, float b12, float b13, - float b21, float b22, float b23, - float b31, float b32, float b33, - // - float &m11, float &m12, float &m13, - float &m21, float &m22, float &m23, - float &m31, float &m32, float &m33) -{ - m11=a11*b11 + a21*b21 + a31*b31; m12=a11*b12 + a21*b22 + a31*b32; m13=a11*b13 + a21*b23 + a31*b33; - m21=a12*b11 + a22*b21 + a32*b31; m22=a12*b12 + a22*b22 + a32*b32; m23=a12*b13 + a22*b23 + a32*b33; - m31=a13*b11 + a23*b21 + a33*b31; m32=a13*b12 + a23*b22 + a33*b32; m33=a13*b13 + a23*b23 + a33*b33; +__host__ __device__ __forceinline__ void multAtB(float a11, + float a12, + float a13, + float a21, + float a22, + float a23, + float a31, + float a32, + float a33, + // + float b11, + float b12, + float b13, + float b21, + float b22, + float b23, + float b31, + float b32, + float b33, + // + float &m11, + float &m12, + float &m13, + float &m21, + float &m22, + float &m23, + float &m31, + float &m32, + float &m33) { + m11 = a11 * b11 + a21 * b21 + a31 * b31; + m12 = a11 * b12 + a21 * b22 + a31 * b32; + m13 = a11 * b13 + a21 * b23 + a31 * b33; + m21 = a12 * b11 + a22 * b21 + a32 * b31; + m22 = a12 * b12 + a22 * b22 + a32 * b32; + m23 = a12 * b13 + a22 * b23 + a32 * b33; + m31 = a13 * b11 + a23 * b21 + a33 * b31; + m32 = a13 * b12 + a23 * b22 + a33 * b32; + m33 = a13 * b13 + a23 * b23 + a33 * b33; } -__host__ __device__ __forceinline__ -void quatToMat3(const float * qV, -float &m11, float &m12, float &m13, -float &m21, float &m22, float &m23, -float &m31, float &m32, float &m33 -) -{ +__host__ __device__ __forceinline__ void quatToMat3(const float *qV, + float &m11, + float &m12, + float &m13, + float &m21, + float &m22, + float &m23, + float &m31, + float &m32, + float &m33) { float w = qV[3]; float x = qV[0]; float y = qV[1]; float z = qV[2]; - float qxx = x*x; - float qyy = y*y; - float qzz = z*z; - float qxz = x*z; - float qxy = x*y; - float qyz = y*z; - float qwx = w*x; - float qwy = w*y; - float qwz = w*z; - - m11=1 - 2*(qyy + qzz); m12=2*(qxy - qwz); m13=2*(qxz + qwy); - m21=2*(qxy + qwz); m22=1 - 2*(qxx + qzz); m23=2*(qyz - qwx); - m31=2*(qxz - qwy); m32=2*(qyz + qwx); m33=1 - 2*(qxx + qyy); + float qxx = x * x; + float qyy = y * y; + float qzz = z * z; + float qxz = x * z; + float qxy = x * y; + float qyz = y * z; + float qwx = w * x; + float qwy = w * y; + float qwz = w * z; + + m11 = 1 - 2 * (qyy + qzz); + m12 = 2 * (qxy - qwz); + m13 = 2 * (qxz + qwy); + m21 = 2 * (qxy + qwz); + m22 = 1 - 2 * (qxx + qzz); + m23 = 2 * (qyz - qwx); + m31 = 2 * (qxz - qwy); + m32 = 2 * (qyz + qwx); + m33 = 1 - 2 * (qxx + qyy); } -__host__ __device__ __forceinline__ -void approximateGivensQuaternion(float a11, float a12, float a22, float &ch, float &sh) -{ -/* +__host__ __device__ __forceinline__ void approximateGivensQuaternion( + float a11, float a12, float a22, float &ch, float &sh) { + /* * Given givens angle computed by approximateGivensAngles, * compute the corresponding rotation quaternion. */ - ch = 2*(a11-a22); + ch = 2 * (a11 - a22); sh = a12; - bool b = _gamma*sh*sh < ch*ch; - float w = rsqrt(ch*ch+sh*sh); - ch=b?w*ch:_cstar; - sh=b?w*sh:_sstar; + bool b = _gamma * sh * sh < ch * ch; + float w = rsqrt(ch * ch + sh * sh); + ch = b ? w * ch : _cstar; + sh = b ? w * sh : _sstar; } -__host__ __device__ __forceinline__ -void jacobiConjugation( const int x, const int y, const int z, - float &s11, - float &s21, float &s22, - float &s31, float &s32, float &s33, - float * qV) -{ - float ch,sh; - approximateGivensQuaternion(s11,s21,s22,ch,sh); - - float scale = ch*ch+sh*sh; - float a = (ch*ch-sh*sh)/scale; - float b = (2*sh*ch)/scale; +__host__ __device__ __forceinline__ void jacobiConjugation(const int x, + const int y, + const int z, + float &s11, + float &s21, + float &s22, + float &s31, + float &s32, + float &s33, + float *qV) { + float ch, sh; + approximateGivensQuaternion(s11, s21, s22, ch, sh); + + float scale = ch * ch + sh * sh; + float a = (ch * ch - sh * sh) / scale; + float b = (2 * sh * ch) / scale; // make temp copy of S float _s11 = s11; - float _s21 = s21; float _s22 = s22; - float _s31 = s31; float _s32 = s32; float _s33 = s33; + float _s21 = s21; + float _s22 = s22; + float _s31 = s31; + float _s32 = s32; + float _s33 = s33; // perform conjugation S = Q'*S*Q // Q already implicitly solved from a, b - s11 =a*(a*_s11 + b*_s21) + b*(a*_s21 + b*_s22); - s21 =a*(-b*_s11 + a*_s21) + b*(-b*_s21 + a*_s22); s22=-b*(-b*_s11 + a*_s21) + a*(-b*_s21 + a*_s22); - s31 =a*_s31 + b*_s32; s32=-b*_s31 + a*_s32; s33=_s33; + s11 = a * (a * _s11 + b * _s21) + b * (a * _s21 + b * _s22); + s21 = a * (-b * _s11 + a * _s21) + b * (-b * _s21 + a * _s22); + s22 = -b * (-b * _s11 + a * _s21) + a * (-b * _s21 + a * _s22); + s31 = a * _s31 + b * _s32; + s32 = -b * _s31 + a * _s32; + s33 = _s33; // update cumulative rotation qV float tmp[3]; - tmp[0]=qV[0]*sh; - tmp[1]=qV[1]*sh; - tmp[2]=qV[2]*sh; + tmp[0] = qV[0] * sh; + tmp[1] = qV[1] * sh; + tmp[2] = qV[2] * sh; sh *= qV[3]; qV[0] *= ch; @@ -174,248 +230,350 @@ void jacobiConjugation( const int x, const int y, const int z, // (x,y,z) corresponds to ((0,1,2),(1,2,0),(2,0,1)) // for (p,q) = ((0,1),(1,2),(0,2)) qV[z] += sh; - qV[3] -= tmp[z]; // w + qV[3] -= tmp[z]; // w qV[x] += tmp[y]; qV[y] -= tmp[x]; // re-arrange matrix for next iteration _s11 = s22; - _s21 = s32; _s22 = s33; - _s31 = s21; _s32 = s31; _s33 = s11; + _s21 = s32; + _s22 = s33; + _s31 = s21; + _s32 = s31; + _s33 = s11; s11 = _s11; - s21 = _s21; s22 = _s22; - s31 = _s31; s32 = _s32; s33 = _s33; - + s21 = _s21; + s22 = _s22; + s31 = _s31; + s32 = _s32; + s33 = _s33; } -__host__ __device__ __forceinline__ -float dist2(float x, float y, float z) -{ - return x*x+y*y+z*z; +__host__ __device__ __forceinline__ float dist2(float x, float y, float z) { + return x * x + y * y + z * z; } // finds transformation that diagonalizes a symmetric matrix -__host__ __device__ __forceinline__ -void jacobiEigenanlysis( // symmetric matrix - float &s11, - float &s21, float &s22, - float &s31, float &s32, float &s33, - // quaternion representation of V - float * qV) -{ - qV[3]=1; qV[0]=0;qV[1]=0;qV[2]=0; // follow same indexing convention as GLM - for (int i=0;i<4;i++) - { +__host__ __device__ __forceinline__ void +jacobiEigenanlysis( // symmetric matrix + float &s11, + float &s21, + float &s22, + float &s31, + float &s32, + float &s33, + // quaternion representation of V + float *qV) { + qV[3] = 1; + qV[0] = 0; + qV[1] = 0; + qV[2] = 0; // follow same indexing convention as GLM + for (int i = 0; i < 4; i++) { // we wish to eliminate the maximum off-diagonal element // on every iteration, but cycling over all 3 possible rotations // in fixed order (p,q) = (1,2) , (2,3), (1,3) still retains // asymptotic convergence - jacobiConjugation(0,1,2,s11,s21,s22,s31,s32,s33,qV); // p,q = 0,1 - jacobiConjugation(1,2,0,s11,s21,s22,s31,s32,s33,qV); // p,q = 1,2 - jacobiConjugation(2,0,1,s11,s21,s22,s31,s32,s33,qV); // p,q = 0,2 + jacobiConjugation(0, 1, 2, s11, s21, s22, s31, s32, s33, + qV); // p,q = 0,1 + jacobiConjugation(1, 2, 0, s11, s21, s22, s31, s32, s33, + qV); // p,q = 1,2 + jacobiConjugation(2, 0, 1, s11, s21, s22, s31, s32, s33, + qV); // p,q = 0,2 } } -__host__ __device__ __forceinline__ -void sortSingularValues(// matrix that we want to decompose - float &b11, float &b12, float &b13, - float &b21, float &b22, float &b23, - float &b31, float &b32, float &b33, - // sort V simultaneously - float &v11, float &v12, float &v13, - float &v21, float &v22, float &v23, - float &v31, float &v32, float &v33) -{ - float rho1 = dist2(b11,b21,b31); - float rho2 = dist2(b12,b22,b32); - float rho3 = dist2(b13,b23,b33); +__host__ __device__ __forceinline__ void +sortSingularValues( // matrix that we want to decompose + float &b11, + float &b12, + float &b13, + float &b21, + float &b22, + float &b23, + float &b31, + float &b32, + float &b33, + // sort V simultaneously + float &v11, + float &v12, + float &v13, + float &v21, + float &v22, + float &v23, + float &v31, + float &v32, + float &v33) { + float rho1 = dist2(b11, b21, b31); + float rho2 = dist2(b12, b22, b32); + float rho3 = dist2(b13, b23, b33); bool c; c = rho1 < rho2; - condNegSwap(c,b11,b12); condNegSwap(c,v11,v12); - condNegSwap(c,b21,b22); condNegSwap(c,v21,v22); - condNegSwap(c,b31,b32); condNegSwap(c,v31,v32); - condSwap(c,rho1,rho2); + condNegSwap(c, b11, b12); + condNegSwap(c, v11, v12); + condNegSwap(c, b21, b22); + condNegSwap(c, v21, v22); + condNegSwap(c, b31, b32); + condNegSwap(c, v31, v32); + condSwap(c, rho1, rho2); c = rho1 < rho3; - condNegSwap(c,b11,b13); condNegSwap(c,v11,v13); - condNegSwap(c,b21,b23); condNegSwap(c,v21,v23); - condNegSwap(c,b31,b33); condNegSwap(c,v31,v33); - condSwap(c,rho1,rho3); + condNegSwap(c, b11, b13); + condNegSwap(c, v11, v13); + condNegSwap(c, b21, b23); + condNegSwap(c, v21, v23); + condNegSwap(c, b31, b33); + condNegSwap(c, v31, v33); + condSwap(c, rho1, rho3); c = rho2 < rho3; - condNegSwap(c,b12,b13); condNegSwap(c,v12,v13); - condNegSwap(c,b22,b23); condNegSwap(c,v22,v23); - condNegSwap(c,b32,b33); condNegSwap(c,v32,v33); + condNegSwap(c, b12, b13); + condNegSwap(c, v12, v13); + condNegSwap(c, b22, b23); + condNegSwap(c, v22, v23); + condNegSwap(c, b32, b33); + condNegSwap(c, v32, v33); } -__host__ __device__ __forceinline__ -void QRGivensQuaternion(float a1, float a2, float &ch, float &sh) -{ +__host__ __device__ __forceinline__ void QRGivensQuaternion(float a1, + float a2, + float &ch, + float &sh) { // a1 = pivot point on diagonal // a2 = lower triangular entry we want to annihilate float epsilon = EPSILON; - float rho = accurateSqrt(a1*a1 + a2*a2); + float rho = accurateSqrt(a1 * a1 + a2 * a2); sh = rho > epsilon ? a2 : 0; - ch = fabs(a1) + fmax(rho,epsilon); + ch = fabs(a1) + fmax(rho, epsilon); bool b = a1 < 0; - condSwap(b,sh,ch); - float w = rsqrt(ch*ch+sh*sh); + condSwap(b, sh, ch); + float w = rsqrt(ch * ch + sh * sh); ch *= w; sh *= w; } -__host__ __device__ __forceinline__ -void QRDecomposition(// matrix that we want to decompose - float b11, float b12, float b13, - float b21, float b22, float b23, - float b31, float b32, float b33, - // output Q - float &q11, float &q12, float &q13, - float &q21, float &q22, float &q23, - float &q31, float &q32, float &q33, - // output R - float &r11, float &r12, float &r13, - float &r21, float &r22, float &r23, - float &r31, float &r32, float &r33) -{ - float ch1,sh1,ch2,sh2,ch3,sh3; - float a,b; +__host__ __device__ __forceinline__ void +QRDecomposition( // matrix that we want to decompose + float b11, + float b12, + float b13, + float b21, + float b22, + float b23, + float b31, + float b32, + float b33, + // output Q + float &q11, + float &q12, + float &q13, + float &q21, + float &q22, + float &q23, + float &q31, + float &q32, + float &q33, + // output R + float &r11, + float &r12, + float &r13, + float &r21, + float &r22, + float &r23, + float &r31, + float &r32, + float &r33) { + float ch1, sh1, ch2, sh2, ch3, sh3; + float a, b; // first givens rotation (ch,0,0,sh) - QRGivensQuaternion(b11,b21,ch1,sh1); - a=1-2*sh1*sh1; - b=2*ch1*sh1; + QRGivensQuaternion(b11, b21, ch1, sh1); + a = 1 - 2 * sh1 * sh1; + b = 2 * ch1 * sh1; // apply B = Q' * B - r11=a*b11+b*b21; r12=a*b12+b*b22; r13=a*b13+b*b23; - r21=-b*b11+a*b21; r22=-b*b12+a*b22; r23=-b*b13+a*b23; - r31=b31; r32=b32; r33=b33; + r11 = a * b11 + b * b21; + r12 = a * b12 + b * b22; + r13 = a * b13 + b * b23; + r21 = -b * b11 + a * b21; + r22 = -b * b12 + a * b22; + r23 = -b * b13 + a * b23; + r31 = b31; + r32 = b32; + r33 = b33; // second givens rotation (ch,0,-sh,0) - QRGivensQuaternion(r11,r31,ch2,sh2); - a=1-2*sh2*sh2; - b=2*ch2*sh2; + QRGivensQuaternion(r11, r31, ch2, sh2); + a = 1 - 2 * sh2 * sh2; + b = 2 * ch2 * sh2; // apply B = Q' * B; - b11=a*r11+b*r31; b12=a*r12+b*r32; b13=a*r13+b*r33; - b21=r21; b22=r22; b23=r23; - b31=-b*r11+a*r31; b32=-b*r12+a*r32; b33=-b*r13+a*r33; + b11 = a * r11 + b * r31; + b12 = a * r12 + b * r32; + b13 = a * r13 + b * r33; + b21 = r21; + b22 = r22; + b23 = r23; + b31 = -b * r11 + a * r31; + b32 = -b * r12 + a * r32; + b33 = -b * r13 + a * r33; // third givens rotation (ch,sh,0,0) - QRGivensQuaternion(b22,b32,ch3,sh3); - a=1-2*sh3*sh3; - b=2*ch3*sh3; + QRGivensQuaternion(b22, b32, ch3, sh3); + a = 1 - 2 * sh3 * sh3; + b = 2 * ch3 * sh3; // R is now set to desired value - r11=b11; r12=b12; r13=b13; - r21=a*b21+b*b31; r22=a*b22+b*b32; r23=a*b23+b*b33; - r31=-b*b21+a*b31; r32=-b*b22+a*b32; r33=-b*b23+a*b33; + r11 = b11; + r12 = b12; + r13 = b13; + r21 = a * b21 + b * b31; + r22 = a * b22 + b * b32; + r23 = a * b23 + b * b33; + r31 = -b * b21 + a * b31; + r32 = -b * b22 + a * b32; + r33 = -b * b23 + a * b33; // construct the cumulative rotation Q=Q1 * Q2 * Q3 - // the number of floating point operations for three quaternion multiplications - // is more or less comparable to the explicit form of the joined matrix. - // certainly more memory-efficient! - float sh12=sh1*sh1; - float sh22=sh2*sh2; - float sh32=sh3*sh3; - - q11=(-1+2*sh12)*(-1+2*sh22); - q12=4*ch2*ch3*(-1+2*sh12)*sh2*sh3+2*ch1*sh1*(-1+2*sh32); - q13=4*ch1*ch3*sh1*sh3-2*ch2*(-1+2*sh12)*sh2*(-1+2*sh32); - - q21=2*ch1*sh1*(1-2*sh22); - q22=-8*ch1*ch2*ch3*sh1*sh2*sh3+(-1+2*sh12)*(-1+2*sh32); - q23=-2*ch3*sh3+4*sh1*(ch3*sh1*sh3+ch1*ch2*sh2*(-1+2*sh32)); - - q31=2*ch2*sh2; - q32=2*ch3*(1-2*sh22)*sh3; - q33=(-1+2*sh22)*(-1+2*sh32); + // the number of floating point operations for three quaternion + // multiplications is more or less comparable to the explicit form of the + // joined matrix. certainly more memory-efficient! + float sh12 = sh1 * sh1; + float sh22 = sh2 * sh2; + float sh32 = sh3 * sh3; + + q11 = (-1 + 2 * sh12) * (-1 + 2 * sh22); + q12 = 4 * ch2 * ch3 * (-1 + 2 * sh12) * sh2 * sh3 + + 2 * ch1 * sh1 * (-1 + 2 * sh32); + q13 = 4 * ch1 * ch3 * sh1 * sh3 - + 2 * ch2 * (-1 + 2 * sh12) * sh2 * (-1 + 2 * sh32); + + q21 = 2 * ch1 * sh1 * (1 - 2 * sh22); + q22 = -8 * ch1 * ch2 * ch3 * sh1 * sh2 * sh3 + + (-1 + 2 * sh12) * (-1 + 2 * sh32); + q23 = -2 * ch3 * sh3 + + 4 * sh1 * (ch3 * sh1 * sh3 + ch1 * ch2 * sh2 * (-1 + 2 * sh32)); + + q31 = 2 * ch2 * sh2; + q32 = 2 * ch3 * (1 - 2 * sh22) * sh3; + q33 = (-1 + 2 * sh22) * (-1 + 2 * sh32); } -__host__ __device__ __forceinline__ -void svd(// input A - float a11, float a12, float a13, - float a21, float a22, float a23, - float a31, float a32, float a33, +__host__ __device__ __forceinline__ void svd( // input A + float a11, + float a12, + float a13, + float a21, + float a22, + float a23, + float a31, + float a32, + float a33, // output U - float &u11, float &u12, float &u13, - float &u21, float &u22, float &u23, - float &u31, float &u32, float &u33, + float &u11, + float &u12, + float &u13, + float &u21, + float &u22, + float &u23, + float &u31, + float &u32, + float &u33, // output S - float &s11, float &s12, float &s13, - float &s21, float &s22, float &s23, - float &s31, float &s32, float &s33, + float &s11, + float &s12, + float &s13, + float &s21, + float &s22, + float &s23, + float &s31, + float &s32, + float &s33, // output V - float &v11, float &v12, float &v13, - float &v21, float &v22, float &v23, - float &v31, float &v32, float &v33) -{ + float &v11, + float &v12, + float &v13, + float &v21, + float &v22, + float &v23, + float &v31, + float &v32, + float &v33) { // normal equations matrix float ATA11, ATA12, ATA13; float ATA21, ATA22, ATA23; float ATA31, ATA32, ATA33; - multAtB(a11,a12,a13,a21,a22,a23,a31,a32,a33, - a11,a12,a13,a21,a22,a23,a31,a32,a33, - ATA11,ATA12,ATA13,ATA21,ATA22,ATA23,ATA31,ATA32,ATA33); + multAtB(a11, a12, a13, a21, a22, a23, a31, a32, a33, a11, a12, a13, a21, + a22, a23, a31, a32, a33, ATA11, ATA12, ATA13, ATA21, ATA22, ATA23, + ATA31, ATA32, ATA33); // symmetric eigenalysis float qV[4]; - jacobiEigenanlysis( ATA11,ATA21,ATA22, ATA31,ATA32,ATA33,qV); - quatToMat3(qV,v11,v12,v13,v21,v22,v23,v31,v32,v33); + jacobiEigenanlysis(ATA11, ATA21, ATA22, ATA31, ATA32, ATA33, qV); + quatToMat3(qV, v11, v12, v13, v21, v22, v23, v31, v32, v33); float b11, b12, b13; float b21, b22, b23; float b31, b32, b33; - multAB(a11,a12,a13,a21,a22,a23,a31,a32,a33, - v11,v12,v13,v21,v22,v23,v31,v32,v33, - b11, b12, b13, b21, b22, b23, b31, b32, b33); + multAB(a11, a12, a13, a21, a22, a23, a31, a32, a33, v11, v12, v13, v21, v22, + v23, v31, v32, v33, b11, b12, b13, b21, b22, b23, b31, b32, b33); // sort singular values and find V - sortSingularValues(b11, b12, b13, b21, b22, b23, b31, b32, b33, - v11,v12,v13,v21,v22,v23,v31,v32,v33); + sortSingularValues(b11, b12, b13, b21, b22, b23, b31, b32, b33, v11, v12, + v13, v21, v22, v23, v31, v32, v33); // QR decomposition - QRDecomposition(b11, b12, b13, b21, b22, b23, b31, b32, b33, - u11, u12, u13, u21, u22, u23, u31, u32, u33, - s11, s12, s13, s21, s22, s23, s31, s32, s33 - ); + QRDecomposition(b11, b12, b13, b21, b22, b23, b31, b32, b33, u11, u12, u13, + u21, u22, u23, u31, u32, u33, s11, s12, s13, s21, s22, s23, + s31, s32, s33); } /// polar decomposition can be reconstructed trivially from SVD result /// A = UP -__host__ __device__ __forceinline__ -void pd(float a11, float a12, float a13, - float a21, float a22, float a23, - float a31, float a32, float a33, - // output U - float &u11, float &u12, float &u13, - float &u21, float &u22, float &u23, - float &u31, float &u32, float &u33, - // output P - float &p11, float &p12, float &p13, - float &p21, float &p22, float &p23, - float &p31, float &p32, float &p33) -{ +__host__ __device__ __forceinline__ void pd(float a11, + float a12, + float a13, + float a21, + float a22, + float a23, + float a31, + float a32, + float a33, + // output U + float &u11, + float &u12, + float &u13, + float &u21, + float &u22, + float &u23, + float &u31, + float &u32, + float &u33, + // output P + float &p11, + float &p12, + float &p13, + float &p21, + float &p22, + float &p23, + float &p31, + float &p32, + float &p33) { float w11, w12, w13, w21, w22, w23, w31, w32, w33; float s11, s12, s13, s21, s22, s23, s31, s32, s33; float v11, v12, v13, v21, v22, v23, v31, v32, v33; - svd(a11, a12, a13, a21, a22, a23, a31, a32, a33, - w11, w12, w13, w21, w22, w23, w31, w32, w33, - s11, s12, s13, s21, s22, s23, s31, s32, s33, - v11, v12, v13, v21, v22, v23, v31, v32, v33); + svd(a11, a12, a13, a21, a22, a23, a31, a32, a33, w11, w12, w13, w21, w22, + w23, w31, w32, w33, s11, s12, s13, s21, s22, s23, s31, s32, s33, v11, + v12, v13, v21, v22, v23, v31, v32, v33); // P = VSV' float t11, t12, t13, t21, t22, t23, t31, t32, t33; - multAB(v11, v12, v13, v21, v22, v23, v31, v32, v33, - s11, s12, s13, s21, s22, s23, s31, s32, s33, - t11, t12, t13, t21, t22, t23, t31, t32, t33); + multAB(v11, v12, v13, v21, v22, v23, v31, v32, v33, s11, s12, s13, s21, s22, + s23, s31, s32, s33, t11, t12, t13, t21, t22, t23, t31, t32, t33); - multAB(t11, t12, t13, t21, t22, t23, t31, t32, t33, - v11, v21, v31, v12, v22, v32, v13, v23, v33, - p11, p12, p13, p21, p22, p23, p31, p32, p33); + multAB(t11, t12, t13, t21, t22, t23, t31, t32, t33, v11, v21, v31, v12, v22, + v32, v13, v23, v33, p11, p12, p13, p21, p22, p23, p31, p32, p33); // U = WV' - multAB(w11, w12, w13, w21, w22, w23, w31, w32, w33, - v11, v21, v31, v12, v22, v32, v13, v23, v33, - u11, u12, u13, u21, u22, u23, u31, u32, u33); + multAB(w11, w12, w13, w21, w22, w23, w31, w32, w33, v11, v21, v31, v12, v22, + v32, v13, v23, v33, u11, u12, u13, u21, u22, u23, u31, u32, u33); } #endif diff --git a/src/cupoch/visualization/shader/image_shader.cu b/src/cupoch/visualization/shader/image_shader.cu index 32dfe624..a368bb40 100644 --- a/src/cupoch/visualization/shader/image_shader.cu +++ b/src/cupoch/visualization/shader/image_shader.cu @@ -17,7 +17,7 @@ namespace { __device__ uint8_t ConvertColorFromFloatToUnsignedChar(float color) { - if (std::isnan(color)) { + if (isnan(color)) { return 0; } else { thrust::minimum min; diff --git a/src/python/Pipfile.lock b/src/python/Pipfile.lock deleted file mode 100644 index 431cb566..00000000 --- a/src/python/Pipfile.lock +++ /dev/null @@ -1,702 +0,0 @@ -{ - "_meta": { - "hash": { - "sha256": "24523acf0ab3f35e3cb2c58ddc8bc8532589596c2b3005d7e572ad91f70e48f9" - }, - "pipfile-spec": 6, - "requires": {}, - "sources": [ - { - "name": "pypi", - "url": "https://pypi.org/simple", - "verify_ssl": true - } - ] - }, - "default": { - "attrs": { - "hashes": [ - "sha256:08a96c641c3a74e44eb59afb61a24f2cb9f4d7188748e76ba4bb5edfa3cb7d1c", - "sha256:f7b7ce16570fe9965acd6d30101a28f62fb4a7f9e926b3bbc9b61f8b04247e72" - ], - "version": "==19.3.0" - }, - "backcall": { - "hashes": [ - "sha256:38ecd85be2c1e78f77fd91700c76e14667dc21e2713b63876c0eb901196e01e4", - "sha256:bbbf4b1e5cd2bdb08f915895b51081c041bac22394fdfcfdfbe9f14b77c08bf2" - ], - "version": "==0.1.0" - }, - "bleach": { - "hashes": [ - "sha256:213336e49e102af26d9cde77dd2d0397afabc5a6bf2fed985dc35b5d1e285a16", - "sha256:3fdf7f77adcf649c9911387df51254b813185e32b2c6619f690b593a617e19fa" - ], - "version": "==3.1.0" - }, - "certifi": { - "hashes": [ - "sha256:017c25db2a153ce562900032d5bc68e9f191e44e9a0f762f373977de9df1fbb3", - "sha256:25b64c7da4cd7479594d035c08c2d809eb4aab3a26e5a990ea98cc450c320f1f" - ], - "version": "==2019.11.28" - }, - "cffi": { - "hashes": [ - "sha256:001bf3242a1bb04d985d63e138230802c6c8d4db3668fb545fb5005ddf5bb5ff", - "sha256:00789914be39dffba161cfc5be31b55775de5ba2235fe49aa28c148236c4e06b", - "sha256:028a579fc9aed3af38f4892bdcc7390508adabc30c6af4a6e4f611b0c680e6ac", - "sha256:14491a910663bf9f13ddf2bc8f60562d6bc5315c1f09c704937ef17293fb85b0", - "sha256:1cae98a7054b5c9391eb3249b86e0e99ab1e02bb0cc0575da191aedadbdf4384", - "sha256:2089ed025da3919d2e75a4d963d008330c96751127dd6f73c8dc0c65041b4c26", - "sha256:2d384f4a127a15ba701207f7639d94106693b6cd64173d6c8988e2c25f3ac2b6", - "sha256:337d448e5a725bba2d8293c48d9353fc68d0e9e4088d62a9571def317797522b", - "sha256:399aed636c7d3749bbed55bc907c3288cb43c65c4389964ad5ff849b6370603e", - "sha256:3b911c2dbd4f423b4c4fcca138cadde747abdb20d196c4a48708b8a2d32b16dd", - "sha256:3d311bcc4a41408cf5854f06ef2c5cab88f9fded37a3b95936c9879c1640d4c2", - "sha256:62ae9af2d069ea2698bf536dcfe1e4eed9090211dbaafeeedf5cb6c41b352f66", - "sha256:66e41db66b47d0d8672d8ed2708ba91b2f2524ece3dee48b5dfb36be8c2f21dc", - "sha256:675686925a9fb403edba0114db74e741d8181683dcf216be697d208857e04ca8", - "sha256:7e63cbcf2429a8dbfe48dcc2322d5f2220b77b2e17b7ba023d6166d84655da55", - "sha256:8a6c688fefb4e1cd56feb6c511984a6c4f7ec7d2a1ff31a10254f3c817054ae4", - "sha256:8c0ffc886aea5df6a1762d0019e9cb05f825d0eec1f520c51be9d198701daee5", - "sha256:95cd16d3dee553f882540c1ffe331d085c9e629499ceadfbda4d4fde635f4b7d", - "sha256:99f748a7e71ff382613b4e1acc0ac83bf7ad167fb3802e35e90d9763daba4d78", - "sha256:b8c78301cefcf5fd914aad35d3c04c2b21ce8629b5e4f4e45ae6812e461910fa", - "sha256:c420917b188a5582a56d8b93bdd8e0f6eca08c84ff623a4c16e809152cd35793", - "sha256:c43866529f2f06fe0edc6246eb4faa34f03fe88b64a0a9a942561c8e22f4b71f", - "sha256:cab50b8c2250b46fe738c77dbd25ce017d5e6fb35d3407606e7a4180656a5a6a", - "sha256:cef128cb4d5e0b3493f058f10ce32365972c554572ff821e175dbc6f8ff6924f", - "sha256:cf16e3cf6c0a5fdd9bc10c21687e19d29ad1fe863372b5543deaec1039581a30", - "sha256:e56c744aa6ff427a607763346e4170629caf7e48ead6921745986db3692f987f", - "sha256:e577934fc5f8779c554639376beeaa5657d54349096ef24abe8c74c5d9c117c3", - "sha256:f2b0fa0c01d8a0c7483afd9f31d7ecf2d71760ca24499c8697aeb5ca37dc090c" - ], - "version": "==1.14.0" - }, - "chardet": { - "hashes": [ - "sha256:84ab92ed1c4d4f16916e05906b6b75a6c0fb5db821cc65e70cbd64a3e2a5eaae", - "sha256:fc323ffcaeaed0e0a02bf4d117757b98aed530d9ed4531e3e15460124c106691" - ], - "version": "==3.0.4" - }, - "cryptography": { - "hashes": [ - "sha256:02079a6addc7b5140ba0825f542c0869ff4df9a69c360e339ecead5baefa843c", - "sha256:1df22371fbf2004c6f64e927668734070a8953362cd8370ddd336774d6743595", - "sha256:369d2346db5934345787451504853ad9d342d7f721ae82d098083e1f49a582ad", - "sha256:3cda1f0ed8747339bbdf71b9f38ca74c7b592f24f65cdb3ab3765e4b02871651", - "sha256:44ff04138935882fef7c686878e1c8fd80a723161ad6a98da31e14b7553170c2", - "sha256:4b1030728872c59687badcca1e225a9103440e467c17d6d1730ab3d2d64bfeff", - "sha256:58363dbd966afb4f89b3b11dfb8ff200058fbc3b947507675c19ceb46104b48d", - "sha256:6ec280fb24d27e3d97aa731e16207d58bd8ae94ef6eab97249a2afe4ba643d42", - "sha256:7270a6c29199adc1297776937a05b59720e8a782531f1f122f2eb8467f9aab4d", - "sha256:73fd30c57fa2d0a1d7a49c561c40c2f79c7d6c374cc7750e9ac7c99176f6428e", - "sha256:7f09806ed4fbea8f51585231ba742b58cbcfbfe823ea197d8c89a5e433c7e912", - "sha256:90df0cc93e1f8d2fba8365fb59a858f51a11a394d64dbf3ef844f783844cc793", - "sha256:971221ed40f058f5662a604bd1ae6e4521d84e6cad0b7b170564cc34169c8f13", - "sha256:a518c153a2b5ed6b8cc03f7ae79d5ffad7315ad4569b2d5333a13c38d64bd8d7", - "sha256:b0de590a8b0979649ebeef8bb9f54394d3a41f66c5584fff4220901739b6b2f0", - "sha256:b43f53f29816ba1db8525f006fa6f49292e9b029554b3eb56a189a70f2a40879", - "sha256:d31402aad60ed889c7e57934a03477b572a03af7794fa8fb1780f21ea8f6551f", - "sha256:de96157ec73458a7f14e3d26f17f8128c959084931e8997b9e655a39c8fde9f9", - "sha256:df6b4dca2e11865e6cfbfb708e800efb18370f5a46fd601d3755bc7f85b3a8a2", - "sha256:ecadccc7ba52193963c0475ac9f6fa28ac01e01349a2ca48509667ef41ffd2cf", - "sha256:fb81c17e0ebe3358486cd8cc3ad78adbae58af12fc2bf2bc0bb84e8090fa5ce8" - ], - "version": "==2.8" - }, - "cycler": { - "hashes": [ - "sha256:1d8a5ae1ff6c5cf9b93e8811e581232ad8920aeec647c37316ceac982b08cb2d", - "sha256:cd7b2d1018258d7247a71425e9f26463dfb444d411c39569972f4ce586b0c9d8" - ], - "version": "==0.10.0" - }, - "decorator": { - "hashes": [ - "sha256:54c38050039232e1db4ad7375cfce6748d7b41c29e95a081c8a6d2c30364a2ce", - "sha256:5d19b92a3c8f7f101c8dd86afd86b0f061a8ce4540ab8cd401fa2542756bce6d" - ], - "version": "==4.4.1" - }, - "defusedxml": { - "hashes": [ - "sha256:6687150770438374ab581bb7a1b327a847dd9c5749e396102de3fad4e8a3ef93", - "sha256:f684034d135af4c6cbb949b8a4d2ed61634515257a67299e5f940fbaa34377f5" - ], - "version": "==0.6.0" - }, - "docutils": { - "hashes": [ - "sha256:0c5b78adfbf7762415433f5515cd5c9e762339e23369dbe8000d84a4bf4ab3af", - "sha256:c2de3a60e9e7d07be26b7f2b00ca0309c207e06c100f9cc2a94931fc75a478fc" - ], - "version": "==0.16" - }, - "entrypoints": { - "hashes": [ - "sha256:589f874b313739ad35be6e0cd7efde2a4e9b6fea91edcc34e58ecbb8dbe56d19", - "sha256:c70dd71abe5a8c85e55e12c19bd91ccfeec11a6e99044204511f9ed547d48451" - ], - "version": "==0.3" - }, - "idna": { - "hashes": [ - "sha256:c357b3f628cf53ae2c4c05627ecc484553142ca23264e593d327bcde5e9c3407", - "sha256:ea8b7f6188e6fa117537c3df7da9fc686d485087abf6ac197f9c46432f7e4a3c" - ], - "version": "==2.8" - }, - "importlib-metadata": { - "hashes": [ - "sha256:06f5b3a99029c7134207dd882428a66992a9de2bef7c2b699b5641f9886c3302", - "sha256:b97607a1a18a5100839aec1dc26a1ea17ee0d93b20b0f008d80a5a050afb200b" - ], - "markers": "python_version < '3.8'", - "version": "==1.5.0" - }, - "ipykernel": { - "hashes": [ - "sha256:7f1f01df22f1229c8879501057877ccaf92a3b01c1d00db708aad5003e5f9238", - "sha256:ba8c9e5561f3223fb47ce06ad7925cb9444337ac367341c0c520ffb68ea6d120" - ], - "version": "==5.1.4" - }, - "ipython": { - "hashes": [ - "sha256:d9459e7237e2e5858738ff9c3e26504b79899b58a6d49e574d352493d80684c6", - "sha256:f6689108b1734501d3b59c84427259fd5ac5141afe2e846cfa8598eb811886c9" - ], - "markers": "python_version >= '3.3'", - "version": "==7.12.0" - }, - "ipython-genutils": { - "hashes": [ - "sha256:72dd37233799e619666c9f639a9da83c34013a73e8bbc79a7a6348d93c61fab8", - "sha256:eb2e116e75ecef9d4d228fdc66af54269afa26ab4463042e33785b887c628ba8" - ], - "version": "==0.2.0" - }, - "ipywidgets": { - "hashes": [ - "sha256:13ffeca438e0c0f91ae583dc22f50379b9d6b28390ac7be8b757140e9a771516", - "sha256:e945f6e02854a74994c596d9db83444a1850c01648f1574adf144fbbabe05c97" - ], - "version": "==7.5.1" - }, - "jedi": { - "hashes": [ - "sha256:b4f4052551025c6b0b0b193b29a6ff7bdb74c52450631206c262aef9f7159ad2", - "sha256:d5c871cb9360b414f981e7072c52c33258d598305280fef91c6cae34739d65d5" - ], - "version": "==0.16.0" - }, - "jeepney": { - "hashes": [ - "sha256:0ba6d8c597e9bef1ebd18aaec595f942a264e25c1a48f164d46120eacaa2e9bb", - "sha256:6f45dce1125cf6c58a1c88123d3831f36a789f9204fbad3172eac15f8ccd08d0" - ], - "markers": "sys_platform == 'linux'", - "version": "==0.4.2" - }, - "jinja2": { - "hashes": [ - "sha256:93187ffbc7808079673ef52771baa950426fd664d3aad1d0fa3e95644360e250", - "sha256:b0eaf100007721b5c16c1fc1eecb87409464edc10469ddc9a22a27a99123be49" - ], - "version": "==2.11.1" - }, - "jsonschema": { - "hashes": [ - "sha256:4e5b3cf8216f577bee9ce139cbe72eca3ea4f292ec60928ff24758ce626cd163", - "sha256:c8a85b28d377cc7737e46e2d9f2b4f44ee3c0e1deac6bf46ddefc7187d30797a" - ], - "version": "==3.2.0" - }, - "jupyter-client": { - "hashes": [ - "sha256:60e6faec1031d63df57f1cc671ed673dced0ed420f4377ea33db37b1c188b910", - "sha256:d0c077c9aaa4432ad485e7733e4d91e48f87b4f4bab7d283d42bb24cbbba0a0f" - ], - "version": "==5.3.4" - }, - "jupyter-core": { - "hashes": [ - "sha256:185dfe42800585ca860aa47b5fe0211ee2c33246576d2d664b0b0b8d22aacf3a", - "sha256:e91785b8bd7f752711c0c20e5ec6ba0d42178d6321a61396082c55818991caee" - ], - "version": "==4.6.2" - }, - "keyring": { - "hashes": [ - "sha256:1f393f7466314068961c7e1d508120c092bd71fa54e3d93b76180b526d4abc56", - "sha256:24ae23ab2d6adc59138339e56843e33ec7b0a6b2f06302662477085c6c0aca00" - ], - "version": "==21.1.0" - }, - "kiwisolver": { - "hashes": [ - "sha256:05b5b061e09f60f56244adc885c4a7867da25ca387376b02c1efc29cc16bcd0f", - "sha256:210d8c39d01758d76c2b9a693567e1657ec661229bc32eac30761fa79b2474b0", - "sha256:26f4fbd6f5e1dabff70a9ba0d2c4bd30761086454aa30dddc5b52764ee4852b7", - "sha256:3b15d56a9cd40c52d7ab763ff0bc700edbb4e1a298dc43715ecccd605002cf11", - "sha256:3b2378ad387f49cbb328205bda569b9f87288d6bc1bf4cd683c34523a2341efe", - "sha256:400599c0fe58d21522cae0e8b22318e09d9729451b17ee61ba8e1e7c0346565c", - "sha256:47b8cb81a7d18dbaf4fed6a61c3cecdb5adec7b4ac292bddb0d016d57e8507d5", - "sha256:53eaed412477c836e1b9522c19858a8557d6e595077830146182225613b11a75", - "sha256:58e626e1f7dfbb620d08d457325a4cdac65d1809680009f46bf41eaf74ad0187", - "sha256:5a52e1b006bfa5be04fe4debbcdd2688432a9af4b207a3f429c74ad625022641", - "sha256:5c7ca4e449ac9f99b3b9d4693debb1d6d237d1542dd6a56b3305fe8a9620f883", - "sha256:682e54f0ce8f45981878756d7203fd01e188cc6c8b2c5e2cf03675390b4534d5", - "sha256:76275ee077772c8dde04fb6c5bc24b91af1bb3e7f4816fd1852f1495a64dad93", - "sha256:79bfb2f0bd7cbf9ea256612c9523367e5ec51d7cd616ae20ca2c90f575d839a2", - "sha256:7f4dd50874177d2bb060d74769210f3bce1af87a8c7cf5b37d032ebf94f0aca3", - "sha256:8944a16020c07b682df861207b7e0efcd2f46c7488619cb55f65882279119389", - "sha256:8aa7009437640beb2768bfd06da049bad0df85f47ff18426261acecd1cf00897", - "sha256:9105ce82dcc32c73eb53a04c869b6a4bc756b43e4385f76ea7943e827f529e4d", - "sha256:933df612c453928f1c6faa9236161a1d999a26cd40abf1dc5d7ebbc6dbfb8fca", - "sha256:939f36f21a8c571686eb491acfffa9c7f1ac345087281b412d63ea39ca14ec4a", - "sha256:9491578147849b93e70d7c1d23cb1229458f71fc79c51d52dce0809b2ca44eea", - "sha256:9733b7f64bd9f807832d673355f79703f81f0b3e52bfce420fc00d8cb28c6a6c", - "sha256:a02f6c3e229d0b7220bd74600e9351e18bc0c361b05f29adae0d10599ae0e326", - "sha256:a0c0a9f06872330d0dd31b45607197caab3c22777600e88031bfe66799e70bb0", - "sha256:aa716b9122307c50686356cfb47bfbc66541868078d0c801341df31dca1232a9", - "sha256:acc4df99308111585121db217681f1ce0eecb48d3a828a2f9bbf9773f4937e9e", - "sha256:b64916959e4ae0ac78af7c3e8cef4becee0c0e9694ad477b4c6b3a536de6a544", - "sha256:d22702cadb86b6fcba0e6b907d9f84a312db9cd6934ee728144ce3018e715ee1", - "sha256:d3fcf0819dc3fea58be1fd1ca390851bdb719a549850e708ed858503ff25d995", - "sha256:d52e3b1868a4e8fd18b5cb15055c76820df514e26aa84cc02f593d99fef6707f", - "sha256:db1a5d3cc4ae943d674718d6c47d2d82488ddd94b93b9e12d24aabdbfe48caee", - "sha256:e3a21a720791712ed721c7b95d433e036134de6f18c77dbe96119eaf7aa08004", - "sha256:e8bf074363ce2babeb4764d94f8e65efd22e6a7c74860a4f05a6947afc020ff2", - "sha256:f16814a4a96dc04bf1da7d53ee8d5b1d6decfc1a92a63349bb15d37b6a263dd9", - "sha256:f2b22153870ca5cf2ab9c940d7bc38e8e9089fa0f7e5856ea195e1cf4ff43d5a", - "sha256:f790f8b3dff3d53453de6a7b7ddd173d2e020fb160baff578d578065b108a05f", - "sha256:fe51b79da0062f8e9d49ed0182a626a7dc7a0cbca0328f612c6ee5e4711c81e4" - ], - "version": "==1.1.0" - }, - "markupsafe": { - "hashes": [ - "sha256:00bc623926325b26bb9605ae9eae8a215691f33cae5df11ca5424f06f2d1f473", - "sha256:09027a7803a62ca78792ad89403b1b7a73a01c8cb65909cd876f7fcebd79b161", - "sha256:09c4b7f37d6c648cb13f9230d847adf22f8171b1ccc4d5682398e77f40309235", - "sha256:1027c282dad077d0bae18be6794e6b6b8c91d58ed8a8d89a89d59693b9131db5", - "sha256:13d3144e1e340870b25e7b10b98d779608c02016d5184cfb9927a9f10c689f42", - "sha256:24982cc2533820871eba85ba648cd53d8623687ff11cbb805be4ff7b4c971aff", - "sha256:29872e92839765e546828bb7754a68c418d927cd064fd4708fab9fe9c8bb116b", - "sha256:43a55c2930bbc139570ac2452adf3d70cdbb3cfe5912c71cdce1c2c6bbd9c5d1", - "sha256:46c99d2de99945ec5cb54f23c8cd5689f6d7177305ebff350a58ce5f8de1669e", - "sha256:500d4957e52ddc3351cabf489e79c91c17f6e0899158447047588650b5e69183", - "sha256:535f6fc4d397c1563d08b88e485c3496cf5784e927af890fb3c3aac7f933ec66", - "sha256:596510de112c685489095da617b5bcbbac7dd6384aeebeda4df6025d0256a81b", - "sha256:62fe6c95e3ec8a7fad637b7f3d372c15ec1caa01ab47926cfdf7a75b40e0eac1", - "sha256:6788b695d50a51edb699cb55e35487e430fa21f1ed838122d722e0ff0ac5ba15", - "sha256:6dd73240d2af64df90aa7c4e7481e23825ea70af4b4922f8ede5b9e35f78a3b1", - "sha256:717ba8fe3ae9cc0006d7c451f0bb265ee07739daf76355d06366154ee68d221e", - "sha256:79855e1c5b8da654cf486b830bd42c06e8780cea587384cf6545b7d9ac013a0b", - "sha256:7c1699dfe0cf8ff607dbdcc1e9b9af1755371f92a68f706051cc8c37d447c905", - "sha256:88e5fcfb52ee7b911e8bb6d6aa2fd21fbecc674eadd44118a9cc3863f938e735", - "sha256:8defac2f2ccd6805ebf65f5eeb132adcf2ab57aa11fdf4c0dd5169a004710e7d", - "sha256:98c7086708b163d425c67c7a91bad6e466bb99d797aa64f965e9d25c12111a5e", - "sha256:9add70b36c5666a2ed02b43b335fe19002ee5235efd4b8a89bfcf9005bebac0d", - "sha256:9bf40443012702a1d2070043cb6291650a0841ece432556f784f004937f0f32c", - "sha256:ade5e387d2ad0d7ebf59146cc00c8044acbd863725f887353a10df825fc8ae21", - "sha256:b00c1de48212e4cc9603895652c5c410df699856a2853135b3967591e4beebc2", - "sha256:b1282f8c00509d99fef04d8ba936b156d419be841854fe901d8ae224c59f0be5", - "sha256:b2051432115498d3562c084a49bba65d97cf251f5a331c64a12ee7e04dacc51b", - "sha256:ba59edeaa2fc6114428f1637ffff42da1e311e29382d81b339c1817d37ec93c6", - "sha256:c8716a48d94b06bb3b2524c2b77e055fb313aeb4ea620c8dd03a105574ba704f", - "sha256:cd5df75523866410809ca100dc9681e301e3c27567cf498077e8551b6d20e42f", - "sha256:cdb132fc825c38e1aeec2c8aa9338310d29d337bebbd7baa06889d09a60a1fa2", - "sha256:e249096428b3ae81b08327a63a485ad0878de3fb939049038579ac0ef61e17e7", - "sha256:e8313f01ba26fbbe36c7be1966a7b7424942f670f38e666995b88d012765b9be" - ], - "version": "==1.1.1" - }, - "matplotlib": { - "hashes": [ - "sha256:23b71560c721109954c0215ffc81f4c80ce8528749d534a01a61e8ab737c5bce", - "sha256:4164265ca573481ce61c83322e6b33628203afeabeb3e22c50376f5d3ee0f9be", - "sha256:470eed601ff5132364e0121a20d7c3d43fab969c8c333422c1b6b72fde2ed3c1", - "sha256:635ded7834f43c8d999076236f7e90074d77f7b8345e5e82cd95af053cc29df1", - "sha256:6a0031774c6c68298183438edf2e738856d63a4c4797876fa81d0ee337f5361c", - "sha256:78d0772412c0653aa3e860c52ff08d1f5ba64334e2b86b09dc2d502657d8ca73", - "sha256:8efff896c49676700dc6adace6137a854ff64a4d44ca057ff726960ffdaa47bf", - "sha256:97f04d29a358826f205320fbc88d46ce5c5ff6fb54ae050042ff396beda52ca4", - "sha256:b4c0010eff09ab65c77ad1a0eec6c7cccb9f6838c3c77dc5b4002fe0cf2912fd", - "sha256:b5ace0531255932ad19fe64c116ada2713f7b38381db8f68df0fa694409e67d1", - "sha256:c7bb7ed3e011324b56462391ec3f4bbb7c8c6af5892ebfb45d312b15b4cdfc8d", - "sha256:db3121f12fb9b99f105d1413aebaeb3d943f269f3d262b45586d12765866f0c6", - "sha256:db8bbba9284845034a2f0e1add91dc5e89db8c996359bdcf677a8d6f88875cf1", - "sha256:f0023322c99328c40ce22678ab0ab5adfc27e338419966539398239996f63e8d" - ], - "index": "pypi", - "version": "==3.1.3" - }, - "mistune": { - "hashes": [ - "sha256:59a3429db53c50b5c6bcc8a07f8848cb00d7dc8bdb431a4ab41920d201d4756e", - "sha256:88a1051873018da288eee8538d476dffe1262495144b33ecb586c4ab266bb8d4" - ], - "version": "==0.8.4" - }, - "nbconvert": { - "hashes": [ - "sha256:21fb48e700b43e82ba0e3142421a659d7739b65568cc832a13976a77be16b523", - "sha256:f0d6ec03875f96df45aa13e21fd9b8450c42d7e1830418cccc008c0df725fcee" - ], - "version": "==5.6.1" - }, - "nbformat": { - "hashes": [ - "sha256:562de41fc7f4f481b79ab5d683279bf3a168858268d4387b489b7b02be0b324a", - "sha256:f4bbbd8089bd346488f00af4ce2efb7f8310a74b2058040d075895429924678c" - ], - "version": "==5.0.4" - }, - "notebook": { - "hashes": [ - "sha256:3edc616c684214292994a3af05eaea4cc043f6b4247d830f3a2f209fa7639a80", - "sha256:47a9092975c9e7965ada00b9a20f0cf637d001db60d241d479f53c0be117ad48" - ], - "version": "==6.0.3" - }, - "numpy": { - "hashes": [ - "sha256:1786a08236f2c92ae0e70423c45e1e62788ed33028f94ca99c4df03f5be6b3c6", - "sha256:17aa7a81fe7599a10f2b7d95856dc5cf84a4eefa45bc96123cbbc3ebc568994e", - "sha256:20b26aaa5b3da029942cdcce719b363dbe58696ad182aff0e5dcb1687ec946dc", - "sha256:2d75908ab3ced4223ccba595b48e538afa5ecc37405923d1fea6906d7c3a50bc", - "sha256:39d2c685af15d3ce682c99ce5925cc66efc824652e10990d2462dfe9b8918c6a", - "sha256:56bc8ded6fcd9adea90f65377438f9fea8c05fcf7c5ba766bef258d0da1554aa", - "sha256:590355aeade1a2eaba17617c19edccb7db8d78760175256e3cf94590a1a964f3", - "sha256:70a840a26f4e61defa7bdf811d7498a284ced303dfbc35acb7be12a39b2aa121", - "sha256:77c3bfe65d8560487052ad55c6998a04b654c2fbc36d546aef2b2e511e760971", - "sha256:9537eecf179f566fd1c160a2e912ca0b8e02d773af0a7a1120ad4f7507cd0d26", - "sha256:9acdf933c1fd263c513a2df3dceecea6f3ff4419d80bf238510976bf9bcb26cd", - "sha256:ae0975f42ab1f28364dcda3dde3cf6c1ddab3e1d4b2909da0cb0191fa9ca0480", - "sha256:b3af02ecc999c8003e538e60c89a2b37646b39b688d4e44d7373e11c2debabec", - "sha256:b6ff59cee96b454516e47e7721098e6ceebef435e3e21ac2d6c3b8b02628eb77", - "sha256:b765ed3930b92812aa698a455847141869ef755a87e099fddd4ccf9d81fffb57", - "sha256:c98c5ffd7d41611407a1103ae11c8b634ad6a43606eca3e2a5a269e5d6e8eb07", - "sha256:cf7eb6b1025d3e169989416b1adcd676624c2dbed9e3bcb7137f51bfc8cc2572", - "sha256:d92350c22b150c1cae7ebb0ee8b5670cc84848f6359cf6b5d8f86617098a9b73", - "sha256:e422c3152921cece8b6a2fb6b0b4d73b6579bd20ae075e7d15143e711f3ca2ca", - "sha256:e840f552a509e3380b0f0ec977e8124d0dc34dc0e68289ca28f4d7c1d0d79474", - "sha256:f3d0a94ad151870978fb93538e95411c83899c9dc63e6fb65542f769568ecfa5" - ], - "index": "pypi", - "version": "==1.18.1" - }, - "open3d": { - "hashes": [ - "sha256:0227e2100784d743a4300ef01b20f61995c94e4398cf91f6f4f8fb4f5fc67666", - "sha256:10b875f2f010f81a8335b0bce4f92f9aa2942abac3f3c1989b347c4a636e526e", - "sha256:1149bf4631c97019aa8ff0836b82512b958456ae22fbbaef6a3b6b42d4d0fc6c", - "sha256:6836d4f080c762b7a0d1263bc7833dd7de69d5563db595d39360f25c99cad377", - "sha256:80d381bae9c789da9db8ecf9f21d94b6b8bfecaf290a56a37be5310f6b587b9f", - "sha256:9558b5ce3deb6ed4b508fb2e726d5fcf320b83295f1df8d3faf3236a3384a935", - "sha256:a0bc835b1019cd5c8942f5657c2217cfc704850d3412f1d6e5fae15d20cc234f", - "sha256:b4f8f509eff5025b4694877c34d75aa77845ed58827c5d56a53849eb763dcbb7", - "sha256:be8df1dc8db8ef98cfb98e3488c24c5fa2ec4a33e0f21ad521ef6d141b7795e0", - "sha256:d1f6b19b11e6698379c3229e759dedffe2b948e78c35f7bf052732055fd236cb", - "sha256:f5448460b333e023a340700c3e48e2ab77341a91a967c566f75e9fd03a54dcb4", - "sha256:f8c1a476178faddfb21cee9403ae97bb2fbf7337f15574227fcf8f248715ed6e" - ], - "index": "pypi", - "version": "==0.9.0.0" - }, - "pandocfilters": { - "hashes": [ - "sha256:b3dd70e169bb5449e6bc6ff96aea89c5eea8c5f6ab5e207fc2f521a2cf4a0da9" - ], - "version": "==1.4.2" - }, - "parso": { - "hashes": [ - "sha256:56b2105a80e9c4df49de85e125feb6be69f49920e121406f15e7acde6c9dfc57", - "sha256:951af01f61e6dccd04159042a0706a31ad437864ec6e25d0d7a96a9fbb9b0095" - ], - "version": "==0.6.1" - }, - "pexpect": { - "hashes": [ - "sha256:0b48a55dcb3c05f3329815901ea4fc1537514d6ba867a152b581d69ae3710937", - "sha256:fc65a43959d153d0114afe13997d439c22823a27cefceb5ff35c2178c6784c0c" - ], - "markers": "sys_platform != 'win32'", - "version": "==4.8.0" - }, - "pickleshare": { - "hashes": [ - "sha256:87683d47965c1da65cdacaf31c8441d12b8044cdec9aca500cd78fc2c683afca", - "sha256:9649af414d74d4df115d5d718f82acb59c9d418196b7b4290ed47a12ce62df56" - ], - "version": "==0.7.5" - }, - "pkginfo": { - "hashes": [ - "sha256:7424f2c8511c186cd5424bbf31045b77435b37a8d604990b79d4e70d741148bb", - "sha256:a6d9e40ca61ad3ebd0b72fbadd4fba16e4c0e4df0428c041e01e06eb6ee71f32" - ], - "version": "==1.5.0.1" - }, - "prometheus-client": { - "hashes": [ - "sha256:71cd24a2b3eb335cb800c7159f423df1bd4dcd5171b234be15e3f31ec9f622da" - ], - "version": "==0.7.1" - }, - "prompt-toolkit": { - "hashes": [ - "sha256:a402e9bf468b63314e37460b68ba68243d55b2f8c4d0192f85a019af3945050e", - "sha256:c93e53af97f630f12f5f62a3274e79527936ed466f038953dfa379d4941f651a" - ], - "version": "==3.0.3" - }, - "ptyprocess": { - "hashes": [ - "sha256:923f299cc5ad920c68f2bc0bc98b75b9f838b93b599941a6b63ddbc2476394c0", - "sha256:d7cc528d76e76342423ca640335bd3633420dc1366f258cb31d05e865ef5ca1f" - ], - "markers": "os_name != 'nt'", - "version": "==0.6.0" - }, - "pycparser": { - "hashes": [ - "sha256:a988718abfad80b6b157acce7bf130a30876d27603738ac39f140993246b25b3" - ], - "version": "==2.19" - }, - "pygments": { - "hashes": [ - "sha256:2a3fe295e54a20164a9df49c75fa58526d3be48e14aceba6d6b1e8ac0bfd6f1b", - "sha256:98c8aa5a9f778fcd1026a17361ddaf7330d1b7c62ae97c3bb0ae73e0b9b6b0fe" - ], - "version": "==2.5.2" - }, - "pyparsing": { - "hashes": [ - "sha256:4c830582a84fb022400b85429791bc551f1f4871c33f23e44f353119e92f969f", - "sha256:c342dccb5250c08d45fd6f8b4a559613ca603b57498511740e65cd11a2e7dcec" - ], - "version": "==2.4.6" - }, - "pyrealsense2": { - "hashes": [ - "sha256:230d603b0f3de41d94edb2719b2662abca07ef227e106865b7da1ab7cb332e2f", - "sha256:338be9aa81f95aac8e6c2a243dd3a1a0d64a0c4a0ba3970036aaf8e9a95919bf", - "sha256:601ad8f449c1788f4ee2ed92d486d498d16be7e2fceb3fc9e2cecc381d525afe", - "sha256:b3d93636c703129dec47d74932486c893cd3849ca324cea74df24983739db31e", - "sha256:b5137ab1636d04dd171f7c1ba3fa7fd0e54e2bfeb9a495bfb949b216133dae17", - "sha256:b8ea201dc2903b0787100e609746802d46820df35efa9252e24570f251a3fc8d", - "sha256:c0f96acda11e02faf8fbf90b36625f06c3550f37901f0a85c8bfb304c802dd8d", - "sha256:dc299a3fc815ddd79598695b39533356056aa536189e26e374c0102c164c2dd3", - "sha256:ee1d08f817d8d6d817fcae69c5ff17000476676399f069d15570d43623464d0c", - "sha256:efc24fa4f3ec245cf053fb9d4f30f2299b81fc2dcefe6399195453cd68febf78" - ], - "index": "pypi", - "version": "==2.32.1.1299" - }, - "pyrsistent": { - "hashes": [ - "sha256:cdc7b5e3ed77bed61270a47d35434a30617b9becdf2478af76ad2c6ade307280" - ], - "version": "==0.15.7" - }, - "python-dateutil": { - "hashes": [ - "sha256:73ebfe9dbf22e832286dafa60473e4cd239f8592f699aa5adaf10050e6e1823c", - "sha256:75bb3f31ea686f1197762692a9ee6a7550b59fc6ca3a1f4b5d7e32fb98e2da2a" - ], - "version": "==2.8.1" - }, - "pyzmq": { - "hashes": [ - "sha256:01b588911714a6696283de3904f564c550c9e12e8b4995e173f1011755e01086", - "sha256:0573b9790aa26faff33fba40f25763657271d26f64bffb55a957a3d4165d6098", - "sha256:0fa82b9fc3334478be95a5566f35f23109f763d1669bb762e3871a8fa2a4a037", - "sha256:1e59b7b19396f26e360f41411a5d4603356d18871049cd7790f1a7d18f65fb2c", - "sha256:2a294b4f44201bb21acc2c1a17ff87fbe57b82060b10ddb00ac03e57f3d7fcfa", - "sha256:355b38d7dd6f884b8ee9771f59036bcd178d98539680c4f87e7ceb2c6fd057b6", - "sha256:4b73d20aec63933bbda7957e30add233289d86d92a0bb9feb3f4746376f33527", - "sha256:4ec47f2b50bdb97df58f1697470e5c58c3c5109289a623e30baf293481ff0166", - "sha256:5541dc8cad3a8486d58bbed076cb113b65b5dd6b91eb94fb3e38a3d1d3022f20", - "sha256:6fca7d11310430e751f9832257866a122edf9d7b635305c5d8c51f74a5174d3d", - "sha256:7369656f89878455a5bcd5d56ca961884f5d096268f71c0750fc33d6732a25e5", - "sha256:75d73ee7ca4b289a2a2dfe0e6bd8f854979fc13b3fe4ebc19381be3b04e37a4a", - "sha256:80c928d5adcfa12346b08d31360988d843b54b94154575cccd628f1fe91446bc", - "sha256:83ce18b133dc7e6789f64cb994e7376c5aa6b4aeced993048bf1d7f9a0fe6d3a", - "sha256:8b8498ceee33a7023deb2f3db907ca41d6940321e282297327a9be41e3983792", - "sha256:8c69a6cbfa94da29a34f6b16193e7c15f5d3220cb772d6d17425ff3faa063a6d", - "sha256:8ff946b20d13a99dc5c21cb76f4b8b253eeddf3eceab4218df8825b0c65ab23d", - "sha256:972d723a36ab6a60b7806faa5c18aa3c080b7d046c407e816a1d8673989e2485", - "sha256:a6c9c42bbdba3f9c73aedbb7671815af1943ae8073e532c2b66efb72f39f4165", - "sha256:aa3872f2ebfc5f9692ef8957fe69abe92d905a029c0608e45ebfcd451ad30ab5", - "sha256:cf08435b14684f7f2ca2df32c9df38a79cdc17c20dc461927789216cb43d8363", - "sha256:d30db4566177a6205ed1badb8dbbac3c043e91b12a2db5ef9171b318c5641b75", - "sha256:d5ac84f38575a601ab20c1878818ffe0d09eb51d6cb8511b636da46d0fd8949a", - "sha256:e37f22eb4bfbf69cd462c7000616e03b0cdc1b65f2d99334acad36ea0e4ddf6b", - "sha256:e6549dd80de7b23b637f586217a4280facd14ac01e9410a037a13854a6977299", - "sha256:ed6205ca0de035f252baa0fd26fdd2bc8a8f633f92f89ca866fd423ff26c6f25", - "sha256:efdde21febb9b5d7a8e0b87ea2549d7e00fda1936459cfb27fb6fca0c36af6c1", - "sha256:f4e72646bfe79ff3adbf1314906bbd2d67ef9ccc71a3a98b8b2ccbcca0ab7bec" - ], - "version": "==18.1.1" - }, - "readme-renderer": { - "hashes": [ - "sha256:bb16f55b259f27f75f640acf5e00cf897845a8b3e4731b5c1a436e4b8529202f", - "sha256:c8532b79afc0375a85f10433eca157d6b50f7d6990f337fa498c96cd4bfc203d" - ], - "version": "==24.0" - }, - "requests": { - "hashes": [ - "sha256:11e007a8a2aa0323f5a921e9e6a2d7e4e67d9877e85773fba9ba6419025cbeb4", - "sha256:9cf5292fcd0f598c671cfc1e0d7d1a7f13bb8085e9a590f48c010551dc6c4b31" - ], - "version": "==2.22.0" - }, - "requests-toolbelt": { - "hashes": [ - "sha256:380606e1d10dc85c3bd47bf5a6095f815ec007be7a8b69c878507068df059e6f", - "sha256:968089d4584ad4ad7c171454f0a5c6dac23971e9472521ea3b6d49d610aa6fc0" - ], - "version": "==0.9.1" - }, - "secretstorage": { - "hashes": [ - "sha256:15da8a989b65498e29be338b3b279965f1b8f09b9668bd8010da183024c8bff6", - "sha256:b5ec909dde94d4ae2fa26af7c089036997030f0cf0a5cb372b4cccabd81c143b" - ], - "markers": "sys_platform == 'linux'", - "version": "==3.1.2" - }, - "send2trash": { - "hashes": [ - "sha256:60001cc07d707fe247c94f74ca6ac0d3255aabcb930529690897ca2a39db28b2", - "sha256:f1691922577b6fa12821234aeb57599d887c4900b9ca537948d2dac34aea888b" - ], - "version": "==1.5.0" - }, - "six": { - "hashes": [ - "sha256:236bdbdce46e6e6a3d61a337c0f8b763ca1e8717c03b369e87a7ec7ce1319c0a", - "sha256:8f3cd2e254d8f793e7f3d6d9df77b92252b52637291d0f0da013c76ea2724b6c" - ], - "version": "==1.14.0" - }, - "terminado": { - "hashes": [ - "sha256:4804a774f802306a7d9af7322193c5390f1da0abb429e082a10ef1d46e6fb2c2", - "sha256:a43dcb3e353bc680dd0783b1d9c3fc28d529f190bc54ba9a229f72fe6e7a54d7" - ], - "version": "==0.8.3" - }, - "testpath": { - "hashes": [ - "sha256:60e0a3261c149755f4399a1fff7d37523179a70fdc3abdf78de9fc2604aeec7e", - "sha256:bfcf9411ef4bf3db7579063e0546938b1edda3d69f4e1fb8756991f5951f85d4" - ], - "version": "==0.4.4" - }, - "torch": { - "hashes": [ - "sha256:0698d0a48014b9b8f36d93e69901eca2e7ec712cd2033908f7a77e7d86a4f0d7", - "sha256:2ac8e58b069232f079bd289aa160366a9367ae1a4616a2c1007dceed19ff9bfa", - "sha256:43a0e28c448ddeea65fb9e956bc743389592afac824095bdbc08e8a87364c639", - "sha256:661ad06b4616663149bd504e8c0271196d0386712e21a92619d95ba88138794a", - "sha256:880a0c22692eaebbce808a5bf2255ab7d345ab43c40795be0a421c6250ba0fb4", - "sha256:a13bf6f78a49d844b85c142b8cd62d2e1833a11ed21ea0bc6b1ac73d24c76415", - "sha256:a8c21f82fd03b67927078ea917040478c3263753fe1906fc19d0f5f0c7d9aa10", - "sha256:b87fd224a7de3bc01ce87eb947698797b4514e27115b0aa60a56991515dd9dd6", - "sha256:f63d489c54b4f170ce8335727bbb196ceb9acd0e7805477bbef8fabc914bc0f9" - ], - "index": "pypi", - "version": "==1.2.0" - }, - "tornado": { - "hashes": [ - "sha256:349884248c36801afa19e342a77cc4458caca694b0eda633f5878e458a44cb2c", - "sha256:398e0d35e086ba38a0427c3b37f4337327231942e731edaa6e9fd1865bbd6f60", - "sha256:4e73ef678b1a859f0cb29e1d895526a20ea64b5ffd510a2307b5998c7df24281", - "sha256:559bce3d31484b665259f50cd94c5c28b961b09315ccd838f284687245f416e5", - "sha256:abbe53a39734ef4aba061fca54e30c6b4639d3e1f59653f0da37a0003de148c7", - "sha256:c845db36ba616912074c5b1ee897f8e0124df269468f25e4fe21fe72f6edd7a9", - "sha256:c9399267c926a4e7c418baa5cbe91c7d1cf362d505a1ef898fde44a07c9dd8a5" - ], - "version": "==6.0.3" - }, - "tqdm": { - "hashes": [ - "sha256:251ee8440dbda126b8dfa8a7c028eb3f13704898caaef7caa699b35e119301e2", - "sha256:fe231261cfcbc6f4a99165455f8f6b9ef4e1032a6e29bccf168b4bf42012f09c" - ], - "version": "==4.42.1" - }, - "traitlets": { - "hashes": [ - "sha256:70b4c6a1d9019d7b4f6846832288f86998aa3b9207c6821f3578a6a6a467fe44", - "sha256:d023ee369ddd2763310e4c3eae1ff649689440d4ae59d7485eb4cfbbe3e359f7" - ], - "version": "==4.3.3" - }, - "twine": { - "hashes": [ - "sha256:c1af8ca391e43b0a06bbc155f7f67db0bf0d19d284bfc88d1675da497a946124", - "sha256:d561a5e511f70275e5a485a6275ff61851c16ffcb3a95a602189161112d9f160" - ], - "index": "pypi", - "version": "==3.1.1" - }, - "urllib3": { - "hashes": [ - "sha256:2f3db8b19923a873b3e5256dc9c2dedfa883e33d87c690d9c7913e1f40673cdc", - "sha256:87716c2d2a7121198ebcb7ce7cccf6ce5e9ba539041cfbaeecfb641dc0bf6acc" - ], - "version": "==1.25.8" - }, - "wcwidth": { - "hashes": [ - "sha256:8fd29383f539be45b20bd4df0dc29c20ba48654a41e661925e612311e9f3c603", - "sha256:f28b3e8a6483e5d49e7f8949ac1a78314e740333ae305b4ba5defd3e74fb37a8" - ], - "version": "==0.1.8" - }, - "webencodings": { - "hashes": [ - "sha256:a0af1213f3c2226497a97e2b3aa01a7e4bee4f403f95be16fc9acd2947514a78", - "sha256:b36a1c245f2d304965eb4e0a82848379241dc04b865afcc4aab16748587e1923" - ], - "version": "==0.5.1" - }, - "wheel": { - "hashes": [ - "sha256:8788e9155fe14f54164c1b9eb0a319d98ef02c160725587ad60f14ddc57b6f96", - "sha256:df277cb51e61359aba502208d680f90c0493adec6f0e848af94948778aed386e" - ], - "index": "pypi", - "version": "==0.34.2" - }, - "widgetsnbextension": { - "hashes": [ - "sha256:079f87d87270bce047512400efd70238820751a11d2d8cb137a5a5bdbaf255c7", - "sha256:bd314f8ceb488571a5ffea6cc5b9fc6cba0adaf88a9d2386b93a489751938bcd" - ], - "version": "==3.5.1" - }, - "zipp": { - "hashes": [ - "sha256:5c56e330306215cd3553342cfafc73dda2c60792384117893f3a83f8a1209f50", - "sha256:d65287feb793213ffe11c0f31b81602be31448f38aeb8ffc2eb286c4f6f6657e" - ], - "version": "==2.2.0" - } - }, - "develop": {} -} diff --git a/src/python/cupoch_pybind/cupoch_pybind.cpp b/src/python/cupoch_pybind/cupoch_pybind.cpp index aedd9fb3..50bef537 100644 --- a/src/python/cupoch_pybind/cupoch_pybind.cpp +++ b/src/python/cupoch_pybind/cupoch_pybind.cpp @@ -1,6 +1,7 @@ #include "cupoch_pybind/cupoch_pybind.h" -#include "cupoch_pybind/docstring.h" + #include "cupoch_pybind/camera/camera.h" +#include "cupoch_pybind/docstring.h" #include "cupoch_pybind/geometry/geometry.h" #include "cupoch_pybind/io/io.h" #include "cupoch_pybind/odometry/odometry.h" @@ -13,22 +14,23 @@ PYBIND11_MODULE(cupoch, m) { py::enum_ rmm_mode(m, "AllocationMode", py::arithmetic()); - rmm_mode.value("CudaDefaultAllocation", CudaDefaultAllocation) - .value("PoolAllocation", PoolAllocation) - .value("CudaManagedMemory", CudaManagedMemory) - .export_values(); + rmm_mode.value("CudaDefaultAllocation", CudaDefaultAllocation) + .value("PoolAllocation", PoolAllocation) + .value("CudaManagedMemory", CudaManagedMemory) + .export_values(); m.def("initialize_allocator", &cupoch::utility::InitializeAllocator, py::arg("mode") = CudaDefaultAllocation, - py::arg("initial_pool_size") = 0, - py::arg("logging") = false, + py::arg("initial_pool_size") = 0, py::arg("logging") = false, py::arg("devices") = std::vector()); cupoch::docstring::FunctionDocInject( m, "initialize_allocator", { - {"mode", "Allocation strategy to use"}, - {"initial_pool_size", "When pool suballocation is enabled, this is the initial pool size in bytes"}, - {"logging", "Enable logging memory manager events"}, - {"devices", "List of GPU device IDs to register"}, + {"mode", "Allocation strategy to use"}, + {"initial_pool_size", + "When pool suballocation is enabled, this is the initial " + "pool size in bytes"}, + {"logging", "Enable logging memory manager events"}, + {"devices", "List of GPU device IDs to register"}, }); bind_device_vector_wrapper(m); pybind_utility(m); diff --git a/src/python/cupoch_pybind/cupoch_pybind.h b/src/python/cupoch_pybind/cupoch_pybind.h index 563b37a3..cae2bcf8 100644 --- a/src/python/cupoch_pybind/cupoch_pybind.h +++ b/src/python/cupoch_pybind/cupoch_pybind.h @@ -8,6 +8,7 @@ #include // Include first to suppress compiler warnings #include #include + #include "cupoch_pybind/device_vector_wrapper.h" namespace py = pybind11; @@ -17,7 +18,8 @@ using namespace py::literals; namespace pybind11 { namespace detail { -template struct type_caster> +template +struct type_caster> : list_caster, Type> {}; template @@ -33,19 +35,30 @@ void bind_copy_functions(Class_ &cl) { } inline void bind_device_vector_wrapper(py::module &m) { - py::class_> dvv3f(m, "device_vector_vector3f"); - dvv3f.def("cpu", &cupoch::wrapper::device_vector_wrapper::cpu); - py::class_> dvv2f(m, "device_vector_vector2f"); - dvv2f.def("cpu", &cupoch::wrapper::device_vector_wrapper::cpu); - py::class_> dvv3i(m, "device_vector_vector3i"); - dvv3i.def("cpu", &cupoch::wrapper::device_vector_wrapper::cpu); - py::class_> dvv2i(m, "device_vector_vector2i"); - dvv2i.def("cpu", &cupoch::wrapper::device_vector_wrapper::cpu); - py::class_> dvf(m, "device_vector_float"); + py::class_> dvv3f( + m, "device_vector_vector3f"); + dvv3f.def("cpu", + &cupoch::wrapper::device_vector_wrapper::cpu); + py::class_> dvv2f( + m, "device_vector_vector2f"); + dvv2f.def("cpu", + &cupoch::wrapper::device_vector_wrapper::cpu); + py::class_> dvv3i( + m, "device_vector_vector3i"); + dvv3i.def("cpu", + &cupoch::wrapper::device_vector_wrapper::cpu); + py::class_> dvv2i( + m, "device_vector_vector2i"); + dvv2i.def("cpu", + &cupoch::wrapper::device_vector_wrapper::cpu); + py::class_> dvf( + m, "device_vector_float"); dvf.def("cpu", &cupoch::wrapper::device_vector_wrapper::cpu); - py::class_> dvi(m, "device_vector_int"); + py::class_> dvi( + m, "device_vector_int"); dvi.def("cpu", &cupoch::wrapper::device_vector_wrapper::cpu); - py::class_> dvs(m, "device_vector_size_t"); + py::class_> dvs( + m, "device_vector_size_t"); dvs.def("cpu", &cupoch::wrapper::device_vector_wrapper::cpu); } @@ -54,4 +67,5 @@ inline void bind_device_vector_wrapper(py::module &m) { PYBIND11_MAKE_OPAQUE(thrust::host_vector>); PYBIND11_MAKE_OPAQUE(thrust::host_vector>); -PYBIND11_MAKE_OPAQUE(thrust::host_vector>); \ No newline at end of file +PYBIND11_MAKE_OPAQUE( + thrust::host_vector>); \ No newline at end of file diff --git a/src/python/cupoch_pybind/device_vector_wrapper.cu b/src/python/cupoch_pybind/device_vector_wrapper.cu index b342d606..aa79b24f 100644 --- a/src/python/cupoch_pybind/device_vector_wrapper.cu +++ b/src/python/cupoch_pybind/device_vector_wrapper.cu @@ -1,29 +1,38 @@ -#include "cupoch_pybind/device_vector_wrapper.h" #include "cupoch/geometry/pointcloud.h" +#include "cupoch_pybind/device_vector_wrapper.h" namespace cupoch { namespace wrapper { -template -device_vector_wrapper::device_vector_wrapper() {}; -template -device_vector_wrapper::device_vector_wrapper(const device_vector_wrapper& other) : data_(other.data_) {} -template -device_vector_wrapper::device_vector_wrapper(const thrust::host_vector& other) : data_(other) {} -template -device_vector_wrapper::device_vector_wrapper(const utility::device_vector& other) : data_(other) {} -template -device_vector_wrapper::device_vector_wrapper(utility::device_vector&& other) noexcept: data_(std::move(other)) {} -template -device_vector_wrapper::~device_vector_wrapper() {}; - -template -device_vector_wrapper &device_vector_wrapper::operator=(const device_vector_wrapper &other) { +template +device_vector_wrapper::device_vector_wrapper(){}; +template +device_vector_wrapper::device_vector_wrapper( + const device_vector_wrapper& other) + : data_(other.data_) {} +template +device_vector_wrapper::device_vector_wrapper( + const thrust::host_vector& other) + : data_(other) {} +template +device_vector_wrapper::device_vector_wrapper( + const utility::device_vector& other) + : data_(other) {} +template +device_vector_wrapper::device_vector_wrapper( + utility::device_vector&& other) noexcept + : data_(std::move(other)) {} +template +device_vector_wrapper::~device_vector_wrapper(){}; + +template +device_vector_wrapper& device_vector_wrapper::operator=( + const device_vector_wrapper& other) { data_ = other.data_; return *this; } -template +template thrust::host_vector device_vector_wrapper::cpu() const { thrust::host_vector ans = data_; return ans; @@ -37,18 +46,30 @@ template class device_vector_wrapper; template class device_vector_wrapper; template class device_vector_wrapper; -template -void FromWrapper(utility::device_vector& dv, const device_vector_wrapper& vec) { +template +void FromWrapper(utility::device_vector& dv, + const device_vector_wrapper& vec) { dv = vec.data_; } -template void FromWrapper(utility::device_vector& dv, const device_vector_wrapper& vec); -template void FromWrapper(utility::device_vector& dv, const device_vector_wrapper& vec); -template void FromWrapper(utility::device_vector& dv, const device_vector_wrapper& vec); -template void FromWrapper(utility::device_vector& dv, const device_vector_wrapper& vec); -template void FromWrapper(utility::device_vector& dv, const device_vector_wrapper& vec); -template void FromWrapper(utility::device_vector& dv, const device_vector_wrapper& vec); -template void FromWrapper(utility::device_vector& dv, const device_vector_wrapper& vec); +template void FromWrapper( + utility::device_vector& dv, + const device_vector_wrapper& vec); +template void FromWrapper( + utility::device_vector& dv, + const device_vector_wrapper& vec); +template void FromWrapper( + utility::device_vector& dv, + const device_vector_wrapper& vec); +template void FromWrapper( + utility::device_vector& dv, + const device_vector_wrapper& vec); +template void FromWrapper(utility::device_vector& dv, + const device_vector_wrapper& vec); +template void FromWrapper(utility::device_vector& dv, + const device_vector_wrapper& vec); +template void FromWrapper(utility::device_vector& dv, + const device_vector_wrapper& vec); -} -} \ No newline at end of file +} // namespace wrapper +} // namespace cupoch \ No newline at end of file diff --git a/src/python/cupoch_pybind/device_vector_wrapper.h b/src/python/cupoch_pybind/device_vector_wrapper.h index f3229575..e27f98a0 100644 --- a/src/python/cupoch_pybind/device_vector_wrapper.h +++ b/src/python/cupoch_pybind/device_vector_wrapper.h @@ -1,7 +1,9 @@ #pragma once -#include #include + +#include + #include "cupoch/utility/device_vector.h" namespace cupoch { @@ -9,11 +11,11 @@ namespace cupoch { namespace geometry { class PointCloud; class TriangleMesh; -} +} // namespace geometry namespace wrapper { -template +template class device_vector_wrapper { public: device_vector_wrapper(); @@ -22,7 +24,8 @@ class device_vector_wrapper { device_vector_wrapper(const utility::device_vector& other); device_vector_wrapper(utility::device_vector&& other) noexcept; ~device_vector_wrapper(); - device_vector_wrapper &operator=(const device_vector_wrapper &other); + device_vector_wrapper& operator=( + const device_vector_wrapper& other); thrust::host_vector cpu() const; utility::device_vector data_; }; @@ -35,8 +38,9 @@ using device_vector_int = device_vector_wrapper; using device_vector_size_t = device_vector_wrapper; using device_vector_float = device_vector_wrapper; -template -void FromWrapper(utility::device_vector& dv, const device_vector_wrapper& vec); +template +void FromWrapper(utility::device_vector& dv, + const device_vector_wrapper& vec); -} -} \ No newline at end of file +} // namespace wrapper +} // namespace cupoch \ No newline at end of file diff --git a/src/python/cupoch_pybind/dl_converter.cpp b/src/python/cupoch_pybind/dl_converter.cpp index 4a72ab47..5d7bf1ad 100644 --- a/src/python/cupoch_pybind/dl_converter.cpp +++ b/src/python/cupoch_pybind/dl_converter.cpp @@ -1,27 +1,31 @@ -#include #include "cupoch_pybind/dl_converter.h" + +#include + #include "cupoch/utility/dl_converter.h" using namespace cupoch; using namespace cupoch::dlpack; -py::capsule cupoch::dlpack::ToDLpackCapsule(utility::device_vector& src) { +py::capsule cupoch::dlpack::ToDLpackCapsule( + utility::device_vector &src) { void const *managed_tensor = utility::ToDLPack(src); return py::capsule(managed_tensor, "dltensor", [](::PyObject *obj) { auto *ptr = ::PyCapsule_GetPointer(obj, "dltensor"); if (ptr != nullptr) { - auto *m_tsr = static_cast<::DLManagedTensor*>(ptr); + auto *m_tsr = static_cast<::DLManagedTensor *>(ptr); m_tsr->deleter(m_tsr); - } - else { + } else { ::PyErr_Clear(); } }); } -void cupoch::dlpack::FromDLpackCapsule(py::capsule dlpack, utility::device_vector& dst) { +void cupoch::dlpack::FromDLpackCapsule( + py::capsule dlpack, utility::device_vector &dst) { auto obj = py::cast(dlpack); - ::DLManagedTensor* managed_tensor = (::DLManagedTensor*)::PyCapsule_GetPointer(obj.ptr(), "dltensor"); + ::DLManagedTensor *managed_tensor = + (::DLManagedTensor *)::PyCapsule_GetPointer(obj.ptr(), "dltensor"); utility::FromDLPack(managed_tensor, dst); } \ No newline at end of file diff --git a/src/python/cupoch_pybind/dl_converter.h b/src/python/cupoch_pybind/dl_converter.h index b3ebd352..8e7af1b0 100644 --- a/src/python/cupoch_pybind/dl_converter.h +++ b/src/python/cupoch_pybind/dl_converter.h @@ -1,8 +1,10 @@ #pragma once #include +#include + #include + #include "cupoch/utility/device_vector.h" -#include namespace py = pybind11; namespace cupoch { @@ -10,7 +12,8 @@ namespace dlpack { py::capsule ToDLpackCapsule(utility::device_vector& src); -void FromDLpackCapsule(py::capsule dlpack, utility::device_vector& dst); +void FromDLpackCapsule(py::capsule dlpack, + utility::device_vector& dst); -} -} \ No newline at end of file +} // namespace dlpack +} // namespace cupoch \ No newline at end of file diff --git a/src/python/cupoch_pybind/docstring.cpp b/src/python/cupoch_pybind/docstring.cpp index 6f436ee7..b43694a5 100644 --- a/src/python/cupoch_pybind/docstring.cpp +++ b/src/python/cupoch_pybind/docstring.cpp @@ -1,3 +1,5 @@ +#include "cupoch_pybind/docstring.h" + #include #include #include @@ -7,7 +9,6 @@ #include "cupoch/utility/console.h" #include "cupoch/utility/helper.h" -#include "cupoch_pybind/docstring.h" namespace cupoch { namespace docstring { diff --git a/src/python/cupoch_pybind/docstring.h b/src/python/cupoch_pybind/docstring.h index d44876c3..0691d452 100644 --- a/src/python/cupoch_pybind/docstring.h +++ b/src/python/cupoch_pybind/docstring.h @@ -1,9 +1,9 @@ #pragma once +#include + #include #include - -#include namespace py = pybind11; namespace cupoch { diff --git a/src/tests/geometry/image.cpp b/src/tests/geometry/image.cpp index d8b5a7b2..cd3efb54 100644 --- a/src/tests/geometry/image.cpp +++ b/src/tests/geometry/image.cpp @@ -1,4 +1,5 @@ #include "cupoch/geometry/image.h" + #include "cupoch/camera/pinhole_camera_intrinsic.h" #include "tests/test_utility/unit_test.h" @@ -386,7 +387,8 @@ TEST(Image, TransposeFloat) { image.Prepare(width, height, num_of_channels, bytes_per_channel); const uint8_t* input_uint8_ptr = reinterpret_cast(input.data()); - thrust::host_vector input_uint8(input_uint8_ptr, input_uint8_ptr + image.data_.size()); + thrust::host_vector input_uint8( + input_uint8_ptr, input_uint8_ptr + image.data_.size()); image.SetData(input_uint8); auto transposed_image = image.Transpose(); @@ -396,7 +398,8 @@ TEST(Image, TransposeFloat) { EXPECT_EQ(num_of_channels, transposed_image->num_of_channels_); EXPECT_EQ(int(sizeof(float)), transposed_image->bytes_per_channel_); - thrust::host_vector transposed_host_data = transposed_image->GetData(); + thrust::host_vector transposed_host_data = + transposed_image->GetData(); const float* transpose_image_floats = reinterpret_cast(transposed_host_data.data()); thrust::host_vector transpose_image_data( diff --git a/src/tests/geometry/kdtree_flann.cpp b/src/tests/geometry/kdtree_flann.cpp index 8deda6f9..6e84ffd3 100644 --- a/src/tests/geometry/kdtree_flann.cpp +++ b/src/tests/geometry/kdtree_flann.cpp @@ -1,8 +1,10 @@ #include "cupoch/geometry/kdtree_flann.h" + +#include +#include + #include "cupoch/geometry/pointcloud.h" #include "tests/test_utility/unit_test.h" -#include -#include using namespace Eigen; using namespace cupoch; @@ -12,18 +14,14 @@ using namespace unit_test; namespace { struct is_minus_one_functor { - bool operator() (int x) const { - return (x == -1); - } + bool operator()(int x) const { return (x == -1); } }; struct is_inf_functor { - bool operator() (float x) const { - return std::isinf(x); - } + bool operator()(float x) const { return std::isinf(x); } }; -} +} // namespace TEST(KDTreeFlann, SearchKNN) { thrust::host_vector ref_indices; @@ -33,7 +31,8 @@ TEST(KDTreeFlann, SearchKNN) { for (int i = 0; i < 30; ++i) ref_indices.push_back(indices0[i]); thrust::host_vector ref_distance2; - float distances0[] = {0.000000, 4.684353, 4.996539, 9.191849, 10.034604, 10.466745, + float distances0[] = { + 0.000000, 4.684353, 4.996539, 9.191849, 10.034604, 10.466745, 10.649751, 11.434066, 12.089195, 13.345638, 13.696270, 14.016148, 16.851978, 17.073435, 18.254518, 20.019994, 21.496347, 23.077277, 23.692427, 23.809303, 24.104578, 25.005770, 26.952710, 27.487888, @@ -77,11 +76,11 @@ TEST(KDTreeFlann, SearchRadius) { for (int i = 0; i < 21; ++i) ref_indices.push_back(indices0[i]); thrust::host_vector ref_distance2; - float distances0[] = { - 0.000000, 4.684353, 4.996539, 9.191849, 10.034604, 10.466745, - 10.649751, 11.434066, 12.089195, 13.345638, 13.696270, 14.016148, - 16.851978, 17.073435, 18.254518, 20.019994, 21.496347, 23.077277, - 23.692427, 23.809303, 24.104578}; + float distances0[] = {0.000000, 4.684353, 4.996539, 9.191849, 10.034604, + 10.466745, 10.649751, 11.434066, 12.089195, 13.345638, + 13.696270, 14.016148, 16.851978, 17.073435, 18.254518, + 20.019994, 21.496347, 23.077277, 23.692427, 23.809303, + 24.104578}; for (int i = 0; i < 21; ++i) ref_distance2.push_back(distances0[i]); int size = 100; @@ -121,8 +120,7 @@ TEST(KDTreeFlann, SearchRadius) { TEST(KDTreeFlann, SearchHybrid) { thrust::host_vector ref_indices; - int indices0[] = {27, 48, 4, 77, 90, 7, 54, 17, - 76, 38, 39, 60, 15, 84, 11}; + int indices0[] = {27, 48, 4, 77, 90, 7, 54, 17, 76, 38, 39, 60, 15, 84, 11}; for (int i = 0; i < 15; ++i) ref_indices.push_back(indices0[i]); thrust::host_vector ref_distance2; @@ -152,7 +150,7 @@ TEST(KDTreeFlann, SearchHybrid) { thrust::host_vector distance2; int result = kdtree.SearchHybrid(query, radius, max_nn, indices, - distance2); + distance2); EXPECT_EQ(result, 15); diff --git a/src/tests/geometry/lineset.cpp b/src/tests/geometry/lineset.cpp index f0e19405..c3134be2 100644 --- a/src/tests/geometry/lineset.cpp +++ b/src/tests/geometry/lineset.cpp @@ -1,4 +1,5 @@ #include "cupoch/geometry/lineset.h" + #include "cupoch/geometry/pointcloud.h" #include "tests/test_utility/raw.h" #include "tests/test_utility/unit_test.h" diff --git a/src/tests/geometry/pointcloud.cpp b/src/tests/geometry/pointcloud.cpp index 1c1f3ae0..ccbf8b7a 100644 --- a/src/tests/geometry/pointcloud.cpp +++ b/src/tests/geometry/pointcloud.cpp @@ -1,8 +1,10 @@ -#include #include "cupoch/geometry/pointcloud.h" + +#include +#include + #include "cupoch/geometry/boundingvolume.h" #include "tests/test_utility/unit_test.h" -#include using namespace Eigen; using namespace cupoch; @@ -461,7 +463,8 @@ TEST(PointCloud, CropPointCloud) { Vector3f minBound(200.0, 200.0, 200.0); Vector3f maxBound(800.0, 800.0, 800.0); - auto output_pc = pc.Crop(geometry::AxisAlignedBoundingBox(minBound, maxBound)); + auto output_pc = + pc.Crop(geometry::AxisAlignedBoundingBox(minBound, maxBound)); ExpectLE(minBound, output_pc->GetPoints()); ExpectGE(maxBound, output_pc->GetPoints()); diff --git a/src/tests/geometry/voxelgrid.cpp b/src/tests/geometry/voxelgrid.cpp index 8097cb43..c62aa7ee 100644 --- a/src/tests/geometry/voxelgrid.cpp +++ b/src/tests/geometry/voxelgrid.cpp @@ -1,4 +1,5 @@ #include "cupoch/geometry/voxelgrid.h" + #include "cupoch/geometry/lineset.h" #include "cupoch/geometry/trianglemesh.h" #include "cupoch/visualization/utility/draw_geometry.h" diff --git a/src/tests/odometry/odometry_tools.cpp b/src/tests/odometry/odometry_tools.cpp index f0d52a99..bfb73d16 100644 --- a/src/tests/odometry/odometry_tools.cpp +++ b/src/tests/odometry/odometry_tools.cpp @@ -1,4 +1,5 @@ #include "tests/odometry/odometry_tools.h" + #include using namespace cupoch; diff --git a/src/tests/odometry/rgbdodometry_jacobian_from_color_term.cpp b/src/tests/odometry/rgbdodometry_jacobian_from_color_term.cpp index 8c0daf7c..88432233 100644 --- a/src/tests/odometry/rgbdodometry_jacobian_from_color_term.cpp +++ b/src/tests/odometry/rgbdodometry_jacobian_from_color_term.cpp @@ -1,7 +1,7 @@ #include "cupoch/geometry/rgbdimage.h" #include "cupoch/odometry/rgbdodometry_jacobian.h" -#include "tests/test_utility/unit_test.h" #include "tests/odometry/odometry_tools.h" +#include "tests/test_utility/unit_test.h" using namespace Eigen; using namespace odometry_tools; @@ -21,9 +21,8 @@ TEST(RGBDOdometryJacobianFromColorTerm, ComputeJacobianAndResidual) { ref_J_r[8] << -2.080471, 1.779082, 0.191770, 0.116250, 0.373750, -2.206175; ref_J_r[9] << -0.015476, 0.054573, -0.002288, 0.027828, 0.005931, -0.046776; - float ref_r_raw[] = {0.419608, -0.360784, 0.274510, 0.564706, - 0.835294, -0.352941, -0.545098, -0.360784, - 0.121569, -0.094118}; + float ref_r_raw[] = {0.419608, -0.360784, 0.274510, 0.564706, 0.835294, + -0.352941, -0.545098, -0.360784, 0.121569, -0.094118}; thrust::host_vector ref_r; for (int i = 0; i < 10; ++i) ref_r.push_back(ref_r_raw[i]); @@ -85,8 +84,7 @@ TEST(RGBDOdometryJacobianFromColorTerm, ComputeJacobianAndResidual) { thrust::raw_pointer_cast(target_dx.depth_.GetData().data()), thrust::raw_pointer_cast(target_dy.color_.GetData().data()), thrust::raw_pointer_cast(target_dy.depth_.GetData().data()), - width, - intrinsic, extrinsic, + width, intrinsic, extrinsic, thrust::raw_pointer_cast(corresps.data())); EXPECT_NEAR(ref_r[row], r[0], THRESHOLD_1E_4); diff --git a/src/tests/odometry/rgbdodometry_jacobian_from_hybrid_term.cpp b/src/tests/odometry/rgbdodometry_jacobian_from_hybrid_term.cpp index cda114c0..dc4f647f 100644 --- a/src/tests/odometry/rgbdodometry_jacobian_from_hybrid_term.cpp +++ b/src/tests/odometry/rgbdodometry_jacobian_from_hybrid_term.cpp @@ -1,7 +1,7 @@ #include "cupoch/geometry/rgbdimage.h" #include "cupoch/odometry/rgbdodometry_jacobian.h" -#include "tests/test_utility/unit_test.h" #include "tests/odometry/odometry_tools.h" +#include "tests/test_utility/unit_test.h" using namespace Eigen; using namespace odometry_tools; @@ -103,8 +103,7 @@ TEST(RGBDOdometryJacobianFromHybridTerm, ComputeJacobianAndResidual) { thrust::raw_pointer_cast(target_dx.depth_.GetData().data()), thrust::raw_pointer_cast(target_dy.color_.GetData().data()), thrust::raw_pointer_cast(target_dy.depth_.GetData().data()), - width, - intrinsic, extrinsic, + width, intrinsic, extrinsic, thrust::raw_pointer_cast(corresps.data())); EXPECT_NEAR(ref_r[2 * row + 0], r[0], THRESHOLD_1E_4); diff --git a/src/tests/registration/kabsch.cpp b/src/tests/registration/kabsch.cpp index 38d6a545..f9866bca 100644 --- a/src/tests/registration/kabsch.cpp +++ b/src/tests/registration/kabsch.cpp @@ -1,4 +1,5 @@ #include "cupoch/registration/kabsch.h" + #include "cupoch/geometry/pointcloud.h" #include "tests/test_utility/unit_test.h" @@ -8,8 +9,8 @@ using namespace std; using namespace unit_test; namespace { - float deg_to_rad(float deg) {return deg / 180.0 * M_PI;} -} +float deg_to_rad(float deg) { return deg / 180.0 * M_PI; } +} // namespace TEST(Kabsch, Kabsch) { const size_t size = 20; @@ -20,10 +21,10 @@ TEST(Kabsch, Kabsch) { geometry::PointCloud source; source.SetPoints(points); const float rad = deg_to_rad(30.0f); - const Matrix4f ref_tf = (Matrix4f() << std::cos(rad), -std::sin(rad), 0.0, 0.0, - std::sin(rad), std::cos(rad), 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0).finished(); + const Matrix4f ref_tf = (Matrix4f() << std::cos(rad), -std::sin(rad), 0.0, + 0.0, std::sin(rad), std::cos(rad), 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0) + .finished(); geometry::PointCloud target = source; target.Transform(ref_tf); diff --git a/src/tests/test_utility/print.h b/src/tests/test_utility/print.h index 46a5e240..795699ba 100644 --- a/src/tests/test_utility/print.h +++ b/src/tests/test_utility/print.h @@ -1,9 +1,10 @@ #pragma once +#include + #include #include #include -#include namespace unit_test { // tab size used for formatting ref data. @@ -69,11 +70,11 @@ void Print(const thrust::host_vector>& v, // Print a vector of Matrix that uses the Eigen::aligned_allocator. template -void Print( - const thrust::host_vector, - Eigen::aligned_allocator>>& v, - const int& tabs = 1, - const char& terminator = ';') { +void Print(const thrust::host_vector< + Eigen::Matrix, + Eigen::aligned_allocator>>& v, + const int& tabs = 1, + const char& terminator = ';') { std::cout << std::setw(tabs * TAB_SIZE) << "{"; std::cout << std::endl; for (size_t i = 0; i < v.size(); i++) { diff --git a/src/tests/test_utility/rand.h b/src/tests/test_utility/rand.h index ec88ece4..ab50ab08 100644 --- a/src/tests/test_utility/rand.h +++ b/src/tests/test_utility/rand.h @@ -1,8 +1,9 @@ #pragma once -#include #include +#include + #include "cupoch/utility/eigen.h" namespace unit_test { diff --git a/src/tests/test_utility/sort.h b/src/tests/test_utility/sort.h index 2cd3a071..7c20ee6c 100644 --- a/src/tests/test_utility/sort.h +++ b/src/tests/test_utility/sort.h @@ -1,8 +1,10 @@ #pragma once +#include + #include + #include "cupoch/utility/eigen.h" -#include namespace unit_test { namespace sort { @@ -13,5 +15,5 @@ bool GE(const Eigen::Vector3f& v0, const Eigen::Vector3f& v1); // method needed because std::sort failed on TravisCI/macOS (works fine on // Linux) void Do(thrust::host_vector& v); -} // namespace Sort +} // namespace sort } // namespace unit_test \ No newline at end of file diff --git a/src/tests/test_utility/unit_test.cpp b/src/tests/test_utility/unit_test.cpp index bf623575..c9aec89d 100644 --- a/src/tests/test_utility/unit_test.cpp +++ b/src/tests/test_utility/unit_test.cpp @@ -1,8 +1,8 @@ +#include "tests/test_utility/unit_test.h" + #include #include -#include "tests/test_utility/unit_test.h" - using namespace thrust; using namespace std; using namespace unit_test; @@ -33,7 +33,8 @@ void unit_test::ExpectEQ(const uint8_t* const v0, // ---------------------------------------------------------------------------- // Test equality of two vectors of uint8_t. // ---------------------------------------------------------------------------- -void unit_test::ExpectEQ(const host_vector& v0, const host_vector& v1) { +void unit_test::ExpectEQ(const host_vector& v0, + const host_vector& v1) { EXPECT_EQ(v0.size(), v1.size()); ExpectEQ(v0.data(), v1.data(), v0.size()); } @@ -50,7 +51,8 @@ void unit_test::ExpectEQ(const int* const v0, // ---------------------------------------------------------------------------- // Test equality of two vectors of int. // ---------------------------------------------------------------------------- -void unit_test::ExpectEQ(const host_vector& v0, const host_vector& v1) { +void unit_test::ExpectEQ(const host_vector& v0, + const host_vector& v1) { EXPECT_EQ(v0.size(), v1.size()); ExpectEQ(v0.data(), v1.data(), v0.size()); } @@ -67,7 +69,8 @@ void unit_test::ExpectEQ(const float* const v0, // ---------------------------------------------------------------------------- // Test equality of two vectors of float. // ---------------------------------------------------------------------------- -void unit_test::ExpectEQ(const host_vector& v0, const host_vector& v1) { +void unit_test::ExpectEQ(const host_vector& v0, + const host_vector& v1) { EXPECT_EQ(v0.size(), v1.size()); ExpectEQ(v0.data(), v1.data(), v0.size()); } diff --git a/src/tests/test_utility/unit_test.h b/src/tests/test_utility/unit_test.h index 9b110f51..0ac20a91 100644 --- a/src/tests/test_utility/unit_test.h +++ b/src/tests/test_utility/unit_test.h @@ -7,9 +7,10 @@ #endif #include -#include #include +#include + #include "tests/test_utility/print.h" #include "tests/test_utility/rand.h" #include "tests/test_utility/sort.h" @@ -100,24 +101,25 @@ void ExpectEQ(const uint8_t* const v0, const size_t& size); // Test equality of two vectors of uint8_t. -void ExpectEQ(const thrust::host_vector& v0, const thrust::host_vector& v1); +void ExpectEQ(const thrust::host_vector& v0, + const thrust::host_vector& v1); // Test equality of two arrays of int. void ExpectEQ(const int* const v0, const int* const v1, const size_t& size); // Test equality of two vectors of int. -void ExpectEQ(const thrust::host_vector& v0, const thrust::host_vector& v1); +void ExpectEQ(const thrust::host_vector& v0, + const thrust::host_vector& v1); // Test equality of two arrays of float. void ExpectEQ(const float* const v0, const float* const v1, const size_t& size); // Test equality of two vectors of float. -void ExpectEQ(const thrust::host_vector& v0, const thrust::host_vector& v1); +void ExpectEQ(const thrust::host_vector& v0, + const thrust::host_vector& v1); // Test equality of two arrays of double. -void ExpectEQ(const float* const v0, - const float* const v1, - const size_t& size); +void ExpectEQ(const float* const v0, const float* const v1, const size_t& size); // Reinterpret cast from uint8_t* to float*. template