Open3D (C++ API)  0.16.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
38namespace open3d {
39
40namespace camera {
41class PinholeCameraIntrinsic;
42}
43
44namespace geometry {
45
46class Image;
47class RGBDImage;
48class TriangleMesh;
49class VoxelGrid;
50
55class PointCloud : public Geometry3D {
56public:
62 PointCloud(const std::vector<Eigen::Vector3d> &points)
64 ~PointCloud() override {}
65
66public:
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 bool robust = false) const override;
75 PointCloud &Transform(const Eigen::Matrix4d &transformation) override;
76 PointCloud &Translate(const Eigen::Vector3d &translation,
77 bool relative = true) override;
78 PointCloud &Scale(const double scale,
79 const Eigen::Vector3d &center) override;
80 PointCloud &Rotate(const Eigen::Matrix3d &R,
81 const Eigen::Vector3d &center) override;
82
83 PointCloud &operator+=(const PointCloud &cloud);
84 PointCloud operator+(const PointCloud &cloud) const;
85
87 bool HasPoints() const { return points_.size() > 0; }
88
90 bool HasNormals() const {
91 return points_.size() > 0 && normals_.size() == points_.size();
92 }
93
95 bool HasColors() const {
96 return points_.size() > 0 && colors_.size() == points_.size();
97 }
98
100 bool HasCovariances() const {
101 return !points_.empty() && covariances_.size() == points_.size();
102 }
103
106 for (size_t i = 0; i < normals_.size(); i++) {
107 normals_[i].normalize();
108 }
109 return *this;
110 }
111
115 PointCloud &PaintUniformColor(const Eigen::Vector3d &color) {
117 return *this;
118 }
119
128 PointCloud &RemoveNonFinitePoints(bool remove_nan = true,
129 bool remove_infinite = true);
130
137
143 std::shared_ptr<PointCloud> SelectByIndex(
144 const std::vector<size_t> &indices, bool invert = false) const;
145
151 std::shared_ptr<PointCloud> VoxelDownSample(double voxel_size) const;
152
161 std::tuple<std::shared_ptr<PointCloud>,
162 Eigen::MatrixXi,
163 std::vector<std::vector<int>>>
164 VoxelDownSampleAndTrace(double voxel_size,
165 const Eigen::Vector3d &min_bound,
166 const Eigen::Vector3d &max_bound,
167 bool approximate_class = false) const;
168
177 std::shared_ptr<PointCloud> UniformDownSample(size_t every_k_points) const;
178
187 std::shared_ptr<PointCloud> RandomDownSample(double sampling_ratio) const;
188
196 std::shared_ptr<PointCloud> FarthestPointDownSample(
197 size_t num_samples) const;
198
205 std::shared_ptr<PointCloud> Crop(const AxisAlignedBoundingBox &bbox) const;
206
213 std::shared_ptr<PointCloud> Crop(const OrientedBoundingBox &bbox) const;
214
221 std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
222 RemoveRadiusOutliers(size_t nb_points,
223 double search_radius,
224 bool print_progress = false) const;
225
231 std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
232 RemoveStatisticalOutliers(size_t nb_neighbors,
233 double std_ratio,
234 bool print_progress = false) const;
235
246 void EstimateNormals(
247 const KDTreeSearchParam &search_param = KDTreeSearchParamKNN(),
248 bool fast_normal_computation = true);
249
255 const Eigen::Vector3d &orientation_reference =
256 Eigen::Vector3d(0.0, 0.0, 1.0));
257
263 const Eigen::Vector3d &camera_location = Eigen::Vector3d::Zero());
264
272
280 std::vector<double> ComputePointCloudDistance(const PointCloud &target);
281
289 static std::vector<Eigen::Matrix3d> EstimatePerPointCovariances(
290 const PointCloud &input,
291 const KDTreeSearchParam &search_param = KDTreeSearchParamKNN());
292
300 const KDTreeSearchParam &search_param = KDTreeSearchParamKNN());
301
304 std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputeMeanAndCovariance()
305 const;
306
311 std::vector<double> ComputeMahalanobisDistance() const;
312
315 std::vector<double> ComputeNearestNeighborDistance() const;
316
323 std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
324 ComputeConvexHull(bool joggle_inputs = false) const;
325
335 std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
336 HiddenPointRemoval(const Eigen::Vector3d &camera_location,
337 const double radius) const;
338
350 std::vector<int> ClusterDBSCAN(double eps,
351 size_t min_points,
352 bool print_progress = false) const;
353
364 std::tuple<Eigen::Vector4d, std::vector<size_t>> SegmentPlane(
365 const double distance_threshold = 0.01,
366 const int ransac_n = 3,
367 const int num_iterations = 100,
368 const double probability = 0.99999999) const;
369
389 static std::shared_ptr<PointCloud> CreateFromDepthImage(
390 const Image &depth,
391 const camera::PinholeCameraIntrinsic &intrinsic,
392 const Eigen::Matrix4d &extrinsic = Eigen::Matrix4d::Identity(),
393 double depth_scale = 1000.0,
394 double depth_trunc = 1000.0,
395 int stride = 1,
396 bool project_valid_depth_only = true);
397
414 static std::shared_ptr<PointCloud> CreateFromRGBDImage(
415 const RGBDImage &image,
416 const camera::PinholeCameraIntrinsic &intrinsic,
417 const Eigen::Matrix4d &extrinsic = Eigen::Matrix4d::Identity(),
418 bool project_valid_depth_only = true);
419
426 std::shared_ptr<PointCloud> CreateFromVoxelGrid(
427 const VoxelGrid &voxel_grid);
428
429public:
431 std::vector<Eigen::Vector3d> points_;
433 std::vector<Eigen::Vector3d> normals_;
435 std::vector<Eigen::Vector3d> colors_;
437 std::vector<Eigen::Matrix3d> covariances_;
438};
439
440} // namespace geometry
441} // namespace open3d
std::shared_ptr< core::Tensor > image
Definition: FilamentRenderer.cpp:202
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:155
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::tuple< Eigen::Vector4d, std::vector< size_t > > SegmentPlane(const double distance_threshold=0.01, const int ransac_n=3, const int num_iterations=100, const double probability=0.99999999) const
Segment PointCloud plane using the RANSAC algorithm.
Definition: PointCloudSegmentation.cpp:166
std::vector< Eigen::Vector3d > colors_
RGB colors of points.
Definition: PointCloud.h:435
std::vector< Eigen::Matrix3d > covariances_
Covariance Matrix for each point.
Definition: PointCloud.h:437
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:671
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:713
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 & NormalizeNormals()
Normalize point normals to length 1.
Definition: PointCloud.h:105
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:758
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:90
std::vector< double > ComputeNearestNeighborDistance() const
Definition: PointCloud.cpp:728
std::shared_ptr< PointCloud > VoxelDownSample(double voxel_size) const
Downsample input pointcloud with a voxel, and return a new point-cloud. Normals, covariances and colo...
Definition: PointCloud.cpp:368
PointCloud & RemoveNonFinitePoints(bool remove_nan=true, bool remove_infinite=true)
Removes all points from the point cloud that have a nan entry, or infinite entries....
Definition: PointCloud.cpp:196
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
PointCloud & PaintUniformColor(const Eigen::Vector3d &color)
Definition: PointCloud.h:115
OrientedBoundingBox GetOrientedBoundingBox(bool robust=false) const override
Definition: PointCloud.cpp:70
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< std::shared_ptr< TriangleMesh >, std::vector< size_t > > ComputeConvexHull(bool joggle_inputs=false) const
Definition: PointCloud.cpp:753
std::tuple< Eigen::Vector3d, Eigen::Matrix3d > ComputeMeanAndCovariance() const
Definition: PointCloud.cpp:703
std::shared_ptr< PointCloud > UniformDownSample(size_t every_k_points) const
Function to downsample input pointcloud into output pointcloud uniformly.
Definition: PointCloud.cpp:493
bool HasPoints() const
Returns 'true' if the point cloud contains points.
Definition: PointCloud.h:87
std::shared_ptr< PointCloud > SelectByIndex(const std::vector< size_t > &indices, bool invert=false) const
Selects points from input pointcloud, with indices in indices, and returns a new point-cloud with sel...
Definition: PointCloud.cpp:231
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:419
~PointCloud() override
Definition: PointCloud.h:64
PointCloud & RemoveDuplicatedPoints()
Removes duplicated points, i.e., points that have identical coordinates. It also removes the correspo...
Definition: PointCloud.cpp:161
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:611
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:174
std::shared_ptr< PointCloud > CreateFromVoxelGrid(const VoxelGrid &voxel_grid)
Function to create a PointCloud from a VoxelGrid.
Definition: PointCloudFactory.cpp:199
std::shared_ptr< PointCloud > Crop(const AxisAlignedBoundingBox &bbox) const
Function to crop pointcloud into output pointcloud.
Definition: PointCloud.cpp:558
std::vector< Eigen::Vector3d > normals_
Points normals.
Definition: PointCloud.h:433
void OrientNormalsTowardsCameraLocation(const Eigen::Vector3d &camera_location=Eigen::Vector3d::Zero())
Function to orient the normals of a point cloud.
Definition: EstimateNormals.cpp:357
std::shared_ptr< PointCloud > RandomDownSample(double sampling_ratio) const
Function to downsample input pointcloud into output pointcloud randomly.
Definition: PointCloud.cpp:505
std::vector< Eigen::Vector3d > points_
Points coordinates.
Definition: PointCloud.h:431
void EstimateCovariances(const KDTreeSearchParam &search_param=KDTreeSearchParamKNN())
Function to compute the covariance matrix for each point of a point cloud.
Definition: PointCloud.cpp:697
void OrientNormalsConsistentTangentPlane(size_t k)
Function to consistently orient estimated normals based on consistent tangent planes as described in ...
Definition: EstimateNormals.cpp:381
bool HasColors() const
Returns true if the point cloud contains point colors.
Definition: PointCloud.h:95
bool HasCovariances() const
Returns 'true' if the point cloud contains per-point covariance matrix.
Definition: PointCloud.h:100
bool IsEmpty() const override
Returns true iff the geometry is empty.
Definition: PointCloud.cpp:54
std::shared_ptr< PointCloud > FarthestPointDownSample(size_t num_samples) const
Function to downsample input pointcloud into output pointcloud with a set of points has farthest dist...
Definition: PointCloud.cpp:523
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:578
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
int points
Definition: FilePCD.cpp:73
Definition: PinholeCameraIntrinsic.cpp:35