style(*): change code styles
This commit is contained in:
Messier
parent
f8d47e6f3a
commit
fff6c84032
|
|
@ -1,8 +1,17 @@
|
||||||
|
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
|
||||||
//
|
//
|
||||||
// Created by 顾涵彬 on 2019-08-28.
|
// 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.
|
||||||
|
|
||||||
#include <cstdint>
|
|
||||||
#include "Matrix.h"
|
#include "Matrix.h"
|
||||||
#include "SquareMatrix.h"
|
#include "SquareMatrix.h"
|
||||||
#include "MatrixSolver.h"
|
#include "MatrixSolver.h"
|
||||||
|
|
@ -32,8 +41,6 @@ namespace Ctain {
|
||||||
typedef Matrix<double> MatrixXcd;
|
typedef Matrix<double> MatrixXcd;
|
||||||
|
|
||||||
typedef Quaternion<double> Quaterniond;
|
typedef Quaternion<double> Quaterniond;
|
||||||
} // end namespace Ctain
|
}
|
||||||
|
|
||||||
#endif // Ctain_CtainBASE_H
|
#endif // Ctain_CtainBASE_H
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,17 +1,27 @@
|
||||||
|
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
|
||||||
//
|
//
|
||||||
// Created by 顾涵彬 on 2019-08-28.
|
// 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 MATRIX_MATRIX_H
|
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_
|
||||||
#define MATRIX_MATRIX_H
|
#define SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_
|
||||||
|
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
||||||
namespace Ctain {
|
namespace Ctain {
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
class Matrix {
|
class Matrix {
|
||||||
public:
|
public:
|
||||||
Matrix(int Rows, int Cols) :
|
Matrix(int Rows, int Cols) :
|
||||||
_Rows(Rows), _Cols(Cols), _isSub(0), input_id(0) {
|
_Rows(Rows), _Cols(Cols), _isSub(0), input_id(0) {
|
||||||
|
|
@ -19,7 +29,7 @@ namespace Ctain {
|
||||||
_startCol = 0;
|
_startCol = 0;
|
||||||
_Rows_raw = Rows;
|
_Rows_raw = Rows;
|
||||||
_Cols_raw = Cols;
|
_Cols_raw = Cols;
|
||||||
data = new _Scalar [_Rows * _Cols];
|
data = new _Scalar[_Rows * _Cols];
|
||||||
memset(data, 0, _Rows * _Cols * sizeof(_Scalar));
|
memset(data, 0, _Rows * _Cols * sizeof(_Scalar));
|
||||||
}
|
}
|
||||||
Matrix() :
|
Matrix() :
|
||||||
|
|
@ -35,7 +45,7 @@ namespace Ctain {
|
||||||
_startCol = 0;
|
_startCol = 0;
|
||||||
_Rows_raw = Rows;
|
_Rows_raw = Rows;
|
||||||
_Cols_raw = Cols;
|
_Cols_raw = Cols;
|
||||||
data = new _Scalar [_Rows * _Cols];
|
data = new _Scalar[Rows * _Cols];
|
||||||
memcpy(data, _data, _Rows * _Cols * sizeof(_Scalar));
|
memcpy(data, _data, _Rows * _Cols * sizeof(_Scalar));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -45,7 +55,7 @@ namespace Ctain {
|
||||||
_startCol = 0;
|
_startCol = 0;
|
||||||
_Rows_raw = Rows;
|
_Rows_raw = Rows;
|
||||||
_Cols_raw = Cols;
|
_Cols_raw = Cols;
|
||||||
data = new _Scalar [_Rows * _Cols];
|
data = new _Scalar[Rows * _Cols];
|
||||||
for (int i = 0; i < _Rows; ++i) {
|
for (int i = 0; i < _Rows; ++i) {
|
||||||
memcpy(data + i * _Cols, *(_data + i), _Cols * sizeof(_Scalar));
|
memcpy(data + i * _Cols, *(_data + i), _Cols * sizeof(_Scalar));
|
||||||
}
|
}
|
||||||
|
|
@ -56,7 +66,7 @@ namespace Ctain {
|
||||||
Matrix<T> res(_Rows, _Cols);
|
Matrix<T> res(_Rows, _Cols);
|
||||||
for (int i = 0; i < _Rows; i++) {
|
for (int i = 0; i < _Rows; i++) {
|
||||||
for (int j = 0; j < _Cols; j++) {
|
for (int j = 0; j < _Cols; j++) {
|
||||||
res(i,j) = cData(i, j);
|
res(i, j) = cData(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
|
|
@ -65,10 +75,10 @@ namespace Ctain {
|
||||||
void setIdentity() {
|
void setIdentity() {
|
||||||
for (int i = 0; i < _Rows; i++) {
|
for (int i = 0; i < _Rows; i++) {
|
||||||
for (int j = 0; j < _Cols; j++) {
|
for (int j = 0; j < _Cols; j++) {
|
||||||
if(i == j) {
|
if (i == j) {
|
||||||
Data(i,j) = 1;
|
Data(i, j) = 1;
|
||||||
}else {
|
} else {
|
||||||
Data(i,j) = 0;
|
Data(i, j) = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -85,28 +95,28 @@ namespace Ctain {
|
||||||
inline int rows() const { return _Rows; }
|
inline int rows() const { return _Rows; }
|
||||||
|
|
||||||
inline int size() const { return cols() * rows(); }
|
inline int size() const { return cols() * rows(); }
|
||||||
_Scalar * addr() {
|
inline _Scalar * addr() {
|
||||||
return data;
|
return data;
|
||||||
}
|
}
|
||||||
|
|
||||||
friend Matrix<_Scalar> &operator <<(Matrix<_Scalar> &m,_Scalar val) {
|
friend Matrix<_Scalar> &operator <<(Matrix<_Scalar> &m, _Scalar val) {
|
||||||
m.Data(m.input_id++) = val;
|
m.Data(m.input_id++) = val;
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
friend std::ostream &operator <<(std::ostream &os,const Matrix<_Scalar> &m) {
|
friend std::ostream &operator <<(std::ostream &os,
|
||||||
// os << std::endl;
|
const Matrix<_Scalar> &m) {
|
||||||
for (int i = 0; i < m._Rows; i++) {
|
for (int i = 0; i < m._Rows; i++) {
|
||||||
for (int j = 0; j < m._Cols - 1; j++) {
|
for (int j = 0; j < m._Cols - 1; j++) {
|
||||||
std::cout.width(10);
|
std::cout.width(10);
|
||||||
std::cout.setf(std::ios::left);
|
std::cout.setf(std::ios::left);
|
||||||
std::cout.precision(6);
|
std::cout.precision(6);
|
||||||
os << m.cData(i,j) ;
|
os << m.cData(i, j);
|
||||||
}
|
}
|
||||||
std::cout.width(9);
|
std::cout.width(9);
|
||||||
std::cout.setf(std::ios::left);
|
std::cout.setf(std::ios::left);
|
||||||
std::cout.precision(8);
|
std::cout.precision(8);
|
||||||
os << m.cData(i,m._Cols - 1) << std::endl;
|
os << m.cData(i, m._Cols - 1) << std::endl;
|
||||||
}
|
}
|
||||||
return os;
|
return os;
|
||||||
}
|
}
|
||||||
|
|
@ -117,7 +127,7 @@ namespace Ctain {
|
||||||
res = m;
|
res = m;
|
||||||
for (int i = 0; i < m._Rows; i++) {
|
for (int i = 0; i < m._Rows; i++) {
|
||||||
for (int j = 0; j < m._Cols; j++) {
|
for (int j = 0; j < m._Cols; j++) {
|
||||||
res.Data(i,j) *= a;
|
res.Data(i, j) *= a;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
|
|
@ -129,7 +139,7 @@ namespace Ctain {
|
||||||
res = m;
|
res = m;
|
||||||
for (int i = 0; i < m._Rows; i++) {
|
for (int i = 0; i < m._Rows; i++) {
|
||||||
for (int j = 0; j < m._Cols; j++) {
|
for (int j = 0; j < m._Cols; j++) {
|
||||||
res.Data(i,j) *= a;
|
res.Data(i, j) *= a;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
|
|
@ -141,7 +151,7 @@ namespace Ctain {
|
||||||
res = m;
|
res = m;
|
||||||
for (int i = 0; i < m._Rows; i++) {
|
for (int i = 0; i < m._Rows; i++) {
|
||||||
for (int j = 0; j < m._Cols; j++) {
|
for (int j = 0; j < m._Cols; j++) {
|
||||||
res.Data(i,j) *= -1;
|
res.Data(i, j) *= -1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
|
|
@ -153,21 +163,20 @@ namespace Ctain {
|
||||||
res = m;
|
res = m;
|
||||||
for (int i = 0; i < m._Rows; i++) {
|
for (int i = 0; i < m._Rows; i++) {
|
||||||
for (int j = 0; j < m._Cols; j++) {
|
for (int j = 0; j < m._Cols; j++) {
|
||||||
res.Data(i,j) += a;
|
res.Data(i, j) += a;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
void operator =(Matrix<_Scalar> m);
|
virtual void operator =(Matrix<_Scalar> m);
|
||||||
// void operator =(Matrix<_Scalar> &m);
|
|
||||||
Matrix<_Scalar> operator +(const Matrix<_Scalar> &m) const;
|
Matrix<_Scalar> operator +(const Matrix<_Scalar> &m) const;
|
||||||
Matrix<_Scalar> operator -(const Matrix<_Scalar> &m) const;
|
Matrix<_Scalar> operator -(const Matrix<_Scalar> &m) const;
|
||||||
Matrix<_Scalar> operator *(const Matrix<_Scalar> &m) const;
|
Matrix<_Scalar> operator *(const Matrix<_Scalar> &m) const;
|
||||||
|
|
||||||
Matrix<_Scalar> operator /(double m) const;
|
Matrix<_Scalar> operator /(double m) const;
|
||||||
_Scalar &operator()(int i, int j) {
|
_Scalar &operator()(int i, int j) {
|
||||||
return Data(i,j);
|
return Data(i, j);
|
||||||
}
|
}
|
||||||
_Scalar &operator()(int id) {
|
_Scalar &operator()(int id) {
|
||||||
return Data(id);
|
return Data(id);
|
||||||
|
|
@ -191,7 +200,7 @@ namespace Ctain {
|
||||||
}
|
}
|
||||||
|
|
||||||
template<int Rows, int Cols>
|
template<int Rows, int Cols>
|
||||||
Matrix<_Scalar> topLeftCorner() const {
|
Matrix<_Scalar> topLeftCorner() {
|
||||||
Matrix<_Scalar> sub;
|
Matrix<_Scalar> sub;
|
||||||
sub = *this;
|
sub = *this;
|
||||||
sub.setSub(0, 0, Rows, Cols, data);
|
sub.setSub(0, 0, Rows, Cols, data);
|
||||||
|
|
@ -199,7 +208,7 @@ namespace Ctain {
|
||||||
}
|
}
|
||||||
|
|
||||||
template<int Rows, int Cols>
|
template<int Rows, int Cols>
|
||||||
Matrix<_Scalar> topRightCorner() const {
|
Matrix<_Scalar> topRightCorner() {
|
||||||
Matrix<_Scalar> sub;
|
Matrix<_Scalar> sub;
|
||||||
sub = *this;
|
sub = *this;
|
||||||
sub.setSub(0, _Cols-Cols, Rows, Cols, data);
|
sub.setSub(0, _Cols-Cols, Rows, Cols, data);
|
||||||
|
|
@ -221,17 +230,14 @@ namespace Ctain {
|
||||||
double norm() const;
|
double norm() const;
|
||||||
|
|
||||||
virtual ~Matrix() {
|
virtual ~Matrix() {
|
||||||
if(!data)
|
if (!data)
|
||||||
delete[] data;
|
delete[] data;
|
||||||
}
|
}
|
||||||
|
|
||||||
// template<int _Rows, int _Cols>
|
|
||||||
// inline Matrix<_Scalar, _Rows, _Cols> block<_Rows, _Cols>(Index i,Index j) {
|
|
||||||
//
|
|
||||||
// }
|
|
||||||
inline _Scalar *Data() {
|
inline _Scalar *Data() {
|
||||||
return data;
|
return data;
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
_Scalar *data;
|
_Scalar *data;
|
||||||
int _Rows;
|
int _Rows;
|
||||||
|
|
@ -241,49 +247,49 @@ namespace Ctain {
|
||||||
int _startCol;
|
int _startCol;
|
||||||
int _Rows_raw;
|
int _Rows_raw;
|
||||||
int _Cols_raw;
|
int _Cols_raw;
|
||||||
int input_id ;
|
int input_id;
|
||||||
inline int id(int i, int j) const {
|
inline int id(int i, int j) const {
|
||||||
if(_isSub)
|
if (_isSub)
|
||||||
return (i + _startRow) * _Cols_raw + j + _startCol;
|
return (i + _startRow) * _Cols_raw + j + _startCol;
|
||||||
else
|
else
|
||||||
return i * _Cols + j;
|
return i * _Cols + j;
|
||||||
}
|
}
|
||||||
inline _Scalar &Data(int i,int j) {
|
inline _Scalar &Data(int i, int j) {
|
||||||
return data[id(i,j)];
|
return data[id(i, j)];
|
||||||
}
|
}
|
||||||
inline _Scalar &Data(int id) {
|
inline _Scalar &Data(int id) {
|
||||||
int i = id / _Cols;
|
int i = id / _Cols;
|
||||||
int j = id % _Cols;
|
int j = id % _Cols;
|
||||||
int index;
|
int index;
|
||||||
if(_isSub)
|
if (_isSub)
|
||||||
index = (i + _startRow) * _Cols_raw + j + _startCol;
|
index = (i + _startRow) * _Cols_raw + j + _startCol;
|
||||||
else
|
else
|
||||||
index = i * _Cols + j;
|
index = i * _Cols + j;
|
||||||
return data[index];
|
return data[index];
|
||||||
}
|
}
|
||||||
inline _Scalar cData(int id) const{
|
inline _Scalar cData(int id) const {
|
||||||
int i = id / _Cols;
|
int i = id / _Cols;
|
||||||
int j = id % _Cols;
|
int j = id % _Cols;
|
||||||
int index;
|
int index;
|
||||||
if(_isSub)
|
if (_isSub)
|
||||||
index = (i + _startRow) * _Cols_raw + j + _startCol;
|
index = (i + _startRow) * _Cols_raw + j + _startCol;
|
||||||
else
|
else
|
||||||
index = i * _Cols + j;
|
index = i * _Cols + j;
|
||||||
return data[index];
|
return data[index];
|
||||||
}
|
}
|
||||||
inline _Scalar cData (int i,int j) const{
|
inline _Scalar cData(int i, int j) const {
|
||||||
return data[id(i,j)];
|
return data[id(i, j)];
|
||||||
}
|
}
|
||||||
|
|
||||||
}; //class Matrix end
|
}; // class Matrix
|
||||||
|
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
void Matrix<_Scalar>::operator =(Matrix<_Scalar> m) {
|
void Matrix<_Scalar>::operator =(Matrix<_Scalar> m) {
|
||||||
if(m._isSub) {
|
if (m._isSub) {
|
||||||
if(_isSub) {
|
if (_isSub) {
|
||||||
for (int i = 0; i < m._Rows; i++) {
|
for (int i = 0; i < m._Rows; i++) {
|
||||||
for (int j = 0; j < m._Cols; j++) {
|
for (int j = 0; j < m._Cols; j++) {
|
||||||
Data(i,j) = m.cData(i, j);
|
Data(i, j) = m.cData(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
|
|
@ -298,15 +304,15 @@ namespace Ctain {
|
||||||
data = m.Data();
|
data = m.Data();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if(!_isSub) {
|
if (!_isSub) {
|
||||||
if(size() != m.size()) {
|
if (size() != m.size()) {
|
||||||
if(size() > 0) {
|
if (size() > 0) {
|
||||||
delete[] data;
|
delete[] data;
|
||||||
}
|
}
|
||||||
_Rows = m._Rows;
|
_Rows = m._Rows;
|
||||||
_Cols = m._Cols;
|
_Cols = m._Cols;
|
||||||
data = new _Scalar[_Rows * _Cols];
|
data = new _Scalar[_Rows * _Cols];
|
||||||
}else {
|
} else {
|
||||||
_Rows = m._Rows;
|
_Rows = m._Rows;
|
||||||
_Cols = m._Cols;
|
_Cols = m._Cols;
|
||||||
}
|
}
|
||||||
|
|
@ -314,34 +320,34 @@ namespace Ctain {
|
||||||
|
|
||||||
for (int i = 0; i < m._Rows; i++) {
|
for (int i = 0; i < m._Rows; i++) {
|
||||||
for (int j = 0; j < m._Cols; j++) {
|
for (int j = 0; j < m._Cols; j++) {
|
||||||
Data(i,j) = m.cData(i, j);
|
Data(i, j) = m.cData(i, j);
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
Matrix<_Scalar> Matrix<_Scalar>::operator +(const Matrix<_Scalar> &m) const{
|
Matrix<_Scalar> Matrix<_Scalar>::operator +(const Matrix<_Scalar> &m) const {
|
||||||
Matrix<_Scalar> sum;
|
Matrix<_Scalar> sum;
|
||||||
sum = *this;
|
sum = *this;
|
||||||
for (int i = 0; i < _Rows * _Cols; i++) {
|
for (int i = 0; i < _Rows * _Cols; i++) {
|
||||||
sum.data[i] += m.data[i];
|
sum.data[i] += m.data[i];
|
||||||
}
|
}
|
||||||
return sum;
|
return sum;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
Matrix<_Scalar> Matrix<_Scalar>::operator -(const Matrix<_Scalar> &m) const{
|
Matrix<_Scalar> Matrix<_Scalar>::operator -(const Matrix<_Scalar> &m) const {
|
||||||
Matrix<_Scalar> sum;
|
Matrix<_Scalar> sum;
|
||||||
sum = *this;
|
sum = *this;
|
||||||
for (int i = 0; i < _Rows * _Cols; i++) {
|
for (int i = 0; i < _Rows * _Cols; i++) {
|
||||||
sum.data[i] -= m.data[i];
|
sum.data[i] -= m.data[i];
|
||||||
}
|
}
|
||||||
return sum;
|
return sum;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
Matrix<_Scalar> Matrix<_Scalar>::transpose() const
|
Matrix<_Scalar> Matrix<_Scalar>::transpose() const
|
||||||
{
|
{
|
||||||
Matrix<_Scalar> res(_Cols, _Rows);
|
Matrix<_Scalar> res(_Cols, _Rows);
|
||||||
for (int i = 0; i < _Rows; i++) {
|
for (int i = 0; i < _Rows; i++) {
|
||||||
for (int j = 0; j < _Cols; j++) {
|
for (int j = 0; j < _Cols; j++) {
|
||||||
|
|
@ -349,12 +355,12 @@ namespace Ctain {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
Matrix<_Scalar> Matrix<_Scalar>::operator *(const Matrix<_Scalar> &m) const {
|
Matrix<_Scalar> Matrix<_Scalar>::operator *(const Matrix<_Scalar> &m) const {
|
||||||
if(_Cols != m._Rows) {
|
if (_Cols != m._Rows) {
|
||||||
//todo:output err
|
// todo:output err
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
Matrix<_Scalar> res(_Rows, m._Cols);
|
Matrix<_Scalar> res(_Rows, m._Cols);
|
||||||
|
|
@ -364,27 +370,27 @@ namespace Ctain {
|
||||||
for (int k = 0; k < _Cols; k++) {
|
for (int k = 0; k < _Cols; k++) {
|
||||||
sum += cData(i, k) * m.cData(k, j);
|
sum += cData(i, k) * m.cData(k, j);
|
||||||
}
|
}
|
||||||
res.Data(i,j) = sum;
|
res.Data(i, j) = sum;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
Matrix<_Scalar> Matrix<_Scalar>::operator /(double m) const {
|
Matrix<_Scalar> Matrix<_Scalar>::operator /(double m) const {
|
||||||
Matrix<_Scalar> res;
|
Matrix<_Scalar> res;
|
||||||
res = *this;
|
res = *this;
|
||||||
for (int i = 0; i < _Rows; i++) {
|
for (int i = 0; i < _Rows; i++) {
|
||||||
for (int j = 0; j < _Cols; j++) {
|
for (int j = 0; j < _Cols; j++) {
|
||||||
res.Data(i,j) /= m;
|
res.Data(i, j) /= m;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
void Matrix<_Scalar>::normalize() {
|
void Matrix<_Scalar>::normalize() {
|
||||||
double sum = 0;
|
double sum = 0;
|
||||||
for (int i = 0; i < _Rows; i++) {
|
for (int i = 0; i < _Rows; i++) {
|
||||||
for (int j = 0; j < _Cols; j++) {
|
for (int j = 0; j < _Cols; j++) {
|
||||||
|
|
@ -397,10 +403,10 @@ namespace Ctain {
|
||||||
Matrix::Data(i, j) /= sum;
|
Matrix::Data(i, j) /= sum;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
double Matrix<_Scalar>::norm() const{
|
double Matrix<_Scalar>::norm() const {
|
||||||
double sum = 0;
|
double sum = 0;
|
||||||
for (int i = 0; i < _Rows; i++) {
|
for (int i = 0; i < _Rows; i++) {
|
||||||
for (int j = 0; j < _Cols; j++) {
|
for (int j = 0; j < _Cols; j++) {
|
||||||
|
|
@ -410,6 +416,6 @@ namespace Ctain {
|
||||||
sum = sqrt(sum);
|
sum = sqrt(sum);
|
||||||
|
|
||||||
return sum;
|
return sum;
|
||||||
}
|
}
|
||||||
} //namespace Ctain end
|
} // namespace Ctain
|
||||||
#endif //MATRIX_MATRIX_H
|
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_
|
||||||
|
|
|
||||||
|
|
@ -2,241 +2,233 @@
|
||||||
// Created by 顾涵彬 on 2019-08-30.
|
// Created by 顾涵彬 on 2019-08-30.
|
||||||
//
|
//
|
||||||
|
|
||||||
#ifndef MATRIX_MATRIXSOLVER_H
|
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
|
||||||
#define MATRIX_MATRIXSOLVER_H
|
#define SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <complex>
|
#include <complex>
|
||||||
static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,double *Ret);
|
|
||||||
static void Matrix_Hessenberg(double *A1,int n,double *ret);
|
static bool Matrix_EigenValue(double *K1, int n,
|
||||||
|
int LoopNumber, double Error1, double *Ret);
|
||||||
|
static void Matrix_Hessenberg(double *A1, int n, double *ret);
|
||||||
|
|
||||||
namespace Ctain {
|
namespace Ctain {
|
||||||
class EigenSolver {
|
class EigenSolver {
|
||||||
public:
|
public:
|
||||||
EigenSolver(SMatrix<double> s) {
|
explicit EigenSolver(SMatrix<double> s) {
|
||||||
double *A = new double[s.rows()*2];
|
double *A = new double[s.rows()*2];
|
||||||
double *B = new double[s.size()];
|
double *B = new double[s.size()];
|
||||||
for (int i = 0; i < s.size(); i++)
|
for (int i = 0; i < s.size(); i++)
|
||||||
B[i] = s(i);
|
B[i] = s(i);
|
||||||
memset(A, 0, sizeof(s.rows()*2));
|
memset(A, 0, sizeof(s.rows()*2));
|
||||||
Matrix_EigenValue(B, s.rows(),1000,1e-10,A);
|
Matrix_EigenValue(B, s.rows(), 1000, 1e-10, A);
|
||||||
Matrix<double> tt(A, s.rows(), 2);
|
Matrix<double> tt(A, s.rows(), 2);
|
||||||
t=tt;
|
t = tt;
|
||||||
std::cout<<"s:"<<s;
|
|
||||||
SMatrix<double> s2(A, s.rows());
|
SMatrix<double> s2(A, s.rows());
|
||||||
std::cout<<"tt:"<<tt;
|
|
||||||
std::cout<<"s2:"<<s2;
|
|
||||||
delete []A;
|
delete []A;
|
||||||
delete []B;
|
delete []B;
|
||||||
}
|
}
|
||||||
Matrix<double> eigenvalues() {
|
Matrix<double> eigenvalues() {
|
||||||
return t;
|
return t;
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Matrix<double> t;
|
Matrix<double> t;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
} // namespace Ctain
|
||||||
|
|
||||||
} //namespace Ctain end
|
static void Matrix_Hessenberg(double *A1, int n, double *ret) {
|
||||||
|
|
||||||
static void Matrix_Hessenberg(double *A1,int n,double *ret)
|
|
||||||
{
|
|
||||||
|
|
||||||
int MaxNumber;
|
int MaxNumber;
|
||||||
double temp,*A;
|
double temp, *A;
|
||||||
A=new double[n*n];
|
A = new double[n*n];
|
||||||
memset(A, 0, sizeof(double)*n*n);
|
memset(A, 0, sizeof(double)*n*n);
|
||||||
for (int i=0;i<n;i++) {
|
for (int i = 0; i < n; i++) {
|
||||||
int k=i*n;
|
int k = i * n;
|
||||||
for (int j=0;j<n;j++)
|
for (int j = 0; j < n; j++) {
|
||||||
{
|
A[k + j] = A1[k + j];
|
||||||
A[k+j]=A1[k+j];
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for (int k=1;k<n-1;k++) {
|
for (int k = 1; k < n-1; k++) {
|
||||||
int i=k-1;
|
int i = k-1;
|
||||||
MaxNumber=k;
|
MaxNumber = k;
|
||||||
temp=fabs(A[k*n+i]);
|
temp = fabs(A[k*n+i]);
|
||||||
for (int j=k+1;j<n;j++) {
|
for (int j = k+1; j < n; j++) {
|
||||||
if (fabs(A[j*n+i])>temp) {
|
if (fabs(A[j * n + i]) > temp) {
|
||||||
temp=fabs(A[j*n+i]);
|
temp = fabs(A[j * n + i]);
|
||||||
MaxNumber=j;
|
MaxNumber = j;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
ret[0]=A[MaxNumber*n+i];
|
ret[0] = A[MaxNumber * n + i];
|
||||||
|
if (ret[0] != 0) {
|
||||||
if (ret[0]!=0) {
|
if (MaxNumber != k) {
|
||||||
if (MaxNumber!=k) {
|
for (int j = k-1; j < n; j++) {
|
||||||
for (int j=k-1;j<n;j++) {
|
temp = A[i * n + j];
|
||||||
temp=A[i*n+j];
|
A[i * n + j] = A[k * n + j];
|
||||||
A[i*n+j]=A[k*n+j];
|
A[k * n + j] = temp;
|
||||||
A[k*n+j]=temp;
|
|
||||||
}
|
}
|
||||||
for (int j=0;j<n;j++) {
|
for (int j = 0; j < n; j++) {
|
||||||
temp=A[j*n+i];
|
temp = A[j * n + i];
|
||||||
A[j*n+i]=A[j*n+k];
|
A[j * n + i] = A[j * n + k];
|
||||||
A[j*n+k]=temp;
|
A[j * n + k] = temp;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for (int i=k+1;i<n;i++) {
|
for (int i = k + 1; i < n; i++) {
|
||||||
temp=A[i*n+k-1]/ret[0];
|
temp = A[i * n + k - 1] / ret[0];
|
||||||
A[i*n+k-1]=0;
|
A[i * n + k - 1] = 0;
|
||||||
for (int j=k;j<n;j++) {
|
for (int j = k; j < n; j++) {
|
||||||
A[i*n+j]-=temp*A[k*n+j];
|
A[i * n + j] -= temp * A[k * n + j];
|
||||||
}
|
}
|
||||||
for (int j=0;j<n;j++) {
|
for (int j = 0; j < n; j++) {
|
||||||
A[j*n+k]+=temp*A[j*n+i];
|
A[j * n + k] += temp * A[j * n + i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for (int i=0;i<n;i++) {
|
for (int i = 0; i < n; i++) {
|
||||||
int k=i*n;
|
int k = i * n;
|
||||||
for (int j=0;j<n;j++) {
|
for (int j = 0; j < n; j++) {
|
||||||
ret[k+j]=A[k+j];
|
ret[k + j] = A[k + j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
delete []A;
|
delete []A;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,double *Ret)
|
static bool Matrix_EigenValue(double *K1, int n,
|
||||||
{
|
int LoopNumber, double Error1, double *Ret) {
|
||||||
int i,j,k,t,m,Loop1;
|
int i, j, k, t, m, Loop1;
|
||||||
double b,c,d,g,xy,p,q,r,x,s,e,f,z,y,temp,*A;
|
double b, c, d, g, xy, p, q, r, x, s, e, f, z, y, temp, *A;
|
||||||
A=new double[n*n];
|
A = new double[n * n];
|
||||||
memset(A, 0, sizeof(double)*n*n);
|
memset(A, 0, sizeof(double) * n * n);
|
||||||
Matrix_Hessenberg(K1,n,A);
|
Matrix_Hessenberg(K1, n, A);
|
||||||
m=n;
|
m = n;
|
||||||
Loop1=LoopNumber;
|
Loop1 = LoopNumber;
|
||||||
while(m!=0) {
|
while (m != 0) {
|
||||||
t=m-1;
|
t = m - 1;
|
||||||
while(t>0) {
|
while (t > 0) {
|
||||||
temp=fabs(A[(t-1)*n+t-1]);
|
temp = fabs(A[(t - 1) * n + t - 1]);
|
||||||
temp+=fabs(A[t*n+t]);
|
temp += fabs(A[t * n + t]);
|
||||||
temp=temp*Error1;
|
temp = temp * Error1;
|
||||||
if (fabs(A[t*n+t-1])>temp) {
|
if (fabs(A[t * n + t - 1]) > temp) {
|
||||||
t--;
|
t--;
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (t==m-1) {
|
if (t == m-1) {
|
||||||
Ret[(m-1)*2]=A[(m-1)*n+m-1];
|
Ret[(m - 1) * 2] = A[(m - 1) * n + m - 1];
|
||||||
Ret[(m-1)*2+1]=0;
|
Ret[(m - 1) * 2 + 1] = 0;
|
||||||
m-=1;
|
m -= 1;
|
||||||
Loop1=LoopNumber;
|
Loop1 = LoopNumber;
|
||||||
|
} else if (t == m - 2) {
|
||||||
|
b = -A[(m - 1) * n + m - 1] - A[(m - 2) * n + m - 2];
|
||||||
|
c = A[(m - 1) * n + m - 1] * A[(m - 2) * n + m - 2]
|
||||||
|
- A[(m - 1) * n + m - 2] * A[(m - 2) * n + m - 1];
|
||||||
|
d = b * b - 4 * c;
|
||||||
|
y = sqrt(fabs(d));
|
||||||
|
if (d > 0) {
|
||||||
|
xy = 1;
|
||||||
|
if (b < 0) {
|
||||||
|
xy = -1;
|
||||||
}
|
}
|
||||||
else if(t==m-2) {
|
Ret[(m - 1) * 2] = -(b + xy * y) / 2;
|
||||||
b=-A[(m-1)*n+m-1]-A[(m-2)*n+m-2];
|
Ret[(m - 1) * 2 + 1] = 0;
|
||||||
c=A[(m-1)*n+m-1]*A[(m-2)*n+m-2]-A[(m-1)*n+m-2]*A[(m-2)*n+m-1];
|
Ret[(m - 2) * 2] = c / Ret[(m - 1) * 2];
|
||||||
d=b*b-4*c;
|
Ret[(m - 2) * 2 + 1] = 0;
|
||||||
y=sqrt(fabs(d));
|
} else {
|
||||||
if (d>0) {
|
Ret[(m - 1) * 2] = -b / 2;
|
||||||
xy=1;
|
Ret[(m - 2) * 2] = -b / 2;
|
||||||
if (b<0) {
|
Ret[(m - 1) * 2 + 1] = y / 2;
|
||||||
xy=-1;
|
Ret[(m - 2) * 2 + 1] = -y / 2;
|
||||||
}
|
}
|
||||||
Ret[(m-1)*2]=-(b+xy*y)/2;
|
m -= 2;
|
||||||
Ret[(m-1)*2+1]=0;
|
Loop1 = LoopNumber;
|
||||||
Ret[(m-2)*2]=c/Ret[(m-1)*2];
|
} else {
|
||||||
Ret[(m-2)*2+1]=0;
|
if (Loop1 < 1) {
|
||||||
}
|
delete []A;
|
||||||
else {
|
|
||||||
Ret[(m-1)*2]=-b/2;
|
|
||||||
Ret[(m-2)*2]=-b/2;
|
|
||||||
Ret[(m-1)*2+1]=y/2;
|
|
||||||
Ret[(m-2)*2+1]=-y/2;
|
|
||||||
}
|
|
||||||
m-=2;
|
|
||||||
Loop1=LoopNumber;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
if (Loop1<1) {
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
Loop1--;
|
Loop1--;
|
||||||
j=t+2;
|
j = t + 2;
|
||||||
while (j<m) {
|
while (j < m) {
|
||||||
A[j*n+j-2]=0;
|
A[j * n + j - 2] = 0;
|
||||||
j++;
|
j++;
|
||||||
}
|
}
|
||||||
j=t+3;
|
j = t + 3;
|
||||||
while (j<m) {
|
while (j < m) {
|
||||||
A[j*n+j-3]=0;
|
A[j * n + j - 3] = 0;
|
||||||
j++;
|
j++;
|
||||||
}
|
}
|
||||||
k=t;
|
k = t;
|
||||||
while (k<m-1) {
|
while (k < m - 1) {
|
||||||
if (k!=t) {
|
if (k != t) {
|
||||||
p=A[k*n+k-1];
|
p = A[k * n + k - 1];
|
||||||
q=A[(k+1)*n+k-1];
|
q = A[(k + 1) * n + k - 1];
|
||||||
if (k!=m-2) {
|
if (k != m - 2) {
|
||||||
r=A[(k+2)*n+k-1];
|
r = A[(k + 2) * n + k - 1];
|
||||||
|
} else {
|
||||||
|
r = 0;
|
||||||
}
|
}
|
||||||
else {
|
} else {
|
||||||
r=0;
|
b = A[(m - 1) * n + m - 1];
|
||||||
|
c = A[(m - 2) * n + m - 2];
|
||||||
|
x = b + c;
|
||||||
|
y = b * c - A[(m - 2) * n + m - 1] * A[(m - 1) * n + m - 2];
|
||||||
|
p = A[t * n + t] * (A[t * n + t] - x) +
|
||||||
|
A[t * n + t + 1] * A[(t + 1) * n + t] + y;
|
||||||
|
q = A[(t + 1) * n + t] * (A[t * n + t] + A[(t + 1) * n + t + 1] - x);
|
||||||
|
r = A[(t + 1) * n + t] * A[(t + 2) * n + t + 1];
|
||||||
}
|
}
|
||||||
|
if (p != 0 || q != 0 || r != 0) {
|
||||||
|
if (p < 0) {
|
||||||
|
xy = -1;
|
||||||
|
} else {
|
||||||
|
xy = 1;
|
||||||
}
|
}
|
||||||
else {
|
s = xy * sqrt(p * p + q * q + r * r);
|
||||||
b=A[(m-1)*n+m-1];
|
if (k != t) {
|
||||||
c=A[(m-2)*n+m-2];
|
A[k * n + k - 1]= -s;
|
||||||
x=b+c;
|
|
||||||
y=b*c-A[(m-2)*n+m-1]*A[(m-1)*n+m-2];
|
|
||||||
p=A[t*n+t]*(A[t*n+t]-x)+A[t*n+t+1]*A[(t+1)*n+t]+y;
|
|
||||||
q=A[(t+1)*n+t]*(A[t*n+t]+A[(t+1)*n+t+1]-x);
|
|
||||||
r=A[(t+1)*n+t]*A[(t+2)*n+t+1];
|
|
||||||
}
|
}
|
||||||
if (p!=0 || q!=0 || r!=0) {
|
e = -q / s;
|
||||||
if (p<0) {
|
f = -r / s;
|
||||||
xy=-1;
|
x = -p / s;
|
||||||
|
y = -x - f * r / (p + s);
|
||||||
|
g = e * r / (p + s);
|
||||||
|
z = -x - e * q / (p + s);
|
||||||
|
for (j = k; j < m; j++) {
|
||||||
|
b = A[k * n + j];
|
||||||
|
c = A[(k + 1) * n + j];
|
||||||
|
p = x * b + e * c;
|
||||||
|
q = e * b + y * c;
|
||||||
|
r = f * b + g * c;
|
||||||
|
if (k != m - 2) {
|
||||||
|
b = A[(k + 2) * n + j];
|
||||||
|
p += f * b;
|
||||||
|
q += g * b;
|
||||||
|
r += z * b;
|
||||||
|
A[(k + 2) * n + j] = r;
|
||||||
}
|
}
|
||||||
else {
|
A[(k + 1) * n + j] = q;
|
||||||
xy=1;
|
A[k * n + j] = p;
|
||||||
}
|
}
|
||||||
s=xy*sqrt(p*p+q*q+r*r);
|
j = k + 3;
|
||||||
if (k!=t) {
|
if (j > m - 2) {
|
||||||
A[k*n+k-1]=-s;
|
j = m - 1;
|
||||||
}
|
}
|
||||||
e=-q/s;
|
for (i = t; i < j + 1; i++) {
|
||||||
f=-r/s;
|
b = A[i * n + k];
|
||||||
x=-p/s;
|
c = A[i * n + k + 1];
|
||||||
y=-x-f*r/(p+s);
|
p = x * b + e * c;
|
||||||
g=e*r/(p+s);
|
q = e * b + y * c;
|
||||||
z=-x-e*q/(p+s);
|
r = f * b + g * c;
|
||||||
for (j=k;j<m;j++) {
|
if (k != m - 2) {
|
||||||
b=A[k*n+j];
|
b = A[i * n + k + 2];
|
||||||
c=A[(k+1)*n+j];
|
p += f * b;
|
||||||
p=x*b+e*c;
|
q += g * b;
|
||||||
q=e*b+y*c;
|
r += z * b;
|
||||||
r=f*b+g*c;
|
A[i * n + k + 2] = r;
|
||||||
if (k!=m-2) {
|
|
||||||
b=A[(k+2)*n+j];
|
|
||||||
p+=f*b;
|
|
||||||
q+=g*b;
|
|
||||||
r+=z*b;
|
|
||||||
A[(k+2)*n+j]=r;
|
|
||||||
}
|
}
|
||||||
A[(k+1)*n+j]=q;
|
A[i * n + k + 1] = q;
|
||||||
A[k*n+j]=p;
|
A[i * n + k] = p;
|
||||||
}
|
|
||||||
j=k+3;
|
|
||||||
if (j>m-2) {
|
|
||||||
j=m-1;
|
|
||||||
}
|
|
||||||
for (i=t;i<j+1;i++) {
|
|
||||||
b=A[i*n+k];
|
|
||||||
c=A[i*n+k+1];
|
|
||||||
p=x*b+e*c;
|
|
||||||
q=e*b+y*c;
|
|
||||||
r=f*b+g*c;
|
|
||||||
if (k!=m-2) {
|
|
||||||
b=A[i*n+k+2];
|
|
||||||
p+=f*b;
|
|
||||||
q+=g*b;
|
|
||||||
r+=z*b;
|
|
||||||
A[i*n+k+2]=r;
|
|
||||||
}
|
|
||||||
A[i*n+k+1]=q;
|
|
||||||
A[i*n+k]=p;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
k++;
|
k++;
|
||||||
|
|
@ -247,4 +239,4 @@ static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,doub
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif //MATRIX_MATRIXSOLVER_H
|
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
|
||||||
|
|
|
||||||
|
|
@ -1,57 +1,66 @@
|
||||||
|
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
|
||||||
//
|
//
|
||||||
// Created by 顾涵彬 on 2019-08-30.
|
// 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 MATRIX_QUATERNION_H
|
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_
|
||||||
#define MATRIX_QUATERNION_H
|
#define SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_
|
||||||
|
|
||||||
#include "SquareMatrix.h"
|
#include "SquareMatrix.h"
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
namespace Ctain {
|
namespace Ctain {
|
||||||
// using SMatrix<double>;
|
template<typename T>
|
||||||
template<typename T>
|
class Quaternion {
|
||||||
class Quaternion {
|
|
||||||
public:
|
public:
|
||||||
Quaternion(){}
|
Quaternion() {}
|
||||||
Quaternion(SMatrix<double> m){
|
explicit Quaternion(SMatrix<double> m) {
|
||||||
//double f = 1e-10;
|
_w = sqrt(m(0) + m(4) + m(8) + 1) / 2;
|
||||||
_w = sqrt(m(0)+m(4)+m(8)+1)/2;
|
_x = (m(5) - m(7)) / (4 * _w);
|
||||||
_x = (m(5)-m(7))/(4*_w);
|
_y = (m(6) - m(2)) / (4 * _w);
|
||||||
_y = (m(6)-m(2))/(4*_w);
|
_z = (m(1) - m(3)) / (4 * _w);
|
||||||
_z = (m(1)-m(3))/(4*_w);
|
|
||||||
}
|
}
|
||||||
Quaternion(T X,T Y,T Z,T W) : _x(X),_y(Y),_z(Z),_w(W){}
|
Quaternion(T X, T Y, T Z, T W) : _x(X), _y(Y), _z(Z), _w(W) {}
|
||||||
void normalize() {
|
void normalize() {
|
||||||
double len;
|
double len;
|
||||||
len = sqrt(_x*_x+_y*_y+_z*_z+_w*_w);
|
len = sqrt(_x * _x + _y * _y + _z * _z + _w * _w);
|
||||||
_x = _x / len;
|
_x = _x / len;
|
||||||
_y = _y / len;
|
_y = _y / len;
|
||||||
_z = _z / len;
|
_z = _z / len;
|
||||||
_w = _w / len;
|
_w = _w / len;
|
||||||
}
|
}
|
||||||
T x(){return _x;}
|
inline T x() {return _x;}
|
||||||
T y(){return _y;}
|
inline T y() {return _y;}
|
||||||
T z(){return _z;}
|
inline T z() {return _z;}
|
||||||
T w(){return _w;}
|
inline T w() {return _w;}
|
||||||
SMatrix<double> toRotationMatrix() const {
|
SMatrix<double> toRotationMatrix() const {
|
||||||
SMatrix<double> r(3);
|
SMatrix<double> r(3);
|
||||||
double q0=_w,q1=_x,q2=_y,q3=_z;
|
double q0 = _w, q1 = _x, q2 = _y, q3 = _z;
|
||||||
r(0) = 1 - 2*q2*q2-2*q3*q3;
|
r(0) = 1 - 2 * q2 * q2 - 2 * q3 * q3;
|
||||||
r(1) = 2*q1*q2+2*q0*q3;
|
r(1) = 2 * q1 * q2 + 2 * q0 * q3;
|
||||||
r(2) = 2*q1*q3-2*q0*q2;
|
r(2) = 2 * q1 * q3 - 2 * q0 * q2;
|
||||||
r(3) = 2*q1*q2 - 2*q0*q3;
|
r(3) = 2 * q1 * q2 - 2 * q0 * q3;
|
||||||
r(4) = 1-2*q1*q1 - 2*q3*q3;
|
r(4) = 1 - 2 * q1 * q1 - 2 * q3 * q3;
|
||||||
r(5) = 2*q2*q3 +2*q0*q1;
|
r(5) = 2 * q2 * q3 + 2 * q0 * q1;
|
||||||
r(6) = 2*q1*q3+2*q0*q2;
|
r(6) = 2 * q1 * q3 + 2 * q0 * q2;
|
||||||
r(7) = 2*q2*q3 - 2*q0*q1;
|
r(7) = 2 * q2 * q3 - 2 * q0 * q1;
|
||||||
r(8) = 1-2*q1*q1 -2*q2*q2;
|
r(8) = 1 - 2 * q1 * q1 - 2 * q2 * q2;
|
||||||
return r;
|
return r;
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
T _x;
|
T _x;
|
||||||
T _y;
|
T _y;
|
||||||
T _z;
|
T _z;
|
||||||
T _w;
|
T _w;
|
||||||
};
|
};
|
||||||
}
|
} // namespace Ctain
|
||||||
#endif //MATRIX_QUATERNION_H
|
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_
|
||||||
|
|
|
||||||
|
|
@ -1,47 +1,53 @@
|
||||||
|
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
|
||||||
//
|
//
|
||||||
// Created by 顾涵彬 on 2019-08-29.
|
// 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 MATRIX_SQUAREMATRIX_H
|
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
|
||||||
#define MATRIX_SQUAREMATRIX_H
|
#define SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
|
||||||
#include "Matrix.h"
|
#include "Matrix.h"
|
||||||
namespace Ctain {
|
namespace Ctain {
|
||||||
#define Matrix Matrix<_Scalar>
|
#define Matrix_ Matrix<_Scalar>
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
class SMatrix: public Matrix{
|
class SMatrix: public Matrix_{
|
||||||
public:
|
public:
|
||||||
SMatrix(int D) : Matrix(D, D) {}
|
explicit SMatrix(int D) : Matrix_(D, D) {}
|
||||||
SMatrix() : Matrix(0, 0) {}
|
SMatrix() : Matrix_(0, 0) {}
|
||||||
SMatrix(_Scalar _data[], int D) :
|
SMatrix(_Scalar _data[], int D) : Matrix_(_data, D, D) {}
|
||||||
Matrix(_data, D, D) {}
|
SMatrix(_Scalar **_data, int D) : Matrix_(_data, D, D) {}
|
||||||
SMatrix(_Scalar **_data, int D) :
|
explicit SMatrix(Matrix_ m) : Matrix_(m) {}
|
||||||
Matrix(_data, D, D) {}
|
|
||||||
SMatrix(Matrix m) :
|
|
||||||
Matrix(m) {}
|
|
||||||
// void operator =(const Matrix &m){
|
|
||||||
// }
|
|
||||||
_Scalar determinant();
|
_Scalar determinant();
|
||||||
_Scalar M(int m, int n);
|
_Scalar M(int m, int n);
|
||||||
SMatrix<_Scalar> inverse() {
|
SMatrix<_Scalar> inverse() {
|
||||||
SMatrix<_Scalar> res(Matrix::_Rows);
|
SMatrix<_Scalar> res(Matrix_::_Rows);
|
||||||
_Scalar d = determinant();
|
_Scalar d = determinant();
|
||||||
for (int i = 0; i < Matrix::_Rows; i++) {
|
for (int i = 0; i < Matrix_::_Rows; i++) {
|
||||||
for (int j = 0; j < Matrix::_Cols; j++) {
|
for (int j = 0; j < Matrix_::_Cols; j++) {
|
||||||
res.Data(j, i) = 1.0*M(i, j)/d;
|
res.Data(j, i) = 1.0 * M(i, j) / d;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
void operator =(Matrix<_Scalar> m) {
|
||||||
|
SMatrix t(m);
|
||||||
|
*this = t;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
template<typename _Scalar>
|
||||||
};//class Matrix end
|
_Scalar SMatrix<_Scalar>::determinant() {
|
||||||
|
|
||||||
template<typename _Scalar>
|
|
||||||
_Scalar SMatrix<_Scalar>::determinant() {
|
|
||||||
int r, c, m;
|
int r, c, m;
|
||||||
int lop = 0;
|
int lop = 0;
|
||||||
int n = Matrix::_Rows;
|
int n = Matrix_::_Rows;
|
||||||
_Scalar result = 0;
|
_Scalar result = 0;
|
||||||
_Scalar mid = 1;
|
_Scalar mid = 1;
|
||||||
if (n != 1) {
|
if (n != 1) {
|
||||||
|
|
@ -49,43 +55,42 @@ namespace Ctain {
|
||||||
for (m = 0; m < lop; m++) {
|
for (m = 0; m < lop; m++) {
|
||||||
mid = 1;
|
mid = 1;
|
||||||
for (r = 0, c = m; r < n; r++, c++) {
|
for (r = 0, c = m; r < n; r++, c++) {
|
||||||
mid = mid * (*(Matrix::data+r*n+c%n));
|
mid = mid * (*(Matrix_::data+r*n+c%n));
|
||||||
}
|
}
|
||||||
result += mid;
|
result += mid;
|
||||||
}
|
}
|
||||||
for (m = 0; m < lop; m++) {
|
for (m = 0; m < lop; m++) {
|
||||||
mid = 1;
|
mid = 1;
|
||||||
for (r = 0, c = n-1-m+n; r < n; r++, c--) {
|
for (r = 0, c = n-1-m+n; r < n; r++, c--) {
|
||||||
mid = mid * (*(Matrix::data+r*n+c%n));
|
mid = mid * (*(Matrix_::data + r * n + c % n));
|
||||||
}
|
}
|
||||||
result -= mid;
|
result -= mid;
|
||||||
}
|
}
|
||||||
|
} else {
|
||||||
|
result = Matrix_::data[0];
|
||||||
}
|
}
|
||||||
else
|
|
||||||
result = Matrix::data[0];
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename _Scalar>
|
template<typename _Scalar>
|
||||||
_Scalar SMatrix<_Scalar>::M(int m, int n) {
|
_Scalar SMatrix<_Scalar>::M(int m, int n) {
|
||||||
float mid_result = 0;
|
float mid_result = 0;
|
||||||
int sign = 1;
|
int sign = 1;
|
||||||
int k = Matrix::_Rows;
|
int k = Matrix_::_Rows;
|
||||||
SMatrix mid(k-1);
|
SMatrix mid(k - 1);
|
||||||
int c = 0;
|
int c = 0;
|
||||||
for (int i = 0; i < k; i++) {
|
for (int i = 0; i < k; i++) {
|
||||||
for (int j = 0; j < k; j++) {
|
for (int j = 0; j < k; j++) {
|
||||||
if (i != m && j != n)
|
if (i != m && j != n) {
|
||||||
{
|
mid.Data(c++) = Matrix_::cData(i, j);
|
||||||
mid.Data(c++) = Matrix::cData(i,j);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
sign = (m+n)%2 == 0 ? 1 : -1;
|
sign = (m+n)%2 == 0 ? 1 : -1;
|
||||||
mid_result = (float)sign*mid.determinant();
|
mid_result = static_cast<_Scalar>(sign) * mid.determinant();
|
||||||
return mid_result;
|
return mid_result;
|
||||||
}
|
}
|
||||||
#undef Matrix
|
#undef Matrix_
|
||||||
|
|
||||||
}//namespace Ctain end
|
} // namespace Ctain
|
||||||
#endif //MATRIX_SQUAREMATRIX_H
|
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
|
||||||
|
|
|
||||||
|
|
@ -127,7 +127,8 @@ class Camera {
|
||||||
|
|
||||||
void projectPoints(
|
void projectPoints(
|
||||||
const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
|
const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
|
||||||
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const;
|
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const; // NOLINT
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
cv::Mat m_mask;
|
cv::Mat m_mask;
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -587,8 +587,7 @@ void EquidistantCamera::fitOddPoly(
|
||||||
}
|
}
|
||||||
Y(i, 0) = y.at(i);
|
Y(i, 0) = y.at(i);
|
||||||
}
|
}
|
||||||
Ctain::SMatrix<double> Tmp;
|
Ctain::SMatrix<double> Tmp(X.transpose() * X);
|
||||||
Tmp = X.transpose() * X;
|
|
||||||
Ctain::MatrixXd A = Tmp.inverse() * X.transpose() * Y;
|
Ctain::MatrixXd A = Tmp.inverse() * X.transpose() * Y;
|
||||||
|
|
||||||
coeffs.resize(A.rows());
|
coeffs.resize(A.rows());
|
||||||
|
|
|
||||||
|
|
@ -127,8 +127,8 @@ class EquidistantCamera : public Camera {
|
||||||
// Projects 3D points to the image plane (Pi function)
|
// Projects 3D points to the image plane (Pi function)
|
||||||
// and calculates jacobian
|
// and calculates jacobian
|
||||||
void spaceToPlane(
|
void spaceToPlane(
|
||||||
const Ctain::Vector3d &P, Ctain::Vector2d &p,
|
const Ctain::Vector3d &P,Ctain::Vector2d &p, // NOLINT
|
||||||
Ctain::Matrix23d &J) const;
|
Ctain::Matrix23d &J) const; // NOLINT
|
||||||
// %output p
|
// %output p
|
||||||
// %output J
|
// %output J
|
||||||
|
|
||||||
|
|
@ -161,7 +161,7 @@ class EquidistantCamera : public Camera {
|
||||||
|
|
||||||
void fitOddPoly(
|
void fitOddPoly(
|
||||||
const std::vector<double> &x, const std::vector<double> &y, int n,
|
const std::vector<double> &x, const std::vector<double> &y, int n,
|
||||||
std::vector<double> &coeffs) const;
|
std::vector<double> &coeffs) const; // NOLINT
|
||||||
|
|
||||||
void backprojectSymmetric(
|
void backprojectSymmetric(
|
||||||
const Ctain::Vector2d &p_u, double &theta, double &phi) const; // NOLINT
|
const Ctain::Vector2d &p_u, double &theta, double &phi) const; // NOLINT
|
||||||
|
|
@ -172,7 +172,7 @@ class EquidistantCamera : public Camera {
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef std::shared_ptr<EquidistantCamera> EquidistantCameraPtr;
|
typedef std::shared_ptr<EquidistantCamera> EquidistantCameraPtr;
|
||||||
typedef std::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr;
|
typedef std::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr; // NOLINT
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
|
T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
|
||||||
|
|
@ -193,7 +193,7 @@ T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void spaceToPlane(
|
void spaceToPlane(
|
||||||
const T *const params, const T *const q, const T *const t,
|
const T *const params, const T *const q, const T *const t,
|
||||||
const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p) {
|
const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p) { // NOLINT
|
||||||
T P_w[3];
|
T P_w[3];
|
||||||
P_w[0] = T(P(0));
|
P_w[0] = T(P(0));
|
||||||
P_w[1] = T(P(1));
|
P_w[1] = T(P(1));
|
||||||
|
|
|
||||||
|
|
@ -109,11 +109,15 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
|
||||||
// these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
|
// these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
|
||||||
double fc_new = DBL_MAX;
|
double fc_new = DBL_MAX;
|
||||||
CvPoint2D64f cc_new[2] = {{0, 0}, {0, 0}};
|
CvPoint2D64f cc_new[2] = {{0, 0}, {0, 0}};
|
||||||
newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;
|
newImgSize = newImgSize.width * newImgSize.height != 0 ?
|
||||||
const double ratio_x = (double)newImgSize.width / imageSize.width / 2;
|
newImgSize : imageSize;
|
||||||
const double ratio_y = (double)newImgSize.height / imageSize.height / 2;
|
const double ratio_x = static_cast<double>(newImgSize.width) /
|
||||||
|
imageSize.width / 2;
|
||||||
|
const double ratio_y = static_cast<double>(newImgSize.height) /
|
||||||
|
imageSize.height / 2;
|
||||||
const double ratio = idx == 1 ? ratio_x : ratio_y;
|
const double ratio = idx == 1 ? ratio_x : ratio_y;
|
||||||
fc_new = (cvmGet(K1, idx ^ 1, idx ^ 1) + cvmGet(K2, idx ^ 1, idx ^ 1)) * ratio;
|
fc_new = (cvmGet(K1, idx ^ 1, idx ^ 1) +
|
||||||
|
cvmGet(K2, idx ^ 1, idx ^ 1)) * ratio;
|
||||||
|
|
||||||
for (k = 0; k < 2; k++) {
|
for (k = 0; k < 2; k++) {
|
||||||
CvPoint2D32f _pts[4];
|
CvPoint2D32f _pts[4];
|
||||||
|
|
@ -126,8 +130,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
|
||||||
Ctain::Vector3d b(3, 1);
|
Ctain::Vector3d b(3, 1);
|
||||||
for (i = 0; i < 4; i++) {
|
for (i = 0; i < 4; i++) {
|
||||||
int j = (i < 2) ? 0 : 1;
|
int j = (i < 2) ? 0 : 1;
|
||||||
a(0) = (float)((i % 2)*(nx));
|
a(0) = static_cast<float>((i % 2)*(nx));
|
||||||
a(1) = (float)(j*(ny));
|
a(1) = static_cast<float>(j*(ny));
|
||||||
if (0 == k) {
|
if (0 == k) {
|
||||||
leftOdo->liftProjective(a, b);
|
leftOdo->liftProjective(a, b);
|
||||||
} else {
|
} else {
|
||||||
|
|
@ -290,7 +294,8 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
|
||||||
// Eigen::Matrix3d R = T.topLeftCorner<3, 3>();
|
// Eigen::Matrix3d R = T.topLeftCorner<3, 3>();
|
||||||
// Eigen::Vector3d t = T.topRightCorner<3, 1>();
|
// Eigen::Vector3d t = T.topRightCorner<3, 1>();
|
||||||
Ctain::Matrix4d T = loadT(ex_right_to_left);
|
Ctain::Matrix4d T = loadT(ex_right_to_left);
|
||||||
Ctain::Matrix3d R = T.topLeftCorner<3, 3>();
|
Ctain::Matrix3d R;
|
||||||
|
R = T.topLeftCorner<3, 3>();
|
||||||
Ctain::Vector3d t = T.topRightCorner<3, 1>();
|
Ctain::Vector3d t = T.topRightCorner<3, 1>();
|
||||||
// cv::Mat cv_R, cv_t;
|
// cv::Mat cv_R, cv_t;
|
||||||
// cv::eigen2cv(R, cv_R);
|
// cv::eigen2cv(R, cv_R);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user