Open3D (C++ API)  0.12.0
Eigen.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 <Eigen/StdVector>
31 #include <tuple>
32 #include <vector>
33 
35 namespace Eigen {
36 
38 typedef Eigen::Matrix<double, 6, 6> Matrix6d;
39 typedef Eigen::Matrix<double, 6, 1> Vector6d;
40 typedef Eigen::Matrix<uint8_t, 3, 1> Vector3uint8;
41 
44 typedef Eigen::Matrix<double, 6, 6, Eigen::DontAlign> Matrix6d_u;
45 typedef Eigen::Matrix<double, 4, 4, Eigen::DontAlign> Matrix4d_u;
46 
47 } // namespace Eigen
49 
50 namespace open3d {
51 namespace utility {
52 
53 using Matrix4d_allocator = Eigen::aligned_allocator<Eigen::Matrix4d>;
54 using Matrix6d_allocator = Eigen::aligned_allocator<Eigen::Matrix6d>;
55 using Vector2d_allocator = Eigen::aligned_allocator<Eigen::Vector2d>;
56 using Vector3uint8_allocator = Eigen::aligned_allocator<Eigen::Vector3uint8>;
57 using Vector4i_allocator = Eigen::aligned_allocator<Eigen::Vector4i>;
58 using Vector4d_allocator = Eigen::aligned_allocator<Eigen::Vector4d>;
59 using Vector6d_allocator = Eigen::aligned_allocator<Eigen::Vector6d>;
60 
64 Eigen::Matrix4d TransformVector6dToMatrix4d(const Eigen::Vector6d &input);
65 
71 Eigen::Vector6d TransformMatrix4dToVector6d(const Eigen::Matrix4d &input);
72 
74 std::tuple<bool, Eigen::VectorXd> SolveLinearSystemPSD(
75  const Eigen::MatrixXd &A,
76  const Eigen::VectorXd &b,
77  bool prefer_sparse = false,
78  bool check_symmetric = false,
79  bool check_det = false,
80  bool check_psd = false);
81 
85 std::tuple<bool, Eigen::Matrix4d> SolveJacobianSystemAndObtainExtrinsicMatrix(
86  const Eigen::Matrix6d &JTJ, const Eigen::Vector6d &JTr);
87 
91 std::tuple<bool, std::vector<Eigen::Matrix4d, Matrix4d_allocator>>
92 SolveJacobianSystemAndObtainExtrinsicMatrixArray(const Eigen::MatrixXd &JTJ,
93  const Eigen::VectorXd &JTr);
94 
100 template <typename MatType, typename VecType>
101 std::tuple<MatType, VecType, double> ComputeJTJandJTr(
102  std::function<void(int, VecType &, double &, double &)> f,
103  int iteration_num,
104  bool verbose = true);
105 
111 template <typename MatType, typename VecType>
112 std::tuple<MatType, VecType, double> ComputeJTJandJTr(
113  std::function<
114  void(int,
115  std::vector<VecType, Eigen::aligned_allocator<VecType>> &,
116  std::vector<double> &,
117  std::vector<double> &)> f,
118  int iteration_num,
119  bool verbose = true);
120 
121 Eigen::Matrix3d RotationMatrixX(double radians);
122 Eigen::Matrix3d RotationMatrixY(double radians);
123 Eigen::Matrix3d RotationMatrixZ(double radians);
124 
127 Eigen::Vector3uint8 ColorToUint8(const Eigen::Vector3d &color);
129 Eigen::Vector3d ColorToDouble(uint8_t r, uint8_t g, uint8_t b);
130 Eigen::Vector3d ColorToDouble(const Eigen::Vector3uint8 &rgb);
131 
133 template <typename IdxType>
134 Eigen::Matrix3d ComputeCovariance(const std::vector<Eigen::Vector3d> &points,
135  const std::vector<IdxType> &indices);
136 
138 template <typename IdxType>
139 std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputeMeanAndCovariance(
140  const std::vector<Eigen::Vector3d> &points,
141  const std::vector<IdxType> &indices);
142 } // namespace utility
143 } // namespace open3d
Eigen::Matrix3d RotationMatrixZ(double radians)
Definition: Eigen.cpp:279
Eigen::aligned_allocator< Eigen::Vector2d > Vector2d_allocator
Definition: Eigen.h:55
std::tuple< MatType, VecType, double > ComputeJTJandJTr(std::function< void(int, VecType &, double &, double &)> f, int iteration_num, bool verbose)
Definition: Eigen.cpp:165
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
Eigen::Vector3uint8 ColorToUint8(const Eigen::Vector3d &color)
Definition: Eigen.cpp:286
std::tuple< bool, Eigen::Matrix4d > SolveJacobianSystemAndObtainExtrinsicMatrix(const Eigen::Matrix6d &JTJ, const Eigen::Vector6d &JTr)
Definition: Eigen.cpp:122
Eigen::aligned_allocator< Eigen::Vector4d > Vector4d_allocator
Definition: Eigen.h:58
Eigen::aligned_allocator< Eigen::Matrix6d > Matrix6d_allocator
Definition: Eigen.h:54
Eigen::Matrix4d TransformVector6dToMatrix4d(const Eigen::Vector6d &input)
Definition: Eigen.cpp:93
Eigen::aligned_allocator< Eigen::Vector4i > Vector4i_allocator
Definition: Eigen.h:57
std::tuple< bool, std::vector< Eigen::Matrix4d, Matrix4d_allocator > > SolveJacobianSystemAndObtainExtrinsicMatrixArray(const Eigen::MatrixXd &JTJ, const Eigen::VectorXd &JTr)
Definition: Eigen.cpp:136
math::float4 color
Definition: LineSetBuffers.cpp:64
Eigen::Matrix3d RotationMatrixX(double radians)
Definition: Eigen.cpp:265
Eigen::Vector6d TransformMatrix4dToVector6d(const Eigen::Matrix4d &input)
Definition: Eigen.cpp:105
std::tuple< bool, Eigen::VectorXd > SolveLinearSystemPSD(const Eigen::MatrixXd &A, const Eigen::VectorXd &b, bool prefer_sparse, bool check_symmetric, bool check_det, bool check_psd)
Function to solve Ax=b.
Definition: Eigen.cpp:38
Eigen::aligned_allocator< Eigen::Vector6d > Vector6d_allocator
Definition: Eigen.h:59
int points
Definition: FilePCD.cpp:73
Definition: PinholeCameraIntrinsic.cpp:35
Eigen::Matrix3d RotationMatrixY(double radians)
Definition: Eigen.cpp:272
Eigen::aligned_allocator< Eigen::Matrix4d > Matrix4d_allocator
Definition: Eigen.h:53
Eigen::Vector3d ColorToDouble(uint8_t r, uint8_t g, uint8_t b)
Color conversion from uint8_t 0-255 to double [0,1].
Definition: Eigen.cpp:295
Eigen::aligned_allocator< Eigen::Vector3uint8 > Vector3uint8_allocator
Definition: Eigen.h:56
Eigen::Matrix3d ComputeCovariance(const std::vector< Eigen::Vector3d > &points, const std::vector< IdxType > &indices)
Function to compute the covariance matrix of a set of points.
Definition: Eigen.cpp:304