Open3D (C++ API)  0.17.0
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_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>
77dot_3x1(const scalar_t* A_3x1_input, const scalar_t* B_3x1_input) {
78 return A_3x1_input[0] * B_3x1_input[0] + A_3x1_input[1] * B_3x1_input[1] +
79 A_3x1_input[2] * B_3x1_input[2];
80}
81
82// ---- Determinant ----
83template <typename scalar_t>
84OPEN3D_DEVICE OPEN3D_FORCE_INLINE scalar_t det2x2(const scalar_t* A_2x2) {
85 return A_2x2[0] * A_2x2[3] - A_2x2[1] * A_2x2[2];
86}
87
88template <typename scalar_t>
89OPEN3D_DEVICE OPEN3D_FORCE_INLINE scalar_t det3x3(const scalar_t* A_3x3) {
90 return A_3x3[0] * (A_3x3[4] * A_3x3[8] - A_3x3[5] * A_3x3[7]) -
91 A_3x3[3] * (A_3x3[1] * A_3x3[8] - A_3x3[2] * A_3x3[7]) +
92 A_3x3[6] * (A_3x3[1] * A_3x3[5] - A_3x3[2] * A_3x3[4]);
93}
94
95template <typename scalar_t>
96OPEN3D_DEVICE OPEN3D_FORCE_INLINE bool inverse2x2(const scalar_t* A_2x2,
97 scalar_t* output_2x2) {
98 scalar_t det = det3x3(A_2x2);
99 if (det < 1e-12) {
100 return false;
101 } else {
102 scalar_t invdet = 1.0 / det;
103 output_2x2[0] = A_2x2[3] * det;
104 output_2x2[1] = -A_2x2[1] * det;
105 output_2x2[2] = -A_2x2[2] * det;
106 output_2x2[3] = A_2x2[0] * det;
107 }
108 return true;
109}
110
111// ---- Matrix Inverse ----
112template <typename scalar_t>
114 scalar_t* output_3x3) {
115 scalar_t det = det3x3(A_3x3);
116 if (det < 1e-12) {
117 return false;
118 } else {
119 scalar_t invdet = 1.0 / det;
120 output_3x3[0] = (A_3x3[4] * A_3x3[8] - A_3x3[7] * A_3x3[5]) * invdet;
121 output_3x3[1] = (A_3x3[2] * A_3x3[7] - A_3x3[1] * A_3x3[8]) * invdet;
122 output_3x3[2] = (A_3x3[1] * A_3x3[5] - A_3x3[2] * A_3x3[4]) * invdet;
123 output_3x3[3] = (A_3x3[5] * A_3x3[6] - A_3x3[3] * A_3x3[8]) * invdet;
124 output_3x3[4] = (A_3x3[0] * A_3x3[8] - A_3x3[2] * A_3x3[6]) * invdet;
125 output_3x3[5] = (A_3x3[3] * A_3x3[2] - A_3x3[0] * A_3x3[5]) * invdet;
126 output_3x3[6] = (A_3x3[3] * A_3x3[7] - A_3x3[6] * A_3x3[4]) * invdet;
127 output_3x3[7] = (A_3x3[6] * A_3x3[1] - A_3x3[0] * A_3x3[7]) * invdet;
128 output_3x3[8] = (A_3x3[0] * A_3x3[4] - A_3x3[3] * A_3x3[1]) * invdet;
129 }
130 return true;
131}
132
133// ---- Matrix Transpose ----
134template <typename scalar_t>
136 scalar_t temp_01 = A_2x2[1];
137 A_2x2[1] = A_2x2[2];
138 A_2x2[2] = temp_01;
139}
140
141template <typename scalar_t>
143 scalar_t* output_2x2) {
144 output_2x2[0] = A_2x2[0];
145 output_2x2[1] = A_2x2[2];
146 output_2x2[2] = A_2x2[1];
147 output_2x2[3] = A_2x2[3];
148}
149
150template <typename scalar_t>
152 scalar_t temp_01 = A_3x3[1];
153 scalar_t temp_02 = A_3x3[2];
154 scalar_t temp_12 = A_3x3[5];
155 A_3x3[1] = A_3x3[3];
156 A_3x3[2] = A_3x3[6];
157 A_3x3[5] = A_3x3[7];
158 A_3x3[3] = temp_01;
159 A_3x3[6] = temp_02;
160 A_3x3[7] = temp_12;
161}
162
163template <typename scalar_t>
165 scalar_t* output_3x3) {
166 output_3x3[0] = A_3x3[0];
167 output_3x3[1] = A_3x3[3];
168 output_3x3[2] = A_3x3[6];
169
170 output_3x3[3] = A_3x3[1];
171 output_3x3[4] = A_3x3[4];
172 output_3x3[5] = A_3x3[7];
173
174 output_3x3[6] = A_3x3[2];
175 output_3x3[7] = A_3x3[5];
176 output_3x3[8] = A_3x3[8];
177}
178
179template <typename scalar_t>
181 scalar_t temp_01 = A_4x4[1];
182 scalar_t temp_02 = A_4x4[2];
183 scalar_t temp_03 = A_4x4[3];
184 scalar_t temp_12 = A_4x4[6];
185 scalar_t temp_13 = A_4x4[7];
186 scalar_t temp_23 = A_4x4[11];
187 A_4x4[1] = A_4x4[4];
188 A_4x4[2] = A_4x4[8];
189 A_4x4[3] = A_4x4[12];
190 A_4x4[6] = A_4x4[9];
191 A_4x4[7] = A_4x4[13];
192 A_4x4[11] = A_4x4[14];
193 A_4x4[4] = temp_01;
194 A_4x4[8] = temp_02;
195 A_4x4[12] = temp_03;
196 A_4x4[9] = temp_12;
197 A_4x4[13] = temp_13;
198 A_4x4[14] = temp_23;
199}
200
201template <typename scalar_t>
203 scalar_t* output_4x4) {
204 output_4x4[0] = A_4x4[0];
205 output_4x4[1] = A_4x4[4];
206 output_4x4[2] = A_4x4[8];
207 output_4x4[3] = A_4x4[12];
208
209 output_4x4[4] = A_4x4[1];
210 output_4x4[5] = A_4x4[5];
211 output_4x4[6] = A_4x4[9];
212 output_4x4[7] = A_4x4[13];
213
214 output_4x4[8] = A_4x4[2];
215 output_4x4[9] = A_4x4[6];
216 output_4x4[10] = A_4x4[10];
217 output_4x4[11] = A_4x4[14];
218
219 output_4x4[12] = A_4x4[3];
220 output_4x4[13] = A_4x4[7];
221 output_4x4[14] = A_4x4[11];
222 output_4x4[15] = A_4x4[15];
223}
224
225} // namespace kernel
226} // namespace linalg
227} // namespace core
228} // 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:151
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose2x2_(scalar_t *A_2x2)
Definition: Matrix.h:135
OPEN3D_DEVICE OPEN3D_FORCE_INLINE scalar_t det2x2(const scalar_t *A_2x2)
Definition: Matrix.h:84
OPEN3D_DEVICE OPEN3D_FORCE_INLINE bool inverse2x2(const scalar_t *A_2x2, scalar_t *output_2x2)
Definition: Matrix.h:96
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:113
OPEN3D_DEVICE OPEN3D_FORCE_INLINE scalar_t det3x3(const scalar_t *A_3x3)
Definition: Matrix.h:89
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose3x3(const scalar_t *A_3x3, scalar_t *output_3x3)
Definition: Matrix.h:164
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose4x4_(scalar_t *A_4x4)
Definition: Matrix.h:180
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose2x2(const scalar_t *A_2x2, scalar_t *output_2x2)
Definition: Matrix.h:142
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose4x4(const scalar_t *A_4x4, scalar_t *output_4x4)
Definition: Matrix.h:202
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:77
Definition: PinholeCameraIntrinsic.cpp:16