Open3D (C++ API)  0.18.0
Loading...
Searching...
No Matches
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
11namespace open3d {
12namespace core {
13namespace linalg {
14namespace kernel {
15
16// ---- Matmul ----
17template <typename scalar_t>
18static 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
38template <typename scalar_t>
39OPEN3D_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
47template <typename scalar_t>
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
62template <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
75template <typename scalar_t>
77cross_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
87template <typename scalar_t>
89dot_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 ----
95template <typename scalar_t>
96OPEN3D_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
100template <typename scalar_t>
101OPEN3D_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
107template <typename scalar_t>
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 ----
124template <typename scalar_t>
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 ----
146template <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
153template <typename scalar_t>
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
162template <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
175template <typename scalar_t>
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
191template <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
213template <typename scalar_t>
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