Open3D (C++ API)  0.12.0
PointCloud.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2018 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26 
27 #pragma once
28 
29 #include <Eigen/Core>
30 #include <memory>
31 #include <tuple>
32 #include <vector>
33 
36 
37 namespace open3d {
38 
39 namespace camera {
40 class PinholeCameraIntrinsic;
41 }
42 
43 namespace geometry {
44 
45 class Image;
46 class RGBDImage;
47 class TriangleMesh;
48 class VoxelGrid;
49 
54 class PointCloud : public Geometry3D {
55 public:
61  PointCloud(const std::vector<Eigen::Vector3d> &points)
62  : Geometry3D(Geometry::GeometryType::PointCloud), points_(points) {}
63  ~PointCloud() override {}
64 
65 public:
66  PointCloud &Clear() override;
67  bool IsEmpty() const override;
68  Eigen::Vector3d GetMinBound() const override;
69  Eigen::Vector3d GetMaxBound() const override;
70  Eigen::Vector3d GetCenter() const override;
71  AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override;
72  OrientedBoundingBox GetOrientedBoundingBox() const override;
73  PointCloud &Transform(const Eigen::Matrix4d &transformation) override;
74  PointCloud &Translate(const Eigen::Vector3d &translation,
75  bool relative = true) override;
76  PointCloud &Scale(const double scale,
77  const Eigen::Vector3d &center) override;
78  PointCloud &Rotate(const Eigen::Matrix3d &R,
79  const Eigen::Vector3d &center) override;
80 
81  PointCloud &operator+=(const PointCloud &cloud);
82  PointCloud operator+(const PointCloud &cloud) const;
83 
85  bool HasPoints() const { return points_.size() > 0; }
86 
88  bool HasNormals() const {
89  return points_.size() > 0 && normals_.size() == points_.size();
90  }
91 
93  bool HasColors() const {
94  return points_.size() > 0 && colors_.size() == points_.size();
95  }
96 
99  for (size_t i = 0; i < normals_.size(); i++) {
100  normals_[i].normalize();
101  }
102  return *this;
103  }
104 
108  PointCloud &PaintUniformColor(const Eigen::Vector3d &color) {
109  ResizeAndPaintUniformColor(colors_, points_.size(), color);
110  return *this;
111  }
112 
120  PointCloud &RemoveNonFinitePoints(bool remove_nan = true,
121  bool remove_infinite = true);
122 
130  std::shared_ptr<PointCloud> SelectByIndex(
131  const std::vector<size_t> &indices, bool invert = false) const;
132 
140  std::shared_ptr<PointCloud> VoxelDownSample(double voxel_size) const;
141 
150  std::tuple<std::shared_ptr<PointCloud>,
151  Eigen::MatrixXi,
152  std::vector<std::vector<int>>>
153  VoxelDownSampleAndTrace(double voxel_size,
154  const Eigen::Vector3d &min_bound,
155  const Eigen::Vector3d &max_bound,
156  bool approximate_class = false) const;
157 
166  std::shared_ptr<PointCloud> UniformDownSample(size_t every_k_points) const;
167 
174  std::shared_ptr<PointCloud> Crop(const AxisAlignedBoundingBox &bbox) const;
175 
182  std::shared_ptr<PointCloud> Crop(const OrientedBoundingBox &bbox) const;
183 
189  std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
190  RemoveRadiusOutliers(size_t nb_points, double search_radius) const;
191 
197  std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
198  RemoveStatisticalOutliers(size_t nb_neighbors, double std_ratio) const;
199 
209  void EstimateNormals(
210  const KDTreeSearchParam &search_param = KDTreeSearchParamKNN(),
211  bool fast_normal_computation = true);
212 
217  void OrientNormalsToAlignWithDirection(
218  const Eigen::Vector3d &orientation_reference =
219  Eigen::Vector3d(0.0, 0.0, 1.0));
220 
225  void OrientNormalsTowardsCameraLocation(
226  const Eigen::Vector3d &camera_location = Eigen::Vector3d::Zero());
227 
234  void OrientNormalsConsistentTangentPlane(size_t k);
235 
243  std::vector<double> ComputePointCloudDistance(const PointCloud &target);
244 
247  std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputeMeanAndCovariance()
248  const;
249 
254  std::vector<double> ComputeMahalanobisDistance() const;
255 
258  std::vector<double> ComputeNearestNeighborDistance() const;
259 
261  std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
262  ComputeConvexHull() const;
263 
273  std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
274  HiddenPointRemoval(const Eigen::Vector3d &camera_location,
275  const double radius) const;
276 
288  std::vector<int> ClusterDBSCAN(double eps,
289  size_t min_points,
290  bool print_progress = false) const;
291 
301  std::tuple<Eigen::Vector4d, std::vector<size_t>> SegmentPlane(
302  const double distance_threshold = 0.01,
303  const int ransac_n = 3,
304  const int num_iterations = 100) const;
305 
325  static std::shared_ptr<PointCloud> CreateFromDepthImage(
326  const Image &depth,
327  const camera::PinholeCameraIntrinsic &intrinsic,
328  const Eigen::Matrix4d &extrinsic = Eigen::Matrix4d::Identity(),
329  double depth_scale = 1000.0,
330  double depth_trunc = 1000.0,
331  int stride = 1,
332  bool project_valid_depth_only = true);
333 
350  static std::shared_ptr<PointCloud> CreateFromRGBDImage(
351  const RGBDImage &image,
352  const camera::PinholeCameraIntrinsic &intrinsic,
353  const Eigen::Matrix4d &extrinsic = Eigen::Matrix4d::Identity(),
354  bool project_valid_depth_only = true);
355 
362  std::shared_ptr<PointCloud> CreateFromVoxelGrid(
363  const VoxelGrid &voxel_grid);
364 
365 public:
367  std::vector<Eigen::Vector3d> points_;
369  std::vector<Eigen::Vector3d> normals_;
371  std::vector<Eigen::Vector3d> colors_;
372 };
373 
374 } // namespace geometry
375 } // namespace open3d
std::tuple< Eigen::Vector3d, Eigen::Matrix3d > ComputeMeanAndCovariance(const std::vector< Eigen::Vector3d > &points, const std::vector< IdxType > &indices)
Function to compute the mean and covariance matrix of a set of points.
Definition: Eigen.cpp:335
The base geometry class.
Definition: Geometry.h:37
A bounding box that is aligned along the coordinate axes.
Definition: BoundingVolume.h:149
Contains the pinhole camera intrinsic parameters.
Definition: PinholeCameraIntrinsic.h:51
A bounding box oriented along an arbitrary frame of reference.
Definition: BoundingVolume.h:44
A point cloud consists of point coordinates, and optionally point colors and point normals...
Definition: PointCloud.h:54
bool HasPoints() const
Returns &#39;true&#39; if the point cloud contains points.
Definition: PointCloud.h:85
PointCloud & NormalizeNormals()
Normalize point normals to length 1.
Definition: PointCloud.h:98
std::vector< Eigen::Vector3d > points_
Points coordinates.
Definition: PointCloud.h:367
PointCloud()
Default Constructor.
Definition: PointCloud.h:57
math::float4 color
Definition: LineSetBuffers.cpp:64
PointCloud & PaintUniformColor(const Eigen::Vector3d &color)
Definition: PointCloud.h:108
std::vector< Eigen::Vector3d > colors_
RGB colors of points.
Definition: PointCloud.h:371
The base geometry class for 3D geometries.
Definition: Geometry3D.h:46
Base class for KDTree search parameters.
Definition: KDTreeSearchParam.h:35
RGBDImage is for a pair of registered color and depth images,.
Definition: RGBDImage.h:46
size_t stride
Definition: TriangleMeshBuffers.cpp:183
std::vector< Eigen::Vector3d > normals_
Points normals.
Definition: PointCloud.h:369
bool HasNormals() const
Returns true if the point cloud contains point normals.
Definition: PointCloud.h:88
int points
Definition: FilePCD.cpp:73
Definition: PinholeCameraIntrinsic.cpp:35
~PointCloud() override
Definition: PointCloud.h:63
VoxelGrid is a collection of voxels which are aligned in grid.
Definition: VoxelGrid.h:80
GeometryType
Specifies possible geometry types.
Definition: Geometry.h:42
Tensor operator+(T scalar_lhs, const Tensor &rhs)
Definition: Tensor.h:1137
bool HasColors() const
Returns true if the point cloud contains point colors.
Definition: PointCloud.h:93
KDTree search parameters for pure KNN search.
Definition: KDTreeSearchParam.h:63
PointCloud(const std::vector< Eigen::Vector3d > &points)
Parameterized Constructor.
Definition: PointCloud.h:61
The Image class stores image with customizable width, height, num of channels and bytes per channel...
Definition: Image.h:53