tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'rmEigen' of http://gitlab.mynt.com/mynteye/mynt-eye-s-sdk into rmEigen

Browse Source
This commit is contained in:
TinyO 2019-09-05 16:18:02 +08:00
commit af65c2674b
4 changed files with 80 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
src/mynteye/api/camera_models/util/matrix.h
View File

@ -27,46 +27,46 @@ 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) {
_startRow = 0; _startRow = 0;
_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(void) :
_Rows(0), _Cols(0), _isSub(0), input_id(0) { _Rows(0), _Cols(0), _isSub(0), input_id(0) {
_startRow = 0; _startRow = 0;
_startCol = 0; _startCol = 0;
_Rows_raw = 0; _Rows_raw = 0;
_Cols_raw = 0; _Cols_raw = 0;
} }
Matrix(_Scalar _data[], int Rows, int Cols) : Matrix(_Scalar _data[], int _rows, int _cols) :
_Rows(Rows), _Cols(Cols), _isSub(0), input_id(0) { _Rows(_rows), _Cols(_cols), _isSub(0), input_id(0) {
_startRow = 0; _startRow = 0;
_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));
} }
Matrix(_Scalar **_data, int Rows, int Cols) : Matrix(_Scalar **_data, int _rows, int _cols) :
_Rows(Rows), _Cols(Cols), _isSub(0), input_id(0) { _Rows(_rows), _Cols(_cols), _isSub(0), input_id(0) {
_startRow = 0; _startRow = 0;
_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));
} }
} }
template<typename T> template<typename T>
Matrix<T> cast() { Matrix<T> cast(void) const{
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++) {
@ -76,7 +76,7 @@ class Matrix {
return res; return res;
} }
void setIdentity() { void setIdentity(void) {
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) {
@ -87,19 +87,19 @@ class Matrix {
} }
} }
} }
void setZero() { void setZero(void) {
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++) {
Data(i, j) = 0; Data(i, j) = 0;
} }
} }
} }
inline int cols() const { return _Cols; } inline int cols(void) const { return _Cols; }
inline int rows() const { return _Rows; } inline int rows(void) const { return _Rows; }
inline int size() const { return cols() * rows(); } inline int size(void) const { return cols() * rows(); }
inline _Scalar * addr() { inline _Scalar * addr(void) {
return data; return data;
} }
@ -196,49 +196,49 @@ class Matrix {
Matrix<_Scalar> row(int Row) { Matrix<_Scalar> row(int Row) {
return block(Row, 0, 1, _Cols); return block(Row, 0, 1, _Cols);
} }
Matrix<_Scalar> block(int sRow, int sCol, int Rows, int Cols) { Matrix<_Scalar> block(int sRow, int sCol, int _rows, int _cols) {
Matrix<_Scalar> sub; Matrix<_Scalar> sub;
sub = *this; sub = *this;
sub.setSub(sRow, sCol, Rows, Cols, data); sub.setSub(sRow, sCol, _rows, _cols, data);
return sub; return sub;
} }
template<int Rows, int Cols> template<int _rows, int _cols>
Matrix<_Scalar> topLeftCorner() { Matrix<_Scalar> topLeftCorner(void) {
Matrix<_Scalar> sub; Matrix<_Scalar> sub;
sub = *this; sub = *this;
sub.setSub(0, 0, Rows, Cols, data); sub.setSub(0, 0, _rows, _cols, data);
return sub; return sub;
} }
template<int Rows, int Cols> template<int _rows, int _cols>
Matrix<_Scalar> topRightCorner() { Matrix<_Scalar> topRightCorner(void) {
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);
return sub; return sub;
} }
void setSub(int sRow, int sCol, int Rows, int Cols, _Scalar *Data) { void setSub(int sRow, int sCol, int _rows, int _cols, _Scalar *Data) {
_isSub = true; _isSub = true;
_Rows_raw = _Rows; _Rows_raw = _Rows;
_Cols_raw = _Cols; _Cols_raw = _Cols;
_Rows = Rows; _Rows = _rows;
_Cols = Cols; _Cols = _cols;
_startRow = sRow; _startRow = sRow;
_startCol = sCol; _startCol = sCol;
data = Data; data = Data;
} }
void normalize(); void normalize(void);
double norm() const; double norm(void) const;
virtual ~Matrix() { virtual ~Matrix() {
if (!data) if (!data)
delete[] data; delete[] data;
} }
inline _Scalar *Data() { inline _Scalar *Data(void) {
return data; return data;
} }
@ -349,7 +349,7 @@ Matrix<_Scalar> Matrix<_Scalar>::operator -(const Matrix<_Scalar> &m) const {
} }
template<typename _Scalar> template<typename _Scalar>
Matrix<_Scalar> Matrix<_Scalar>::transpose() const { Matrix<_Scalar> Matrix<_Scalar>::transpose(void) 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++) {
@ -392,7 +392,7 @@ Matrix<_Scalar> Matrix<_Scalar>::operator /(double m) const {
template<typename _Scalar> template<typename _Scalar>
void Matrix<_Scalar>::normalize() { void Matrix<_Scalar>::normalize(void) {
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++) {
@ -408,7 +408,7 @@ void Matrix<_Scalar>::normalize() {
} }
template<typename _Scalar> template<typename _Scalar>
double Matrix<_Scalar>::norm() const { double Matrix<_Scalar>::norm(void) 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++) {

46
src/mynteye/api/camera_models/util/matrix_solver.h
View File

@ -20,9 +20,9 @@
MYNTEYE_BEGIN_NAMESPACE MYNTEYE_BEGIN_NAMESPACE
static bool Matrix_EigenValue(double *K1, int n, static bool Matrix_EigenValue(double *k1, int n,
int LoopNumber, double Error1, double *Ret); int loop_number, double error1, double *ret);
static void Matrix_Hessenberg(double *A1, int n, double *ret); static void Matrix_Hessenberg(double *a1, int n, double *ret);
namespace ctain { namespace ctain {
class EigenSolver { class EigenSolver {
@ -40,7 +40,7 @@ class EigenSolver {
delete []A; delete []A;
delete []B; delete []B;
} }
Matrix<double> eigenvalues() { Matrix<double> eigenvalues(void) {
return t; return t;
} }
@ -50,7 +50,7 @@ class EigenSolver {
} // namespace ctain } // namespace ctain
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];
@ -58,7 +58,7 @@ static void Matrix_Hessenberg(double *A1, int n, double *ret) {
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++) {
@ -106,21 +106,21 @@ static void Matrix_Hessenberg(double *A1, int n, double *ret) {
delete []A; delete []A;
} }
static bool Matrix_EigenValue(double *K1, int n, static bool Matrix_EigenValue(double *k1, int n,
int LoopNumber, double Error1, double *Ret) { int loop_number, 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 = loop_number;
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 {
@ -128,10 +128,10 @@ static bool Matrix_EigenValue(double *K1, int n,
} }
} }
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 = loop_number;
} else if (t == m - 2) { } else if (t == m - 2) {
b = -A[(m - 1) * n + m - 1] - A[(m - 2) * n + 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] c = A[(m - 1) * n + m - 1] * A[(m - 2) * n + m - 2]
@ -143,18 +143,18 @@ static bool Matrix_EigenValue(double *K1, int n,
if (b < 0) { if (b < 0) {
xy = -1; xy = -1;
} }
Ret[(m - 1) * 2] = -(b + xy * y) / 2; ret[(m - 1) * 2] = -(b + xy * y) / 2;
Ret[(m - 1) * 2 + 1] = 0; ret[(m - 1) * 2 + 1] = 0;
Ret[(m - 2) * 2] = c / Ret[(m - 1) * 2]; ret[(m - 2) * 2] = c / ret[(m - 1) * 2];
Ret[(m - 2) * 2 + 1] = 0; ret[(m - 2) * 2 + 1] = 0;
} else { } else {
Ret[(m - 1) * 2] = -b / 2; ret[(m - 1) * 2] = -b / 2;
Ret[(m - 2) * 2] = -b / 2; ret[(m - 2) * 2] = -b / 2;
Ret[(m - 1) * 2 + 1] = y / 2; ret[(m - 1) * 2 + 1] = y / 2;
Ret[(m - 2) * 2 + 1] = -y / 2; ret[(m - 2) * 2 + 1] = -y / 2;
} }
m -= 2; m -= 2;
Loop1 = LoopNumber; Loop1 = loop_number;
} else { } else {
if (Loop1 < 1) { if (Loop1 < 1) {
delete []A; delete []A;

25
src/mynteye/api/camera_models/util/matrix_square.h
View File

@ -24,14 +24,22 @@ namespace ctain {
template<typename _Scalar> template<typename _Scalar>
class SMatrix: public Matrix_{ class SMatrix: public Matrix_{
public: public:
explicit SMatrix(int D) : Matrix_(D, D) {} explicit SMatrix(int dim) : Matrix_(dim, dim) {}
SMatrix() : Matrix_(0, 0) {} SMatrix() : Matrix_(0, 0) {}
SMatrix(_Scalar _data[], int D) : Matrix_(_data, D, D) {} SMatrix(_Scalar _data[], int dim) : Matrix_(_data, dim, dim) {}
SMatrix(_Scalar **_data, int D) : Matrix_(_data, D, D) {} SMatrix(_Scalar **_data, int dim) : Matrix_(_data, dim, dim) {}
explicit SMatrix(Matrix_ m) : Matrix_(m) {} explicit SMatrix(Matrix_ m) : Matrix_(m) {}
_Scalar determinant(); _Scalar determinant(void) const;
_Scalar M(int m, int n); _Scalar M(int m, int n);
SMatrix<_Scalar> inverse() { SMatrix<_Scalar> inverse(void);
void operator =(Matrix<_Scalar> m) {
SMatrix t(m);
*this = t;
}
};
template<typename _Scalar>
SMatrix<_Scalar> inverse(void) const{
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++) {
@ -41,14 +49,9 @@ class SMatrix: public Matrix_{
} }
return res; return res;
} }
void operator =(Matrix<_Scalar> m) {
SMatrix t(m);
*this = t;
}
};
template<typename _Scalar> template<typename _Scalar>
_Scalar SMatrix<_Scalar>::determinant() { _Scalar SMatrix<_Scalar>::determinant(void) const{
int r, c, m; int r, c, m;
int lop = 0; int lop = 0;
int n = Matrix_::_Rows; int n = Matrix_::_Rows;

6
src/mynteye/api/camera_models/util/quaternion.h
View File

@ -25,7 +25,7 @@ namespace ctain {
template<typename T> template<typename T>
class Quaternion { class Quaternion {
public: public:
Quaternion() {} Quaternion(void) {}
explicit Quaternion(SMatrix<double> m) { explicit Quaternion(SMatrix<double> m) {
_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);
@ -33,7 +33,7 @@ class Quaternion {
_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(void) {
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;
@ -45,7 +45,7 @@ class Quaternion {
inline T y() {return _y;} inline T y() {return _y;}
inline T z() {return _z;} inline T z() {return _z;}
inline T w() {return _w;} inline T w() {return _w;}
SMatrix<double> toRotationMatrix() const { SMatrix<double> toRotationMatrix(void) 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;