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

style(*): change code styles

Browse Source
This commit is contained in:
Messier 2019-09-05 14:44:36 +08:00
parent f8d47e6f3a
commit fff6c84032
10 changed files with 821 additions and 797 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/mynteye/api/camera_models/CtainBase.h
View File

@ -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 "SquareMatrix.h"
#include "MatrixSolver.h"
@ -32,8 +41,6 @@ namespace Ctain {
typedef Matrix<double> MatrixXcd;
typedef Quaternion<double> Quaterniond;
} // end namespace Ctain
}
#endif // Ctain_CtainBASE_H

44
src/mynteye/api/camera_models/Matrix.h
View File

@ -1,9 +1,19 @@
// 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
#define MATRIX_MATRIX_H
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_
#define SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_
#include <cstring>
#include <iostream>
@ -35,7 +45,7 @@ namespace Ctain {
_startCol = 0;
_Rows_raw = Rows;
_Cols_raw = Cols;
data = new _Scalar [_Rows * _Cols];
data = new _Scalar[Rows * _Cols];
memcpy(data, _data, _Rows * _Cols * sizeof(_Scalar));
}
@ -45,7 +55,7 @@ namespace Ctain {
_startCol = 0;
_Rows_raw = Rows;
_Cols_raw = Cols;
data = new _Scalar [_Rows * _Cols];
data = new _Scalar[Rows * _Cols];
for (int i = 0; i < _Rows; ++i) {
memcpy(data + i * _Cols, *(_data + i), _Cols * sizeof(_Scalar));
}
@ -85,7 +95,7 @@ namespace Ctain {
inline int rows() const { return _Rows; }
inline int size() const { return cols() * rows(); }
_Scalar * addr() {
inline _Scalar * addr() {
return data;
}
@ -94,8 +104,8 @@ namespace Ctain {
return m;
}
friend std::ostream &operator <<(std::ostream &os,const Matrix<_Scalar> &m) {
// os << std::endl;
friend std::ostream &operator <<(std::ostream &os,
const Matrix<_Scalar> &m) {
for (int i = 0; i < m._Rows; i++) {
for (int j = 0; j < m._Cols - 1; j++) {
std::cout.width(10);
@ -159,8 +169,7 @@ namespace Ctain {
return res;
}
void operator =(Matrix<_Scalar> m);
// void operator =(Matrix<_Scalar> &m);
virtual 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;
@ -191,7 +200,7 @@ namespace Ctain {
}
template<int Rows, int Cols>
Matrix<_Scalar> topLeftCorner() const {
Matrix<_Scalar> topLeftCorner() {
Matrix<_Scalar> sub;
sub = *this;
sub.setSub(0, 0, Rows, Cols, data);
@ -199,7 +208,7 @@ namespace Ctain {
}
template<int Rows, int Cols>
Matrix<_Scalar> topRightCorner() const {
Matrix<_Scalar> topRightCorner() {
Matrix<_Scalar> sub;
sub = *this;
sub.setSub(0, _Cols-Cols, Rows, Cols, data);
@ -225,13 +234,10 @@ namespace Ctain {
delete[] data;
}
// template<int _Rows, int _Cols>
// inline Matrix<_Scalar, _Rows, _Cols> block<_Rows, _Cols>(Index i,Index j) {
//
// }
inline _Scalar *Data() {
return data;
}
protected:
_Scalar *data;
int _Rows;
@ -275,7 +281,7 @@ namespace Ctain {
return data[id(i, j)];
}
}; //class Matrix end
}; // class Matrix
template<typename _Scalar>
void Matrix<_Scalar>::operator =(Matrix<_Scalar> m) {
@ -411,5 +417,5 @@ namespace Ctain {
return sum;
}
} //namespace Ctain end
#endif //MATRIX_MATRIX_H
} // namespace Ctain
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_

60
src/mynteye/api/camera_models/MatrixSolver.h
View File

@ -2,16 +2,19 @@
// Created by 顾涵彬 on 2019-08-30.
//
#ifndef MATRIX_MATRIXSOLVER_H
#define MATRIX_MATRIXSOLVER_H
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
#define SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
#include <cmath>
#include <complex>
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);
static void Matrix_Hessenberg(double *A1, int n, double *ret);
namespace Ctain {
class EigenSolver {
public:
EigenSolver(SMatrix<double> s) {
explicit EigenSolver(SMatrix<double> s) {
double *A = new double[s.rows()*2];
double *B = new double[s.size()];
for (int i = 0; i < s.size(); i++)
@ -20,34 +23,28 @@ namespace Ctain {
Matrix_EigenValue(B, s.rows(), 1000, 1e-10, A);
Matrix<double> tt(A, s.rows(), 2);
t = tt;
std::cout<<"s:"<<s;
SMatrix<double> s2(A, s.rows());
std::cout<<"tt:"<<tt;
std::cout<<"s2:"<<s2;
delete []A;
delete []B;
}
Matrix<double> eigenvalues() {
return t;
}
private:
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;
double temp, *A;
A = new double[n*n];
memset(A, 0, sizeof(double)*n*n);
for (int i = 0; i < n; i++) {
int k = i * n;
for (int j=0;j<n;j++)
{
for (int j = 0; j < n; j++) {
A[k + j] = A1[k + j];
}
}
@ -62,7 +59,6 @@ static void Matrix_Hessenberg(double *A1,int n,double *ret)
}
}
ret[0] = A[MaxNumber * n + i];
if (ret[0] != 0) {
if (MaxNumber != k) {
for (int j = k-1; j < n; j++) {
@ -97,8 +93,8 @@ 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 LoopNumber, 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];
@ -114,8 +110,7 @@ static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,doub
temp = temp * Error1;
if (fabs(A[t * n + t - 1]) > temp) {
t--;
}
else {
} else {
break;
}
}
@ -124,10 +119,10 @@ static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,doub
Ret[(m - 1) * 2 + 1] = 0;
m -= 1;
Loop1 = LoopNumber;
}
else if(t==m-2) {
} 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];
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) {
@ -139,8 +134,7 @@ static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,doub
Ret[(m - 1) * 2 + 1] = 0;
Ret[(m - 2) * 2] = c / Ret[(m - 1) * 2];
Ret[(m - 2) * 2 + 1] = 0;
}
else {
} else {
Ret[(m - 1) * 2] = -b / 2;
Ret[(m - 2) * 2] = -b / 2;
Ret[(m - 1) * 2 + 1] = y / 2;
@ -148,9 +142,9 @@ static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,doub
}
m -= 2;
Loop1 = LoopNumber;
}
else {
} else {
if (Loop1 < 1) {
delete []A;
return false;
}
Loop1--;
@ -171,25 +165,23 @@ static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,doub
q = A[(k + 1) * n + k - 1];
if (k != m - 2) {
r = A[(k + 2) * n + k - 1];
}
else {
} else {
r = 0;
}
}
else {
} else {
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;
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 {
} else {
xy = 1;
}
s = xy * sqrt(p * p + q * q + r * r);
@ -247,4 +239,4 @@ static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,doub
return true;
}
#endif //MATRIX_MATRIXSOLVER_H
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_

33
src/mynteye/api/camera_models/Quaternion.h
View File

@ -1,20 +1,28 @@
// 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
#define MATRIX_QUATERNION_H
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_
#define SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_
#include "SquareMatrix.h"
#include <cmath>
namespace Ctain {
// using SMatrix<double>;
template<typename T>
class Quaternion {
public:
Quaternion() {}
Quaternion(SMatrix<double> m){
//double f = 1e-10;
explicit Quaternion(SMatrix<double> m) {
_w = sqrt(m(0) + m(4) + m(8) + 1) / 2;
_x = (m(5) - m(7)) / (4 * _w);
_y = (m(6) - m(2)) / (4 * _w);
@ -29,10 +37,10 @@ namespace Ctain {
_z = _z / len;
_w = _w / len;
}
T x(){return _x;}
T y(){return _y;}
T z(){return _z;}
T w(){return _w;}
inline T x() {return _x;}
inline T y() {return _y;}
inline T z() {return _z;}
inline T w() {return _w;}
SMatrix<double> toRotationMatrix() const {
SMatrix<double> r(3);
double q0 = _w, q1 = _x, q2 = _y, q3 = _z;
@ -47,11 +55,12 @@ namespace Ctain {
r(8) = 1 - 2 * q1 * q1 - 2 * q2 * q2;
return r;
}
private:
T _x;
T _y;
T _z;
T _w;
};
}
#endif //MATRIX_QUATERNION_H
} // namespace Ctain
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_

75
src/mynteye/api/camera_models/SquareMatrix.h
View File

@ -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
#define MATRIX_SQUAREMATRIX_H
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
#define SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
#include "Matrix.h"
namespace Ctain {
#define Matrix Matrix<_Scalar>
#define Matrix_ Matrix<_Scalar>
template<typename _Scalar>
class SMatrix: public Matrix{
class SMatrix: public Matrix_{
public:
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) {}
SMatrix(Matrix m) :
Matrix(m) {}
// void operator =(const Matrix &m){
// }
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);
SMatrix<_Scalar> res(Matrix_::_Rows);
_Scalar d = determinant();
for (int i = 0; i < Matrix::_Rows; i++) {
for (int j = 0; j < Matrix::_Cols; j++) {
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;
}
};//class Matrix end
void operator =(Matrix<_Scalar> m) {
SMatrix t(m);
*this = t;
}
};
template<typename _Scalar>
_Scalar SMatrix<_Scalar>::determinant() {
int r, c, m;
int lop = 0;
int n = Matrix::_Rows;
int n = Matrix_::_Rows;
_Scalar result = 0;
_Scalar mid = 1;
if (n != 1) {
@ -49,20 +55,20 @@ namespace Ctain {
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));
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));
mid = mid * (*(Matrix_::data + r * n + c % n));
}
result -= mid;
}
} else {
result = Matrix_::data[0];
}
else
result = Matrix::data[0];
return result;
}
@ -70,22 +76,21 @@ namespace Ctain {
_Scalar SMatrix<_Scalar>::M(int m, int n) {
float mid_result = 0;
int sign = 1;
int k = Matrix::_Rows;
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);
if (i != m && j != n) {
mid.Data(c++) = Matrix_::cData(i, j);
}
}
}
sign = (m+n)%2 == 0 ? 1 : -1;
mid_result = (float)sign*mid.determinant();
mid_result = static_cast<_Scalar>(sign) * mid.determinant();
return mid_result;
}
#undef Matrix
#undef Matrix_
}//namespace Ctain end
#endif //MATRIX_SQUAREMATRIX_H
} // namespace Ctain
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_

