Open3D (C++ API)  0.14.1
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-2021 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 
37 
38 namespace open3d {
39 
40 namespace camera {
41 class PinholeCameraIntrinsic;
42 }
43 
44 namespace geometry {
45 
46 class Image;
47 class RGBDImage;
48 class TriangleMesh;
49 class VoxelGrid;
50 
55 class PointCloud : public Geometry3D {
56 public:
62  PointCloud(const std::vector<Eigen::Vector3d> &points)
64  ~PointCloud() override {}
65 
66 public:
67  PointCloud &Clear() override;
68  bool IsEmpty() const override;
69  Eigen::Vector3d GetMinBound() const override;
70  Eigen::Vector3d GetMaxBound() const override;
71  Eigen::Vector3d GetCenter() const override;
74  PointCloud &Transform(const Eigen::Matrix4d &transformation) override;
75  PointCloud &Translate(const Eigen::Vector3d &translation,
76  bool relative = true) override;
77  PointCloud &Scale(const double scale,
78  const Eigen::Vector3d &center) override;
79  PointCloud &Rotate(const Eigen::Matrix3d &R,
80  const Eigen::Vector3d &center) override;
81 
82  PointCloud &operator+=(const PointCloud &cloud);
83  PointCloud operator+(const PointCloud &cloud) const;
84 
86  bool HasPoints() const { return points_.size() > 0; }
87 
89  bool HasNormals() const {
90  return points_.size() > 0 && normals_.size() == points_.size();
91  }
92 
94  bool HasColors() const {
95  return points_.size() > 0 && colors_.size() == points_.size();
96  }
97 
99  bool HasCovariances() const {
100  return !points_.empty() && covariances_.size() == points_.size();
101  }
102 
105  for (size_t i = 0; i < normals_.size(); i++) {
106  normals_[i].normalize();
107  }
108  return *this;
109  }
110 
114  PointCloud &PaintUniformColor(const Eigen::Vector3d &color) {
116  return *this;
117  }
118 
126  PointCloud &RemoveNonFinitePoints(bool remove_nan = true,
127  bool remove_infinite = true);
128 
136  std::shared_ptr<PointCloud> SelectByIndex(
137  const std::vector<size_t> &indices, bool invert = false) const;
138 
146  std::shared_ptr<PointCloud> VoxelDownSample(double voxel_size) const;
147 
156  std::tuple<std::shared_ptr<PointCloud>,
157  Eigen::MatrixXi,
158  std::vector<std::vector<int>>>
159  VoxelDownSampleAndTrace(double voxel_size,
160  const Eigen::Vector3d &min_bound,
161  const Eigen::Vector3d &max_bound,
162  bool approximate_class = false) const;
163 
172  std::shared_ptr<PointCloud> UniformDownSample(size_t every_k_points) const;
173 
182  std::shared_ptr<PointCloud> RandomDownSample(double sampling_ratio) const;
189  std::shared_ptr<PointCloud> Crop(const AxisAlignedBoundingBox &bbox) const;
190 
197  std::shared_ptr<PointCloud> Crop(const OrientedBoundingBox &bbox) const;
198 
205  std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
206  RemoveRadiusOutliers(size_t nb_points,
207  double search_radius,
208  bool print_progress = false) const;
209 
215  std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
216  RemoveStatisticalOutliers(size_t nb_neighbors,
217  double std_ratio,
218  bool print_progress = false) const;
219 
230  void EstimateNormals(
231  const KDTreeSearchParam &search_param = KDTreeSearchParamKNN(),
232  bool fast_normal_computation = true);
233 
239  const Eigen::Vector3d &orientation_reference =
240  Eigen::Vector3d(0.0, 0.0, 1.0));
241 
247  const Eigen::Vector3d &camera_location = Eigen::Vector3d::Zero());
248 
256 
264  std::vector<double> ComputePointCloudDistance(const PointCloud &target);
265 
273  static std::vector<Eigen::Matrix3d> EstimatePerPointCovariances(
274  const PointCloud &input,
275  const KDTreeSearchParam &search_param = KDTreeSearchParamKNN());
276 
283  void EstimateCovariances(
284  const KDTreeSearchParam &search_param = KDTreeSearchParamKNN());
285 
288  std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputeMeanAndCovariance()
289  const;
290 
295  std::vector<double> ComputeMahalanobisDistance() const;
296 
299  std::vector<double> ComputeNearestNeighborDistance() const;
300 
302  std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
303  ComputeConvexHull() const;
304 
314  std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
315  HiddenPointRemoval(const Eigen::Vector3d &camera_location,
316  const double radius) const;
317 
329  std::vector<int> ClusterDBSCAN(double eps,
330  size_t min_points,
331  bool print_progress = false) const;
332 
345  std::tuple<Eigen::Vector4d, std::vector<size_t>> SegmentPlane(
346  const double distance_threshold = 0.01,
347  const int ransac_n = 3,
348  const int num_iterations = 100,
350 
370  static std::shared_ptr<PointCloud> CreateFromDepthImage(
371  const Image &depth,
372  const camera::PinholeCameraIntrinsic &intrinsic,
373  const Eigen::Matrix4d &extrinsic = Eigen::Matrix4d::Identity(),
374  double depth_scale = 1000.0,
375  double depth_trunc = 1000.0,
376  int stride = 1,
377  bool project_valid_depth_only = true);
378 
395  static std::shared_ptr<PointCloud> CreateFromRGBDImage(
396  const RGBDImage &image,
397  const camera::PinholeCameraIntrinsic &intrinsic,
398  const Eigen::Matrix4d &extrinsic = Eigen::Matrix4d::Identity(),
399  bool project_valid_depth_only = true);
400 
407  std::shared_ptr<PointCloud> CreateFromVoxelGrid(
408  const VoxelGrid &voxel_grid);
409 
410 public:
412  std::vector<Eigen::Vector3d> points_;
414  std::vector<Eigen::Vector3d> normals_;
416  std::vector<Eigen::Vector3d> colors_;
418  std::vector<Eigen::Matrix3d> covariances_;
419 };
420 
421 } // namespace geometry
422 } // namespace open3d
std::shared_ptr< core::Tensor > image
Definition: FilamentRenderer.cpp:228
math::float4 color
Definition: LineSetBuffers.cpp:64
size_t stride
Definition: TriangleMeshBuffers.cpp:184
Contains the pinhole camera intrinsic parameters.
Definition: PinholeCameraIntrinsic.h:51
A bounding box that is aligned along the coordinate axes.
Definition: BoundingVolume.h:150
The base geometry class for 3D geometries.
Definition: Geometry3D.h:47
void ResizeAndPaintUniformColor(std::vector< Eigen::Vector3d > &colors, const size_t size, const Eigen::Vector3d &color) const
Resizes the colors vector and paints a uniform color.
Definition: Geometry3D.cpp:75
The base geometry class.
Definition: Geometry.h:37
GeometryType
Specifies possible geometry types.
Definition: Geometry.h:42
The Image class stores image with customizable width, height, num of channels and bytes per channel.
Definition: Image.h:53
Base class for KDTree search parameters.
Definition: KDTreeSearchParam.h:35
KDTree search parameters for pure KNN search.
Definition: KDTreeSearchParam.h:63
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:55
std::vector< Eigen::Vector3d > colors_
RGB colors of points.
Definition: PointCloud.h:416
PointCloud & PaintUniformColor(const Eigen::Vector3d &color)
Definition: PointCloud.h:114
std::vector< Eigen::Matrix3d > covariances_
Covariance Matrix for each point.
Definition: PointCloud.h:418
void EstimateNormals(const KDTreeSearchParam &search_param=KDTreeSearchParamKNN(), bool fast_normal_computation=true)
Function to compute the normals of a point cloud.
Definition: EstimateNormals.cpp:307
AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override
Returns an axis-aligned bounding box of the geometry.
Definition: PointCloud.cpp:66
PointCloud & Translate(const Eigen::Vector3d &translation, bool relative=true) override
Apply translation to the geometry coordinates.
Definition: PointCloud.cpp:81
static std::vector< Eigen::Matrix3d > EstimatePerPointCovariances(const PointCloud &input, const KDTreeSearchParam &search_param=KDTreeSearchParamKNN())
Static function to compute the covariance matrix for each point of a point cloud. Doesn't change the ...
Definition: PointCloud.cpp:594
Eigen::Vector3d GetCenter() const override
Returns the center of the geometry coordinates.
Definition: PointCloud.cpp:64
std::vector< double > ComputeMahalanobisDistance() const
Function to compute the Mahalanobis distance for points in an input point cloud.
Definition: PointCloud.cpp:636
std::vector< double > ComputePointCloudDistance(const PointCloud &target)
Function to compute the point to point distances between point clouds.
Definition: PointCloud.cpp:139
std::vector< int > ClusterDBSCAN(double eps, size_t min_points, bool print_progress=false) const
Cluster PointCloud using the DBSCAN algorithm Ester et al., "A Density-Based Algorithm for Discoverin...
Definition: PointCloudCluster.cpp:39
PointCloud operator+(const PointCloud &cloud) const
Definition: PointCloud.cpp:135
std::tuple< std::shared_ptr< TriangleMesh >, std::vector< size_t > > HiddenPointRemoval(const Eigen::Vector3d &camera_location, const double radius) const
This is an implementation of the Hidden Point Removal operator described in Katz et....
Definition: PointCloud.cpp:681
Eigen::Vector3d GetMinBound() const override
Returns min bounds for geometry coordinates.
Definition: PointCloud.cpp:56
Eigen::Vector3d GetMaxBound() const override
Returns max bounds for geometry coordinates.
Definition: PointCloud.cpp:60
PointCloud & Clear() override
Clear all elements in the geometry.
Definition: PointCloud.cpp:46
PointCloud & operator+=(const PointCloud &cloud)
Definition: PointCloud.cpp:101
bool HasNormals() const
Returns true if the point cloud contains point normals.
Definition: PointCloud.h:89
std::vector< double > ComputeNearestNeighborDistance() const
Definition: PointCloud.cpp:651
std::shared_ptr< PointCloud > VoxelDownSample(double voxel_size) const
Function to downsample input pointcloud into output pointcloud with a voxel.
Definition: PointCloud.cpp:328
PointCloud & RemoveNonFinitePoints(bool remove_nan=true, bool remove_infinite=true)
Remove all points from the point cloud that have a nan entry, or infinite entries.
Definition: PointCloud.cpp:161
PointCloud & Scale(const double scale, const Eigen::Vector3d &center) override
Apply scaling to the geometry coordinates. Given a scaling factor , and center , a given point is tr...
Definition: PointCloud.cpp:87
static std::shared_ptr< PointCloud > CreateFromDepthImage(const Image &depth, const camera::PinholeCameraIntrinsic &intrinsic, const Eigen::Matrix4d &extrinsic=Eigen::Matrix4d::Identity(), double depth_scale=1000.0, double depth_trunc=1000.0, int stride=1, bool project_valid_depth_only=true)
Factory function to create a pointcloud from a depth image and a camera model.
Definition: PointCloudFactory.cpp:149
PointCloud(const std::vector< Eigen::Vector3d > &points)
Parameterized Constructor.
Definition: PointCloud.h:62
std::tuple< Eigen::Vector3d, Eigen::Matrix3d > ComputeMeanAndCovariance() const
Definition: PointCloud.cpp:626
std::shared_ptr< PointCloud > UniformDownSample(size_t every_k_points) const
Function to downsample input pointcloud into output pointcloud uniformly.
Definition: PointCloud.cpp:453
OrientedBoundingBox GetOrientedBoundingBox() const override
Returns an oriented bounding box of the geometry.
Definition: PointCloud.cpp:70
bool HasPoints() const
Returns 'true' if the point cloud contains points.
Definition: PointCloud.h:86
std::shared_ptr< PointCloud > SelectByIndex(const std::vector< size_t > &indices, bool invert=false) const
Function to select points from input pointcloud into output pointcloud.
Definition: PointCloud.cpp:193
PointCloud & Rotate(const Eigen::Matrix3d &R, const Eigen::Vector3d &center) override
Apply rotation to the geometry coordinates and normals. Given a rotation matrix , and center ,...
Definition: PointCloud.cpp:93
void OrientNormalsToAlignWithDirection(const Eigen::Vector3d &orientation_reference=Eigen::Vector3d(0.0, 0.0, 1.0))
Function to orient the normals of a point cloud.
Definition: EstimateNormals.cpp:338
std::tuple< std::shared_ptr< PointCloud >, Eigen::MatrixXi, std::vector< std::vector< int > > > VoxelDownSampleAndTrace(double voxel_size, const Eigen::Vector3d &min_bound, const Eigen::Vector3d &max_bound, bool approximate_class=false) const
Function to downsample using geometry.PointCloud.VoxelDownSample.
Definition: PointCloud.cpp:379
~PointCloud() override
Definition: PointCloud.h:64
std::tuple< std::shared_ptr< PointCloud >, std::vector< size_t > > RemoveStatisticalOutliers(size_t nb_neighbors, double std_ratio, bool print_progress=false) const
Function to remove points that are further away from their nb_neighbor neighbors in average.
Definition: PointCloud.cpp:534
PointCloud & Transform(const Eigen::Matrix4d &transformation) override
Apply transformation (4x4 matrix) to the geometry coordinates.
Definition: PointCloud.cpp:74
PointCloud()
Default Constructor.
Definition: PointCloud.h:58
static std::shared_ptr< PointCloud > CreateFromRGBDImage(const RGBDImage &image, const camera::PinholeCameraIntrinsic &intrinsic, const Eigen::Matrix4d &extrinsic=Eigen::Matrix4d::Identity(), bool project_valid_depth_only=true)
Factory function to create a pointcloud from an RGB-D image and a camera model.
Definition: PointCloudFactory.cpp:175
std::shared_ptr< PointCloud > CreateFromVoxelGrid(const VoxelGrid &voxel_grid)
Function to create a PointCloud from a VoxelGrid.
Definition: PointCloudFactory.cpp:201
std::shared_ptr< PointCloud > Crop(const AxisAlignedBoundingBox &bbox) const
Function to crop pointcloud into output pointcloud.
Definition: PointCloud.cpp:481
std::vector< Eigen::Vector3d > normals_
Points normals.
Definition: PointCloud.h:414
void OrientNormalsTowardsCameraLocation(const Eigen::Vector3d &camera_location=Eigen::Vector3d::Zero())
Function to orient the normals of a point cloud.
Definition: EstimateNormals.cpp:358
std::tuple< Eigen::Vector4d, std::vector< size_t > > SegmentPlane(const double distance_threshold=0.01, const int ransac_n=3, const int num_iterations=100, utility::optional< int > seed=utility::nullopt) const
Segment PointCloud plane using the RANSAC algorithm.
Definition: PointCloudSegmentation.cpp:135
std::tuple< std::shared_ptr< TriangleMesh >, std::vector< size_t > > ComputeConvexHull() const
Function that computes the convex hull of the point cloud using qhull.
Definition: PointCloud.cpp:676
std::shared_ptr< PointCloud > RandomDownSample(double sampling_ratio) const
Function to downsample input pointcloud into output pointcloud randomly.
Definition: PointCloud.cpp:465
std::vector< Eigen::Vector3d > points_
Points coordinates.
Definition: PointCloud.h:412
void EstimateCovariances(const KDTreeSearchParam &search_param=KDTreeSearchParamKNN())
Function to compute the covariance matrix for each point of a point cloud.
Definition: PointCloud.cpp:620
void OrientNormalsConsistentTangentPlane(size_t k)
Function to consistently orient estimated normals based on consistent tangent planes as described in ...
Definition: EstimateNormals.cpp:383
bool HasColors() const
Returns true if the point cloud contains point colors.
Definition: PointCloud.h:94
PointCloud & NormalizeNormals()
Normalize point normals to length 1.
Definition: PointCloud.h:104
bool HasCovariances() const
Returns 'true' if the point cloud contains per-point covariance matrix.
Definition: PointCloud.h:99
bool IsEmpty() const override
Returns true iff the geometry is empty.
Definition: PointCloud.cpp:54
std::tuple< std::shared_ptr< PointCloud >, std::vector< size_t > > RemoveRadiusOutliers(size_t nb_points, double search_radius, bool print_progress=false) const
Function to remove points that have less than nb_points in a sphere of a given radius.
Definition: PointCloud.cpp:501
RGBDImage is for a pair of registered color and depth images,.
Definition: RGBDImage.h:46
VoxelGrid is a collection of voxels which are aligned in grid.
Definition: VoxelGrid.h:80
Definition: Optional.h:278
int points
Definition: FilePCD.cpp:73
constexpr nullopt_t nullopt
Definition: Optional.h:171
Definition: PinholeCameraIntrinsic.cpp:35