Open3D (C++ API)  0.18.0+252c867
Matrix.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
10 
11 namespace open3d {
12 namespace core {
13 namespace linalg {
14 namespace kernel {
15 
16 // ---- Matmul ----
17 template <typename scalar_t>
18 static OPEN3D_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x1(const scalar_t& m00,
19  const scalar_t& m01,
20  const scalar_t& m02,
21  const scalar_t& m10,
22  const scalar_t& m11,
23  const scalar_t& m12,
24  const scalar_t& m20,
25  const scalar_t& m21,
26  const scalar_t& m22,
27  const scalar_t& v0,
28  const scalar_t& v1,
29  const scalar_t& v2,
30  scalar_t& o0,
31  scalar_t& o1,
32  scalar_t& o2) {
33  o0 = m00 * v0 + m01 * v1 + m02 * v2;
34  o1 = m10 * v0 + m11 * v1 + m12 * v2;
35  o2 = m20 * v0 + m21 * v1 + m22 * v2;
36 }
37 
38 template <typename scalar_t>
39 OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x1(const scalar_t* A_3x3,
40  const scalar_t* B_3x1,
41  scalar_t* C_3x1) {
42  C_3x1[0] = A_3x3[0] * B_3x1[0] + A_3x3[1] * B_3x1[1] + A_3x3[2] * B_3x1[2];
43  C_3x1[1] = A_3x3[3] * B_3x1[0] + A_3x3[4] * B_3x1[1] + A_3x3[5] * B_3x1[2];
44  C_3x1[2] = A_3x3[6] * B_3x1[0] + A_3x3[7] * B_3x1[1] + A_3x3[8] * B_3x1[2];
45 }
46 
47 template <typename scalar_t>
48 OPEN3D_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x3(const scalar_t* A_3x3,
49  const scalar_t* B_3x3,
50  scalar_t* C_3x3) {
51  matmul3x3_3x1(A_3x3[0], A_3x3[1], A_3x3[2], A_3x3[3], A_3x3[4], A_3x3[5],
52  A_3x3[6], A_3x3[7], A_3x3[8], B_3x3[0], B_3x3[3], B_3x3[6],
53  C_3x3[0], C_3x3[3], C_3x3[6]);
54  matmul3x3_3x1(A_3x3[0], A_3x3[1], A_3x3[2], A_3x3[3], A_3x3[4], A_3x3[5],
55  A_3x3[6], A_3x3[7], A_3x3[8], B_3x3[1], B_3x3[4], B_3x3[7],
56  C_3x3[1], C_3x3[4], C_3x3[7]);
57  matmul3x3_3x1(A_3x3[0], A_3x3[1], A_3x3[2], A_3x3[3], A_3x3[4], A_3x3[5],
58  A_3x3[6], A_3x3[7], A_3x3[8], B_3x3[2], B_3x3[5], B_3x3[8],
59  C_3x3[2], C_3x3[5], C_3x3[8]);
60 }
61 
62 template <typename scalar_t>
64  const scalar_t* A_3x1_input,
65  const scalar_t* B_3x1_input,
66  scalar_t* C_3x1_output) {
67  C_3x1_output[0] =
68  A_3x1_input[1] * B_3x1_input[2] - A_3x1_input[2] * B_3x1_input[1];
69  C_3x1_output[1] =
70  A_3x1_input[2] * B_3x1_input[0] - A_3x1_input[0] * B_3x1_input[2];
71  C_3x1_output[2] =
72  A_3x1_input[0] * B_3x1_input[1] - A_3x1_input[1] * B_3x1_input[0];
73 }
74 
75 template <typename scalar_t>
77 cross_mag_3x1(const scalar_t* A_3x1_input, const scalar_t* B_3x1_input) {
78  scalar_t temp_0 =
79  A_3x1_input[1] * B_3x1_input[2] - A_3x1_input[2] * B_3x1_input[1];
80  scalar_t temp_1 =
81  A_3x1_input[2] * B_3x1_input[0] - A_3x1_input[0] * B_3x1_input[2];
82  scalar_t temp_2 =
83  A_3x1_input[0] * B_3x1_input[1] - A_3x1_input[1] * B_3x1_input[0];
84  return sqrt(temp_0 * temp_0 + temp_1 * temp_1 + temp_2 * temp_2);
85 }
86 
87 template <typename scalar_t>
89 dot_3x1(const scalar_t* A_3x1_input, const scalar_t* B_3x1_input) {
90  return A_3x1_input[0] * B_3x1_input[0] + A_3x1_input[1] * B_3x1_input[1] +
91  A_3x1_input[2] * B_3x1_input[2];
92 }
93 
94 // ---- Determinant ----
95 template <typename scalar_t>
96 OPEN3D_DEVICE OPEN3D_FORCE_INLINE scalar_t det2x2(const scalar_t* A_2x2) {
97  return A_2x2[0] * A_2x2[3] - A_2x2[1] * A_2x2[2];
98 }
99 
100 template <typename scalar_t>
101 OPEN3D_DEVICE OPEN3D_FORCE_INLINE scalar_t det3x3(const scalar_t* A_3x3) {
102  return A_3x3[0] * (A_3x3[4] * A_3x3[8] - A_3x3[5] * A_3x3[7]) -
103  A_3x3[3] * (A_3x3[1] * A_3x3[8] - A_3x3[2] * A_3x3[7]) +
104  A_3x3[6] * (A_3x3[1] * A_3x3[5] - A_3x3[2] * A_3x3[4]);
105 }
106 
107 template <typename scalar_t>
108 OPEN3D_DEVICE OPEN3D_FORCE_INLINE bool inverse2x2(const scalar_t* A_2x2,
109  scalar_t* output_2x2) {
110  scalar_t det = det3x3(A_2x2);
111  if (det < 1e-12) {
112  return false;
113  } else {
114  scalar_t invdet = 1.0 / det;
115  output_2x2[0] = A_2x2[3] * det;
116  output_2x2[1] = -A_2x2[1] * det;
117  output_2x2[2] = -A_2x2[2] * det;
118  output_2x2[3] = A_2x2[0] * det;
119  }
120  return true;
121 }
122 
123 // ---- Matrix Inverse ----
124 template <typename scalar_t>
125 OPEN3D_DEVICE OPEN3D_FORCE_INLINE bool inverse3x3(const scalar_t* A_3x3,
126  scalar_t* output_3x3) {
127  scalar_t det = det3x3(A_3x3);
128  if (det < 1e-12) {
129  return false;
130  } else {
131  scalar_t invdet = 1.0 / det;
132  output_3x3[0] = (A_3x3[4] * A_3x3[8] - A_3x3[7] * A_3x3[5]) * invdet;
133  output_3x3[1] = (A_3x3[2] * A_3x3[7] - A_3x3[1] * A_3x3[8]) * invdet;
134  output_3x3[2] = (A_3x3[1] * A_3x3[5] - A_3x3[2] * A_3x3[4]) * invdet;
135  output_3x3[3] = (A_3x3[5] * A_3x3[6] - A_3x3[3] * A_3x3[8]) * invdet;
136  output_3x3[4] = (A_3x3[0] * A_3x3[8] - A_3x3[2] * A_3x3[6]) * invdet;
137  output_3x3[5] = (A_3x3[3] * A_3x3[2] - A_3x3[0] * A_3x3[5]) * invdet;
138  output_3x3[6] = (A_3x3[3] * A_3x3[7] - A_3x3[6] * A_3x3[4]) * invdet;
139  output_3x3[7] = (A_3x3[6] * A_3x3[1] - A_3x3[0] * A_3x3[7]) * invdet;
140  output_3x3[8] = (A_3x3[0] * A_3x3[4] - A_3x3[3] * A_3x3[1]) * invdet;
141  }
142  return true;
143 }
144 
145 // ---- Matrix Transpose ----
146 template <typename scalar_t>
148  scalar_t temp_01 = A_2x2[1];
149  A_2x2[1] = A_2x2[2];
150  A_2x2[2] = temp_01;
151 }
152 
153 template <typename scalar_t>
154 OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose2x2(const scalar_t* A_2x2,
155  scalar_t* output_2x2) {
156  output_2x2[0] = A_2x2[0];
157  output_2x2[1] = A_2x2[2];
158  output_2x2[2] = A_2x2[1];
159  output_2x2[3] = A_2x2[3];
160 }
161 
162 template <typename scalar_t>
164  scalar_t temp_01 = A_3x3[1];
165  scalar_t temp_02 = A_3x3[2];
166  scalar_t temp_12 = A_3x3[5];
167  A_3x3[1] = A_3x3[3];
168  A_3x3[2] = A_3x3[6];
169  A_3x3[5] = A_3x3[7];
170  A_3x3[3] = temp_01;
171  A_3x3[6] = temp_02;
172  A_3x3[7] = temp_12;
173 }
174 
175 template <typename scalar_t>
176 OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose3x3(const scalar_t* A_3x3,
177  scalar_t* output_3x3) {
178  output_3x3[0] = A_3x3[0];
179  output_3x3[1] = A_3x3[3];
180  output_3x3[2] = A_3x3[6];
181 
182  output_3x3[3] = A_3x3[1];
183  output_3x3[4] = A_3x3[4];
184  output_3x3[5] = A_3x3[7];
185 
186  output_3x3[6] = A_3x3[2];
187  output_3x3[7] = A_3x3[5];
188  output_3x3[8] = A_3x3[8];
189 }
190 
191 template <typename scalar_t>
193  scalar_t temp_01 = A_4x4[1];
194  scalar_t temp_02 = A_4x4[2];
195  scalar_t temp_03 = A_4x4[3];
196  scalar_t temp_12 = A_4x4[6];
197  scalar_t temp_13 = A_4x4[7];
198  scalar_t temp_23 = A_4x4[11];
199  A_4x4[1] = A_4x4[4];
200  A_4x4[2] = A_4x4[8];
201  A_4x4[3] = A_4x4[12];
202  A_4x4[6] = A_4x4[9];
203  A_4x4[7] = A_4x4[13];
204  A_4x4[11] = A_4x4[14];
205  A_4x4[4] = temp_01;
206  A_4x4[8] = temp_02;
207  A_4x4[12] = temp_03;
208  A_4x4[9] = temp_12;
209  A_4x4[13] = temp_13;
210  A_4x4[14] = temp_23;
211 }
212 
213 template <typename scalar_t>
214 OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose4x4(const scalar_t* A_4x4,
215  scalar_t* output_4x4) {
216  output_4x4[0] = A_4x4[0];
217  output_4x4[1] = A_4x4[4];
218  output_4x4[2] = A_4x4[8];
219  output_4x4[3] = A_4x4[12];
220 
221  output_4x4[4] = A_4x4[1];
222  output_4x4[5] = A_4x4[5];
223  output_4x4[6] = A_4x4[9];
224  output_4x4[7] = A_4x4[13];
225 
226  output_4x4[8] = A_4x4[2];
227  output_4x4[9] = A_4x4[6];
228  output_4x4[10] = A_4x4[10];
229  output_4x4[11] = A_4x4[14];
230 
231  output_4x4[12] = A_4x4[3];
232  output_4x4[13] = A_4x4[7];
233  output_4x4[14] = A_4x4[11];
234  output_4x4[15] = A_4x4[15];
235 }
236 
237 } // namespace kernel
238 } // namespace linalg
239 } // namespace core
240 } // namespace open3d
Common CUDA utilities.
#define OPEN3D_HOST_DEVICE
Definition: CUDAUtils.h:44
#define OPEN3D_DEVICE
Definition: CUDAUtils.h:45
#define OPEN3D_FORCE_INLINE
Definition: CUDAUtils.h:43
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose3x3_(scalar_t *A_3x3)
Definition: Matrix.h:163
OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE scalar_t cross_mag_3x1(const scalar_t *A_3x1_input, const scalar_t *B_3x1_input)
Definition: Matrix.h:77
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose2x2_(scalar_t *A_2x2)
Definition: Matrix.h:147
OPEN3D_DEVICE OPEN3D_FORCE_INLINE scalar_t det2x2(const scalar_t *A_2x2)
Definition: Matrix.h:96
OPEN3D_DEVICE OPEN3D_FORCE_INLINE bool inverse2x2(const scalar_t *A_2x2, scalar_t *output_2x2)
Definition: Matrix.h:108
OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE void cross_3x1(const scalar_t *A_3x1_input, const scalar_t *B_3x1_input, scalar_t *C_3x1_output)
Definition: Matrix.h:63
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x3(const scalar_t *A_3x3, const scalar_t *B_3x3, scalar_t *C_3x3)
Definition: Matrix.h:48
OPEN3D_DEVICE OPEN3D_FORCE_INLINE bool inverse3x3(const scalar_t *A_3x3, scalar_t *output_3x3)
Definition: Matrix.h:125
OPEN3D_DEVICE OPEN3D_FORCE_INLINE scalar_t det3x3(const scalar_t *A_3x3)
Definition: Matrix.h:101
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose3x3(const scalar_t *A_3x3, scalar_t *output_3x3)
Definition: Matrix.h:176
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose4x4_(scalar_t *A_4x4)
Definition: Matrix.h:192
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose2x2(const scalar_t *A_2x2, scalar_t *output_2x2)
Definition: Matrix.h:154
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose4x4(const scalar_t *A_4x4, scalar_t *output_4x4)
Definition: Matrix.h:214
OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE scalar_t dot_3x1(const scalar_t *A_3x1_input, const scalar_t *B_3x1_input)
Definition: Matrix.h:89
Definition: PinholeCameraIntrinsic.cpp:16