3
src/mynteye/api/camera_models/camera.h
View File

@ -127,7 +127,8 @@ class Camera {
void projectPoints(
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:
cv::Mat m_mask;
};

3
src/mynteye/api/camera_models/equidistant_camera.cc
View File

@ -587,8 +587,7 @@ void EquidistantCamera::fitOddPoly(
}
Y(i, 0) = y.at(i);
}
Ctain::SMatrix<double> Tmp;
Tmp = X.transpose() * X;
Ctain::SMatrix<double> Tmp(X.transpose() * X);
Ctain::MatrixXd A = Tmp.inverse() * X.transpose() * Y;
coeffs.resize(A.rows());

10
src/mynteye/api/camera_models/equidistant_camera.h
View File

@ -127,8 +127,8 @@ class EquidistantCamera : public Camera {
// Projects 3D points to the image plane (Pi function)
// and calculates jacobian
void spaceToPlane(
const Ctain::Vector3d &P, Ctain::Vector2d &p,
Ctain::Matrix23d &J) const;
const Ctain::Vector3d &P,Ctain::Vector2d &p, // NOLINT
Ctain::Matrix23d &J) const; // NOLINT
// %output p
// %output J
@ -161,7 +161,7 @@ class EquidistantCamera : public Camera {
void fitOddPoly(
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(
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<const EquidistantCamera> EquidistantCameraConstPtr;
typedef std::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr; // NOLINT
template <typename T>
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>
void spaceToPlane(
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];
P_w[0] = T(P(0));
P_w[1] = T(P(1));

19
src/mynteye/api/processor/rectify_processor.cc
View File

@ -109,11 +109,15 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
// these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
double fc_new = DBL_MAX;
CvPoint2D64f cc_new[2] = {{0, 0}, {0, 0}};
newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;
const double ratio_x = (double)newImgSize.width / imageSize.width / 2;
const double ratio_y = (double)newImgSize.height / imageSize.height / 2;
newImgSize = newImgSize.width * newImgSize.height != 0 ?
newImgSize : imageSize;
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;
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++) {
CvPoint2D32f _pts[4];
@ -126,8 +130,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
Ctain::Vector3d b(3, 1);
for (i = 0; i < 4; i++) {
int j = (i < 2) ? 0 : 1;
a(0) = (float)((i % 2)*(nx));
a(1) = (float)(j*(ny));
a(0) = static_cast<float>((i % 2)*(nx));
a(1) = static_cast<float>(j*(ny));
if (0 == k) {
leftOdo->liftProjective(a, b);
} else {
@ -290,7 +294,8 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
// Eigen::Matrix3d R = T.topLeftCorner<3, 3>();
// Eigen::Vector3d t = T.topRightCorner<3, 1>();
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>();
// cv::Mat cv_R, cv_t;
// cv::eigen2cv(R, cv_R);