Open3D (C++ API)  0.18.0+252c867
Geometry3D.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // Copyright (c) 2018-2023 www.open3d.org
5 // SPDX-License-Identifier: MIT
6 // ----------------------------------------------------------------------------
7 
8 #pragma once
9 
10 #include <Eigen/Core>
11 #include <Eigen/Geometry>
12 #include <vector>
13 
15 #include "open3d/utility/Eigen.h"
16 
17 namespace open3d {
18 namespace geometry {
19 
20 class AxisAlignedBoundingBox;
21 class OrientedBoundingBox;
22 
28 class Geometry3D : public Geometry {
29 public:
30  ~Geometry3D() override {}
31 
32 protected:
37 
38 public:
39  Geometry3D& Clear() override = 0;
40  bool IsEmpty() const override = 0;
42  virtual Eigen::Vector3d GetMinBound() const = 0;
44  virtual Eigen::Vector3d GetMaxBound() const = 0;
46  virtual Eigen::Vector3d GetCenter() const = 0;
47 
51 
58  bool robust = false) const = 0;
59 
67  bool robust = false) const = 0;
68 
70  virtual Geometry3D& Transform(const Eigen::Matrix4d& transformation) = 0;
71 
77  virtual Geometry3D& Translate(const Eigen::Vector3d& translation,
78  bool relative = true) = 0;
86  virtual Geometry3D& Scale(const double scale,
87  const Eigen::Vector3d& center) = 0;
88 
95  virtual Geometry3D& Rotate(const Eigen::Matrix3d& R,
96  const Eigen::Vector3d& center) = 0;
97 
98  virtual Geometry3D& Rotate(const Eigen::Matrix3d& R);
99 
101  static Eigen::Matrix3d GetRotationMatrixFromXYZ(
102  const Eigen::Vector3d& rotation);
104  static Eigen::Matrix3d GetRotationMatrixFromYZX(
105  const Eigen::Vector3d& rotation);
107  static Eigen::Matrix3d GetRotationMatrixFromZXY(
108  const Eigen::Vector3d& rotation);
110  static Eigen::Matrix3d GetRotationMatrixFromXZY(
111  const Eigen::Vector3d& rotation);
113  static Eigen::Matrix3d GetRotationMatrixFromZYX(
114  const Eigen::Vector3d& rotation);
116  static Eigen::Matrix3d GetRotationMatrixFromYXZ(
117  const Eigen::Vector3d& rotation);
119  static Eigen::Matrix3d GetRotationMatrixFromAxisAngle(
120  const Eigen::Vector3d& rotation);
122  static Eigen::Matrix3d GetRotationMatrixFromQuaternion(
123  const Eigen::Vector4d& rotation);
124 
125 protected:
127  Eigen::Vector3d ComputeMinBound(
128  const std::vector<Eigen::Vector3d>& points) const;
130  Eigen::Vector3d ComputeMaxBound(
131  const std::vector<Eigen::Vector3d>& points) const;
133  Eigen::Vector3d ComputeCenter(
134  const std::vector<Eigen::Vector3d>& points) const;
135 
141  void ResizeAndPaintUniformColor(std::vector<Eigen::Vector3d>& colors,
142  const size_t size,
143  const Eigen::Vector3d& color) const;
144 
149  void TransformPoints(const Eigen::Matrix4d& transformation,
150  std::vector<Eigen::Vector3d>& points) const;
151 
156  void TransformNormals(const Eigen::Matrix4d& transformation,
157  std::vector<Eigen::Vector3d>& normals) const;
158 
163  void TransformCovariances(const Eigen::Matrix4d& transformation,
164  std::vector<Eigen::Matrix3d>& covariances) const;
165 
173  void TranslatePoints(const Eigen::Vector3d& translation,
174  std::vector<Eigen::Vector3d>& points,
175  bool relative) const;
176 
183  void ScalePoints(const double scale,
184  std::vector<Eigen::Vector3d>& points,
185  const Eigen::Vector3d& center) const;
186 
193  void RotatePoints(const Eigen::Matrix3d& R,
194  std::vector<Eigen::Vector3d>& points,
195  const Eigen::Vector3d& center) const;
196 
201  void RotateNormals(const Eigen::Matrix3d& R,
202  std::vector<Eigen::Vector3d>& normals) const;
203 
208  void RotateCovariances(const Eigen::Matrix3d& R,
209  std::vector<Eigen::Matrix3d>& covariances) const;
210 };
211 
212 } // namespace geometry
213 } // namespace open3d
math::float4 color
Definition: LineSetBuffers.cpp:45
A bounding box that is aligned along the coordinate axes and defined by the min_bound and max_bound.
Definition: BoundingVolume.h:160
The base geometry class for 3D geometries.
Definition: Geometry3D.h:28
void TransformPoints(const Eigen::Matrix4d &transformation, std::vector< Eigen::Vector3d > &points) const
Transforms all points with the transformation matrix.
Definition: Geometry3D.cpp:77
virtual Geometry3D & Translate(const Eigen::Vector3d &translation, bool relative=true)=0
Apply translation to the geometry coordinates.
static Eigen::Matrix3d GetRotationMatrixFromYXZ(const Eigen::Vector3d &rotation)
Get Rotation Matrix from YXZ RotationType.
Definition: Geometry3D.cpp:184
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:56
void ScalePoints(const double scale, std::vector< Eigen::Vector3d > &points, const Eigen::Vector3d &center) const
Scale the coordinates of all points by the scaling factor scale.
Definition: Geometry3D.cpp:115
virtual Eigen::Vector3d GetCenter() const =0
Returns the center of the geometry coordinates.
virtual OrientedBoundingBox GetMinimalOrientedBoundingBox(bool robust=false) const =0
~Geometry3D() override
Definition: Geometry3D.h:30
void TranslatePoints(const Eigen::Vector3d &translation, std::vector< Eigen::Vector3d > &points, bool relative) const
Apply translation to the geometry coordinates.
Definition: Geometry3D.cpp:103
Eigen::Vector3d ComputeMaxBound(const std::vector< Eigen::Vector3d > &points) const
Compute max bound of a list points.
Definition: Geometry3D.cpp:34
virtual Eigen::Vector3d GetMaxBound() const =0
Returns max bounds for geometry coordinates.
virtual Geometry3D & Scale(const double scale, const Eigen::Vector3d &center)=0
Apply scaling to the geometry coordinates. Given a scaling factor , and center , a given point is tr...
virtual Eigen::Vector3d GetMinBound() const =0
Returns min bounds for geometry coordinates.
bool IsEmpty() const override=0
Returns true iff the geometry is empty.
Geometry3D & Clear() override=0
Clear all elements in the geometry.
void TransformCovariances(const Eigen::Matrix4d &transformation, std::vector< Eigen::Matrix3d > &covariances) const
Transforms all covariance matrices with the transformation.
Definition: Geometry3D.cpp:97
virtual AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const =0
static Eigen::Matrix3d GetRotationMatrixFromXZY(const Eigen::Vector3d &rotation)
Get Rotation Matrix from XZY RotationType.
Definition: Geometry3D.cpp:170
void RotatePoints(const Eigen::Matrix3d &R, std::vector< Eigen::Vector3d > &points, const Eigen::Vector3d &center) const
Rotate all points with the rotation matrix R.
Definition: Geometry3D.cpp:123
void RotateCovariances(const Eigen::Matrix3d &R, std::vector< Eigen::Matrix3d > &covariances) const
Rotate all covariance matrices with the rotation matrix R.
Definition: Geometry3D.cpp:141
Eigen::Vector3d ComputeMinBound(const std::vector< Eigen::Vector3d > &points) const
Compute min bound of a list points.
Definition: Geometry3D.cpp:22
void RotateNormals(const Eigen::Matrix3d &R, std::vector< Eigen::Vector3d > &normals) const
Rotate all normals with the rotation matrix R.
Definition: Geometry3D.cpp:131
virtual OrientedBoundingBox GetOrientedBoundingBox(bool robust=false) const =0
static Eigen::Matrix3d GetRotationMatrixFromAxisAngle(const Eigen::Vector3d &rotation)
Get Rotation Matrix from AxisAngle RotationType.
Definition: Geometry3D.cpp:191
virtual Geometry3D & Rotate(const Eigen::Matrix3d &R, const Eigen::Vector3d &center)=0
Apply rotation to the geometry coordinates and normals. Given a rotation matrix , and center ,...
static Eigen::Matrix3d GetRotationMatrixFromXYZ(const Eigen::Vector3d &rotation)
Get Rotation Matrix from XYZ RotationType.
Definition: Geometry3D.cpp:149
Geometry3D(GeometryType type)
Parameterized Constructor.
Definition: Geometry3D.h:36
virtual Geometry3D & Transform(const Eigen::Matrix4d &transformation)=0
Apply transformation (4x4 matrix) to the geometry coordinates.
void TransformNormals(const Eigen::Matrix4d &transformation, std::vector< Eigen::Vector3d > &normals) const
Transforms the normals with the transformation matrix.
Definition: Geometry3D.cpp:87
Eigen::Vector3d ComputeCenter(const std::vector< Eigen::Vector3d > &points) const
Computer center of a list of points.
Definition: Geometry3D.cpp:45
static Eigen::Matrix3d GetRotationMatrixFromZYX(const Eigen::Vector3d &rotation)
Get Rotation Matrix from ZYX RotationType.
Definition: Geometry3D.cpp:177
static Eigen::Matrix3d GetRotationMatrixFromQuaternion(const Eigen::Vector4d &rotation)
Get Rotation Matrix from Quaternion.
Definition: Geometry3D.cpp:200
static Eigen::Matrix3d GetRotationMatrixFromYZX(const Eigen::Vector3d &rotation)
Get Rotation Matrix from YZX RotationType.
Definition: Geometry3D.cpp:156
static Eigen::Matrix3d GetRotationMatrixFromZXY(const Eigen::Vector3d &rotation)
Get Rotation Matrix from ZXY RotationType.
Definition: Geometry3D.cpp:163
The base geometry class.
Definition: Geometry.h:18
GeometryType
Specifies possible geometry types.
Definition: Geometry.h:23
A bounding box oriented along an arbitrary frame of reference.
Definition: BoundingVolume.h:25
int size
Definition: FilePCD.cpp:40
int points
Definition: FilePCD.cpp:54
char type
Definition: FilePCD.cpp:41
Definition: PinholeCameraIntrinsic.cpp:16