// Copyright 2018 Slightech Co., Ltd. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_ #define SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_ #include "mynteye/mynteye.h" MYNTEYE_BEGIN_NAMESPACE namespace ctain { #define Matrix_ Matrix<_Scalar> template class SMatrix: public Matrix_{ public: explicit SMatrix(int D) : Matrix_(D, D) {} SMatrix() : Matrix_(0, 0) {} SMatrix(_Scalar _data[], int D) : Matrix_(_data, D, D) {} SMatrix(_Scalar **_data, int D) : Matrix_(_data, D, D) {} explicit SMatrix(Matrix_ m) : Matrix_(m) {} _Scalar determinant(); _Scalar M(int m, int n); SMatrix<_Scalar> inverse() { SMatrix<_Scalar> res(Matrix_::_Rows); _Scalar d = determinant(); for (int i = 0; i < Matrix_::_Rows; i++) { for (int j = 0; j < Matrix_::_Cols; j++) { res.Data(j, i) = 1.0 * M(i, j) / d; } } return res; } void operator =(Matrix<_Scalar> m) { SMatrix t(m); *this = t; } }; template _Scalar SMatrix<_Scalar>::determinant() { int r, c, m; int lop = 0; int n = Matrix_::_Rows; _Scalar result = 0; _Scalar mid = 1; if (n != 1) { lop = (n == 2) ? 1 : n; for (m = 0; m < lop; m++) { mid = 1; for (r = 0, c = m; r < n; r++, c++) { mid = mid * (*(Matrix_::data+r*n+c%n)); } result += mid; } for (m = 0; m < lop; m++) { mid = 1; for (r = 0, c = n-1-m+n; r < n; r++, c--) { mid = mid * (*(Matrix_::data + r * n + c % n)); } result -= mid; } } else { result = Matrix_::data[0]; } return result; } template _Scalar SMatrix<_Scalar>::M(int m, int n) { float mid_result = 0; int sign = 1; int k = Matrix_::_Rows; SMatrix mid(k - 1); int c = 0; for (int i = 0; i < k; i++) { for (int j = 0; j < k; j++) { if (i != m && j != n) { mid.Data(c++) = Matrix_::cData(i, j); } } } sign = (m+n)%2 == 0 ? 1 : -1; mid_result = static_cast<_Scalar>(sign) * mid.determinant(); return mid_result; } #undef Matrix_ } // namespace ctain MYNTEYE_END_NAMESPACE #endif // SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_