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>
class Matrix {
public:
Matrix(int Rows, int Cols) :
_Rows(Rows), _Cols(Cols), _isSub(0), input_id(0) {
Matrix(int _rows, int _cols) :
_Rows(_rows), _Cols(_cols), _isSub(0), input_id(0) {
_startRow = 0;
_startCol = 0;
_Rows_raw = Rows;
_Cols_raw = Cols;
_Rows_raw = _rows;
_Cols_raw = _cols;
data = new _Scalar[_Rows * _Cols];
memset(data, 0, _Rows * _Cols * sizeof(_Scalar));
}
Matrix() :
Matrix(void) :
_Rows(0), _Cols(0), _isSub(0), input_id(0) {
_startRow = 0;
_startCol = 0;
_Rows_raw = 0;
_Cols_raw = 0;
}
Matrix(_Scalar _data[], int Rows, int Cols) :
_Rows(Rows), _Cols(Cols), _isSub(0), input_id(0) {
Matrix(_Scalar _data[], int _rows, int _cols) :
_Rows(_rows), _Cols(_cols), _isSub(0), input_id(0) {
_startRow = 0;
_startCol = 0;
_Rows_raw = Rows;
_Cols_raw = Cols;
data = new _Scalar[Rows * _Cols];
_Rows_raw = _rows;
_Cols_raw = _cols;
data = new _Scalar[_rows * _Cols];
memcpy(data, _data, _Rows * _Cols * sizeof(_Scalar));
}
Matrix(_Scalar **_data, int Rows, int Cols) :
_Rows(Rows), _Cols(Cols), _isSub(0), input_id(0) {
Matrix(_Scalar **_data, int _rows, int _cols) :
_Rows(_rows), _Cols(_cols), _isSub(0), input_id(0) {
_startRow = 0;
_startCol = 0;
_Rows_raw = Rows;
_Cols_raw = Cols;
data = new _Scalar[Rows * _Cols];
_Rows_raw = _rows;
_Cols_raw = _cols;
data = new _Scalar[_rows * _Cols];
for (int i = 0; i < _Rows; ++i) {
memcpy(data + i * _Cols, *(_data + i), _Cols * sizeof(_Scalar));
}
}
template<typename T>
Matrix<T> cast() {
Matrix<T> cast(void) const{
Matrix<T> res(_Rows, _Cols);
for (int i = 0; i < _Rows; i++) {
for (int j = 0; j < _Cols; j++) {
@ -76,7 +76,7 @@ class Matrix {
return res;
}
void setIdentity() {
void setIdentity(void) {
for (int i = 0; i < _Rows; i++) {
for (int j = 0; j < _Cols; j++) {
if (i == j) {
@ -87,19 +87,19 @@ class Matrix {
}
}
}
void setZero() {
void setZero(void) {
for (int i = 0; i < _Rows; i++) {
for (int j = 0; j < _Cols; j++) {
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 _Scalar * addr() {
inline int size(void) const { return cols() * rows(); }
inline _Scalar * addr(void) {
return data;
}
@ -196,49 +196,49 @@ class Matrix {
Matrix<_Scalar> row(int Row) {
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;
sub = *this;
sub.setSub(sRow, sCol, Rows, Cols, data);
sub.setSub(sRow, sCol, _rows, _cols, data);
return sub;
}
template<int Rows, int Cols>
Matrix<_Scalar> topLeftCorner() {
template<int _rows, int _cols>
Matrix<_Scalar> topLeftCorner(void) {
Matrix<_Scalar> sub;
sub = *this;
sub.setSub(0, 0, Rows, Cols, data);
sub.setSub(0, 0, _rows, _cols, data);
return sub;
}
template<int Rows, int Cols>
Matrix<_Scalar> topRightCorner() {
template<int _rows, int _cols>
Matrix<_Scalar> topRightCorner(void) {
Matrix<_Scalar> sub;
sub = *this;
sub.setSub(0, _Cols-Cols, Rows, Cols, data);
sub.setSub(0, _Cols-_cols, _rows, _cols, data);
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;
_Rows_raw = _Rows;
_Cols_raw = _Cols;
_Rows = Rows;
_Cols = Cols;
_Rows = _rows;
_Cols = _cols;
_startRow = sRow;
_startCol = sCol;
data = Data;
}
void normalize();
double norm() const;
void normalize(void);
double norm(void) const;
virtual ~Matrix() {
if (!data)
delete[] data;
}
inline _Scalar *Data() {
inline _Scalar *Data(void) {
return data;
}
@ -349,7 +349,7 @@ Matrix<_Scalar> Matrix<_Scalar>::operator -(const Matrix<_Scalar> &m) const {
}
template<typename _Scalar>
Matrix<_Scalar> Matrix<_Scalar>::transpose() const {
Matrix<_Scalar> Matrix<_Scalar>::transpose(void) const {
Matrix<_Scalar> res(_Cols, _Rows);
for (int i = 0; i < _Rows; i++) {
for (int j = 0; j < _Cols; j++) {
@ -392,7 +392,7 @@ Matrix<_Scalar> Matrix<_Scalar>::operator /(double m) const {
template<typename _Scalar>
void Matrix<_Scalar>::normalize() {
void Matrix<_Scalar>::normalize(void) {
double sum = 0;
for (int i = 0; i < _Rows; i++) {
for (int j = 0; j < _Cols; j++) {
@ -408,7 +408,7 @@ void Matrix<_Scalar>::normalize() {
}
template<typename _Scalar>
double Matrix<_Scalar>::norm() const {
double Matrix<_Scalar>::norm(void) const {
double sum = 0;
for (int i = 0; i < _Rows; i++) {
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
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 loop_number, double error1, double *ret);
static void Matrix_Hessenberg(double *a1, int n, double *ret);
namespace ctain {
class EigenSolver {
@ -40,7 +40,7 @@ class EigenSolver {
delete []A;
delete []B;
}
Matrix<double> eigenvalues() {
Matrix<double> eigenvalues(void) {
return t;
}
@ -50,7 +50,7 @@ class EigenSolver {
} // namespace ctain
static void Matrix_Hessenberg(double *A1, int n, double *ret) {
static void Matrix_Hessenberg(double *a1, int n, double *ret) {
int MaxNumber;
double temp, *A;
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++) {
int k = i * n;
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++) {
@ -106,21 +106,21 @@ static void Matrix_Hessenberg(double *A1, int n, double *ret) {
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 loop_number, double error1, double *ret) {
int i, j, k, t, m, Loop1;
double b, c, d, g, xy, p, q, r, x, s, e, f, z, y, temp, *A;
A = new double[n * n];
memset(A, 0, sizeof(double) * n * n);
Matrix_Hessenberg(K1, n, A);
Matrix_Hessenberg(k1, n, A);
m = n;
Loop1 = LoopNumber;
Loop1 = loop_number;
while (m != 0) {
t = m - 1;
while (t > 0) {
temp = fabs(A[(t - 1) * n + t - 1]);
temp += fabs(A[t * n + t]);
temp = temp * Error1;
temp = temp * error1;
if (fabs(A[t * n + t - 1]) > temp) {
t--;
} else {
@ -128,10 +128,10 @@ static bool Matrix_EigenValue(double *K1, int n,
}
}
if (t == m-1) {
Ret[(m - 1) * 2] = A[(m - 1) * n + m - 1];
Ret[(m - 1) * 2 + 1] = 0;
ret[(m - 1) * 2] = A[(m - 1) * n + m - 1];
ret[(m - 1) * 2 + 1] = 0;
m -= 1;
Loop1 = LoopNumber;
Loop1 = loop_number;
} 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]
@ -143,18 +143,18 @@ static bool Matrix_EigenValue(double *K1, int n,
if (b < 0) {
xy = -1;
}
Ret[(m - 1) * 2] = -(b + xy * y) / 2;
Ret[(m - 1) * 2 + 1] = 0;
Ret[(m - 2) * 2] = c / Ret[(m - 1) * 2];
Ret[(m - 2) * 2 + 1] = 0;
ret[(m - 1) * 2] = -(b + xy * y) / 2;
ret[(m - 1) * 2 + 1] = 0;
ret[(m - 2) * 2] = c / ret[(m - 1) * 2];
ret[(m - 2) * 2 + 1] = 0;
} 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;
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;
Loop1 = loop_number;
} else {
if (Loop1 < 1) {
delete []A;

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

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

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

@ -25,7 +25,7 @@ namespace ctain {
template<typename T>
class Quaternion {
public:
Quaternion() {}
Quaternion(void) {}
explicit Quaternion(SMatrix<double> m) {
_w = sqrt(m(0) + m(4) + m(8) + 1) / 2;
_x = (m(5) - m(7)) / (4 * _w);
@ -33,7 +33,7 @@ class Quaternion {
_z = (m(1) - m(3)) / (4 * _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;
len = sqrt(_x * _x + _y * _y + _z * _z + _w * _w);
_x = _x / len;
@ -45,7 +45,7 @@ class Quaternion {
inline T y() {return _y;}
inline T z() {return _z;}
inline T w() {return _w;}
SMatrix<double> toRotationMatrix() const {
SMatrix<double> toRotationMatrix(void) const {
SMatrix<double> r(3);
double q0 = _w, q1 = _x, q2 = _y, q3 = _z;
r(0) = 1 - 2 * q2 * q2 - 2 * q3 * q3;