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