refactor(src/api/camera_models):add independent Matrix functions to camera_models
This commit is contained in:
Messier
parent
0ac8d7bc16
commit
8bc2401bb3
38
include/Ctain/CtainBase.h
Normal file
38
include/Ctain/CtainBase.h
Normal file
|
|
@ -0,0 +1,38 @@
|
|||
//
|
||||
// Created by 顾涵彬 on 2019-08-28.
|
||||
//
|
||||
|
||||
#include <cstdint>
|
||||
#include "Matrix.h"
|
||||
#include "SquareMatrix.h"
|
||||
#include "MatrixSolver.h"
|
||||
#include "Quaternion.h"
|
||||
#ifndef MATRIX_CTAIN_H
|
||||
#define MATRIX_CTAIN_H
|
||||
|
||||
|
||||
namespace Ctain {
|
||||
typedef SMatrix<double> Matrixd;
|
||||
typedef Matrix<double> MatrixXd;
|
||||
typedef Matrix<double> Matrix23d;
|
||||
typedef SMatrix<double> Matrix33d;
|
||||
|
||||
typedef SMatrix<float> Matrixf;
|
||||
typedef Matrixf Matrix2f;
|
||||
typedef Matrixf Matrix3f;
|
||||
|
||||
typedef Matrix<float> Vectorf;
|
||||
typedef Vectorf Vector2f;
|
||||
typedef Vectorf Vector3f;
|
||||
|
||||
typedef Matrix<double> Vectord;
|
||||
typedef Matrix<double> Vector2d;
|
||||
typedef Matrix<double> Vector3d;
|
||||
typedef Matrix<double> MatrixXcd;
|
||||
|
||||
typedef Quaternion<double> Quaterniond;
|
||||
} // end namespace Ctain
|
||||
|
||||
#endif // Ctain_CtainBASE_H
|
||||
|
||||
|
||||
395
include/Ctain/Matrix.h
Normal file
395
include/Ctain/Matrix.h
Normal file
|
|
@ -0,0 +1,395 @@
|
|||
//
|
||||
// Created by 顾涵彬 on 2019-08-28.
|
||||
//
|
||||
|
||||
#ifndef MATRIX_MATRIX_H
|
||||
#define MATRIX_MATRIX_H
|
||||
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
|
||||
namespace Ctain {
|
||||
template<typename _Scalar>
|
||||
class Matrix {
|
||||
public:
|
||||
Matrix(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];
|
||||
memset(data, 0, _Rows * _Cols * sizeof(_Scalar));
|
||||
}
|
||||
Matrix() :
|
||||
_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) {
|
||||
_startRow = 0;
|
||||
_startCol = 0;
|
||||
_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) {
|
||||
_startRow = 0;
|
||||
_startCol = 0;
|
||||
_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> res(_Rows, _Cols);
|
||||
for (int i = 0; i < _Rows; i++) {
|
||||
for (int j = 0; j < _Cols; j++) {
|
||||
res(i,j) = cData(i, j);
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
void setIdentity() {
|
||||
for (int i = 0; i < _Rows; i++) {
|
||||
for (int j = 0; j < _Cols; j++) {
|
||||
if(i == j) {
|
||||
Data(i,j) = 1;
|
||||
}else {
|
||||
Data(i,j) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
void setZero() {
|
||||
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 rows() const { return _Rows; }
|
||||
|
||||
inline int size() const { return cols() * rows(); }
|
||||
_Scalar * addr() {
|
||||
return data;
|
||||
}
|
||||
|
||||
friend Matrix<_Scalar> &operator <<(Matrix<_Scalar> &m,_Scalar val) {
|
||||
m.Data(m.input_id++) = val;
|
||||
return m;
|
||||
}
|
||||
|
||||
|
||||
friend std::ostream &operator <<(std::ostream &os,const Matrix<_Scalar> &m) {
|
||||
// os << std::endl;
|
||||
for (int i = 0; i < m._Rows; i++) {
|
||||
for (int j = 0; j < m._Cols - 1; j++) {
|
||||
std::cout.width(10);
|
||||
std::cout.setf(std::ios::left);
|
||||
std::cout.precision(6);
|
||||
os << m.cData(i,j) ;
|
||||
}
|
||||
std::cout.width(9);
|
||||
std::cout.setf(std::ios::left);
|
||||
std::cout.precision(8);
|
||||
os << m.cData(i,m._Cols - 1) << std::endl;
|
||||
}
|
||||
return os;
|
||||
}
|
||||
|
||||
friend Matrix<_Scalar> operator *(
|
||||
double a, const Matrix<_Scalar> &m) {
|
||||
Matrix<_Scalar> res;
|
||||
res = m;
|
||||
for(int i = 0; i < m._Rows; i++) {
|
||||
for(int j = 0; j < m._Cols; j++) {
|
||||
res.Data(i,j) *= a;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
friend Matrix<_Scalar> operator -(
|
||||
const Matrix<_Scalar> &m) {
|
||||
Matrix<_Scalar> res;
|
||||
res = m;
|
||||
for(int i = 0; i < m._Rows; i++) {
|
||||
for(int j = 0; j < m._Cols; j++) {
|
||||
res.Data(i,j) *= -1;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
friend Matrix<_Scalar> operator +(
|
||||
double a, const Matrix<_Scalar> &m) {
|
||||
Matrix<_Scalar> res;
|
||||
res = m;
|
||||
for(int i = 0; i < m._Rows; i++) {
|
||||
for(int j = 0; j < m._Cols; j++) {
|
||||
res.Data(i,j) += a;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
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 /(double m) const;
|
||||
_Scalar &operator()(int i, int j) {
|
||||
return Data(i,j);
|
||||
}
|
||||
_Scalar &operator()(int id) {
|
||||
return Data(id);
|
||||
}
|
||||
_Scalar operator()(int id) const {
|
||||
return cData(id);
|
||||
}
|
||||
Matrix<_Scalar> transpose() const;
|
||||
|
||||
Matrix<_Scalar> col(int Col) {
|
||||
return block(0, Col, _Rows, 1);
|
||||
}
|
||||
Matrix<_Scalar> row(int Row) {
|
||||
return block(Row, 0, 1, _Cols);
|
||||
}
|
||||
Matrix<_Scalar> block(int sRow, int sCol, int Rows, int Cols) {
|
||||
Matrix<_Scalar> sub;
|
||||
sub = *this;
|
||||
sub.setSub(sRow, sCol, Rows, Cols, data);
|
||||
return sub;
|
||||
}
|
||||
|
||||
template<int Rows, int Cols>
|
||||
Matrix<_Scalar> topLeftCorner() const {
|
||||
Matrix<_Scalar> sub;
|
||||
sub = *this;
|
||||
sub.setSub(0, 0, Rows, Cols, data);
|
||||
return sub;
|
||||
}
|
||||
|
||||
template<int Rows, int Cols>
|
||||
Matrix<_Scalar> topRightCorner() const {
|
||||
Matrix<_Scalar> sub;
|
||||
sub = *this;
|
||||
sub.setSub(_Rows-1-Rows, _Cols-1-Cols, Rows, Cols, data);
|
||||
return sub;
|
||||
}
|
||||
|
||||
void setSub(int sRow, int sCol, int Rows, int Cols, _Scalar *Data) {
|
||||
_isSub = true;
|
||||
_Rows_raw = _Rows;
|
||||
_Cols_raw = _Cols;
|
||||
_Rows = Rows;
|
||||
_Cols = Cols;
|
||||
_startRow = sRow;
|
||||
_startCol = sCol;
|
||||
data = Data;
|
||||
}
|
||||
|
||||
void normalize();
|
||||
double norm() const;
|
||||
|
||||
virtual ~Matrix() {
|
||||
if(!data)
|
||||
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;
|
||||
int _Cols;
|
||||
bool _isSub;
|
||||
int _startRow;
|
||||
int _startCol;
|
||||
int _Rows_raw;
|
||||
int _Cols_raw;
|
||||
int input_id ;
|
||||
inline int id(int i, int j) const {
|
||||
if(_isSub)
|
||||
return (i + _startRow) * _Cols_raw + j + _startCol;
|
||||
else
|
||||
return i * _Cols + j;
|
||||
}
|
||||
inline _Scalar &Data(int i,int j) {
|
||||
return data[id(i,j)];
|
||||
}
|
||||
inline _Scalar &Data(int id) {
|
||||
int i = id / _Cols;
|
||||
int j = id % _Cols;
|
||||
int index;
|
||||
if(_isSub)
|
||||
index = (i + _startRow) * _Cols_raw + j + _startCol;
|
||||
else
|
||||
index = i * _Cols + j;
|
||||
return data[index];
|
||||
}
|
||||
inline _Scalar cData(int id) const{
|
||||
int i = id / _Cols;
|
||||
int j = id % _Cols;
|
||||
int index;
|
||||
if(_isSub)
|
||||
index = (i + _startRow) * _Cols_raw + j + _startCol;
|
||||
else
|
||||
index = i * _Cols + j;
|
||||
return data[index];
|
||||
}
|
||||
inline _Scalar cData (int i,int j) const{
|
||||
return data[id(i,j)];
|
||||
}
|
||||
|
||||
}; //class Matrix end
|
||||
|
||||
template<typename _Scalar>
|
||||
void Matrix<_Scalar>::operator =(Matrix<_Scalar> m) {
|
||||
if(m._isSub) {
|
||||
_isSub = true;
|
||||
_Rows = m._Rows;
|
||||
_Cols = m._Cols;
|
||||
_Rows_raw = m._Rows_raw;
|
||||
_Cols_raw = m._Cols_raw;
|
||||
_startRow = m._startRow;
|
||||
_startCol = m._startCol;
|
||||
data = m.Data();
|
||||
return;
|
||||
}
|
||||
if(!_isSub) {
|
||||
if(size() != m.size()) {
|
||||
if(size() > 0) {
|
||||
delete[] data;
|
||||
}
|
||||
_Rows = m._Rows;
|
||||
_Cols = m._Cols;
|
||||
data = new _Scalar[_Rows * _Cols];
|
||||
}else {
|
||||
_Rows = m._Rows;
|
||||
_Cols = m._Cols;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < m._Rows; i++) {
|
||||
for (int j = 0; j < m._Cols; j++) {
|
||||
Data(i,j) = m.cData(i, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template<typename _Scalar>
|
||||
Matrix<_Scalar> Matrix<_Scalar>::operator +(const Matrix<_Scalar> &m) const{
|
||||
Matrix<_Scalar> sum;
|
||||
sum = *this;
|
||||
for(int i = 0; i < _Rows * _Cols; i++) {
|
||||
sum.data[i] += m.data[i];
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
template<typename _Scalar>
|
||||
Matrix<_Scalar> Matrix<_Scalar>::operator -(const Matrix<_Scalar> &m) const{
|
||||
Matrix<_Scalar> sum;
|
||||
sum = *this;
|
||||
for(int i = 0; i < _Rows * _Cols; i++) {
|
||||
sum.data[i] -= m.data[i];
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
template<typename _Scalar>
|
||||
Matrix<_Scalar> Matrix<_Scalar>::transpose() const
|
||||
{
|
||||
Matrix<_Scalar> res(_Cols, _Rows);
|
||||
for(int i = 0; i < _Rows; i++) {
|
||||
for (int j = 0; j < _Cols; j++) {
|
||||
res.Data(j, i) = cData(i, j);
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename _Scalar>
|
||||
Matrix<_Scalar> Matrix<_Scalar>::operator *(const Matrix<_Scalar> &m) const {
|
||||
if(_Cols != m._Rows) {
|
||||
//todo:output err
|
||||
return m;
|
||||
}
|
||||
Matrix<_Scalar> res(_Rows, m._Cols);
|
||||
for(int i = 0; i < _Rows; i++) {
|
||||
for(int j = 0; j < m._Cols; j++) {
|
||||
int sum = 0;
|
||||
for(int k = 0; k < _Cols; k++) {
|
||||
sum += cData(i, k) * m.cData(k, j);
|
||||
}
|
||||
res.Data(i,j) = sum;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename _Scalar>
|
||||
Matrix<_Scalar> Matrix<_Scalar>::operator /(double m) const {
|
||||
Matrix<_Scalar> res(_Rows, _Cols);
|
||||
for(int i = 0; i < _Rows; i++) {
|
||||
for(int j = 0; j < _Cols; j++) {
|
||||
res.Data(i,j) /= m;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
template<typename _Scalar>
|
||||
void Matrix<_Scalar>::normalize() {
|
||||
double sum = 0;
|
||||
for(int i = 0; i < _Rows; i++) {
|
||||
for(int j = 0; j < _Cols; j++) {
|
||||
sum += Matrix::cData(i, j);
|
||||
}
|
||||
}
|
||||
sum = sqrt(sum);
|
||||
for(int i = 0; i < _Rows; i++) {
|
||||
for(int j = 0; j < _Cols; j++) {
|
||||
Matrix::Data(i, j) /= sum;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template<typename _Scalar>
|
||||
double Matrix<_Scalar>::norm() const{
|
||||
double sum = 0;
|
||||
for(int i = 0; i < _Rows; i++) {
|
||||
for(int j = 0; j < _Cols; j++) {
|
||||
sum += Matrix::cData(i, j);
|
||||
}
|
||||
}
|
||||
sum = sqrt(sum);
|
||||
|
||||
return sum;
|
||||
}
|
||||
} //namespace Ctain end
|
||||
#endif //MATRIX_MATRIX_H
|
||||
237
include/Ctain/MatrixSolver.h
Normal file
237
include/Ctain/MatrixSolver.h
Normal file
|
|
@ -0,0 +1,237 @@
|
|||
//
|
||||
// Created by 顾涵彬 on 2019-08-30.
|
||||
//
|
||||
|
||||
#ifndef MATRIX_MATRIXSOLVER_H
|
||||
#define MATRIX_MATRIXSOLVER_H
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,double *Ret);
|
||||
namespace Ctain {
|
||||
class EigenSolver {
|
||||
public:
|
||||
EigenSolver(SMatrix<double> s) {
|
||||
_matrix = s;
|
||||
Matrix<double> tt(_matrix.rows(),2);
|
||||
Matrix_EigenValue(_matrix.addr(),_matrix.rows(),1000,1e-10,tt.addr());
|
||||
t=tt;
|
||||
}
|
||||
Matrix<double> eigenvalues() {
|
||||
return t;
|
||||
}
|
||||
private:
|
||||
SMatrix<double> _matrix;
|
||||
Matrix<double> t;
|
||||
};
|
||||
|
||||
|
||||
} //namespace Ctain end
|
||||
|
||||
static void Matrix_Hessenberg(double *A1,int n,double *ret)
|
||||
{
|
||||
int i,j,k,MaxNumber;
|
||||
double temp,*A;
|
||||
A=new double[n*n];
|
||||
for (i=0;i<n;i++) {
|
||||
k=i*n;
|
||||
for (j=0;j<n;j++)
|
||||
{
|
||||
A[k+j]=A1[k+j];
|
||||
}
|
||||
}
|
||||
for (k=1;k<n-1;k++) {
|
||||
i=k-1;
|
||||
MaxNumber=k;
|
||||
temp=abs(A[k*n+i]);
|
||||
for (j=k+1;j<n;j++) {
|
||||
if (abs(A[j*n+i])>temp) {
|
||||
temp=abs(A[j*n+i]);
|
||||
MaxNumber=j;
|
||||
}
|
||||
}
|
||||
ret[0]=A[MaxNumber*n+i];
|
||||
i=MaxNumber;
|
||||
if (ret[0]!=0) {
|
||||
if (i!=k) {
|
||||
for(j=k-1;j<n;j++) {
|
||||
temp=A[i*n+j];
|
||||
A[i*n+j]=A[k*n+j];
|
||||
A[k*n+j]=temp;
|
||||
}
|
||||
for(j=0;j<n;j++) {
|
||||
temp=A[j*n+i];
|
||||
A[j*n+i]=A[j*n+k];
|
||||
A[j*n+k]=temp;
|
||||
}
|
||||
}
|
||||
for (i=k+1;i<n;i++) {
|
||||
temp=A[i*n+k-1]/ret[0];
|
||||
A[i*n+k-1]=0;
|
||||
for (j=k;j<n;j++) {
|
||||
A[i*n+j]-=temp*A[k*n+j];
|
||||
}
|
||||
for (j=0;j<n;j++) {
|
||||
A[j*n+k]+=temp*A[j*n+i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i=0;i<n;i++) {
|
||||
k=i*n;
|
||||
for (j=0;j<n;j++) {
|
||||
ret[k+j]=A[k+j];
|
||||
}
|
||||
}
|
||||
delete []A;
|
||||
}
|
||||
|
||||
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];
|
||||
Matrix_Hessenberg(K1,n,A);
|
||||
m=n;
|
||||
Loop1=LoopNumber;
|
||||
while(m!=0) {
|
||||
t=m-1;
|
||||
while(t>0) {
|
||||
temp=abs(A[(t-1)*n+t-1]);
|
||||
temp+=abs(A[t*n+t]);
|
||||
temp=temp*Error1;
|
||||
if (abs(A[t*n+t-1])>temp) {
|
||||
t--;
|
||||
}
|
||||
else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (t==m-1) {
|
||||
Ret[(m-1)*2]=A[(m-1)*n+m-1];
|
||||
Ret[(m-1)*2+1]=0;
|
||||
m-=1;
|
||||
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(abs(d));
|
||||
if (d>0) {
|
||||
xy=1;
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
Loop1--;
|
||||
j=t+2;
|
||||
while (j<m) {
|
||||
A[j*n+j-2]=0;
|
||||
j++;
|
||||
}
|
||||
j=t+3;
|
||||
while (j<m) {
|
||||
A[j*n+j-3]=0;
|
||||
j++;
|
||||
}
|
||||
k=t;
|
||||
while (k<m-1) {
|
||||
if (k!=t) {
|
||||
p=A[k*n+k-1];
|
||||
q=A[(k+1)*n+k-1];
|
||||
if (k!=m-2) {
|
||||
r=A[(k+2)*n+k-1];
|
||||
}
|
||||
else {
|
||||
r=0;
|
||||
}
|
||||
}
|
||||
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;
|
||||
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;
|
||||
}
|
||||
s=xy*sqrt(p*p+q*q+r*r);
|
||||
if (k!=t) {
|
||||
A[k*n+k-1]=-s;
|
||||
}
|
||||
e=-q/s;
|
||||
f=-r/s;
|
||||
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;
|
||||
}
|
||||
A[(k+1)*n+j]=q;
|
||||
A[k*n+j]=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++;
|
||||
}
|
||||
}
|
||||
}
|
||||
delete []A;
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif //MATRIX_MATRIXSOLVER_H
|
||||
57
include/Ctain/Quaternion.h
Normal file
57
include/Ctain/Quaternion.h
Normal file
|
|
@ -0,0 +1,57 @@
|
|||
//
|
||||
// Created by 顾涵彬 on 2019-08-30.
|
||||
//
|
||||
|
||||
#ifndef MATRIX_QUATERNION_H
|
||||
#define MATRIX_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;
|
||||
_w = sqrt(m(0)+m(4)+m(8)+1)/2;
|
||||
_x = (m(5)-m(7))/(4*_w);
|
||||
_y = (m(6)-m(2))/(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){}
|
||||
void normalize() {
|
||||
double len;
|
||||
len = sqrt(_x*_x+_y*_y+_z*_z+_w*_w);
|
||||
_x = _x / len;
|
||||
_y = _y / len;
|
||||
_z = _z / len;
|
||||
_w = _w / len;
|
||||
}
|
||||
T x(){return _x;}
|
||||
T y(){return _y;}
|
||||
T z(){return _z;}
|
||||
T w(){return _w;}
|
||||
SMatrix<double> toRotationMatrix() const {
|
||||
SMatrix<double> r(3);
|
||||
double q0=_w,q1=_x,q2=_y,q3=_z;
|
||||
r(0) = 1 - 2*q2*q2-2*q3*q3;
|
||||
r(1) = 2*q1*q2+2*q0*q3;
|
||||
r(2) = 2*q1*q3-2*q0*q2;
|
||||
r(3) = 2*q1*q2 - 2*q0*q3;
|
||||
r(4) = 1-2*q1*q1 - 2*q3*q3;
|
||||
r(5) = 2*q2*q3 +2*q0*q1;
|
||||
r(6) = 2*q1*q3+2*q0*q2;
|
||||
r(7) = 2*q2*q3 - 2*q0*q1;
|
||||
r(8) = 1-2*q1*q1 -2*q2*q2;
|
||||
return r;
|
||||
}
|
||||
private:
|
||||
T _x;
|
||||
T _y;
|
||||
T _z;
|
||||
T _w;
|
||||
};
|
||||
}
|
||||
#endif //MATRIX_QUATERNION_H
|
||||
91
include/Ctain/SquareMatrix.h
Normal file
91
include/Ctain/SquareMatrix.h
Normal file
|
|
@ -0,0 +1,91 @@
|
|||
//
|
||||
// Created by 顾涵彬 on 2019-08-29.
|
||||
//
|
||||
|
||||
#ifndef MATRIX_SQUAREMATRIX_H
|
||||
#define MATRIX_SQUAREMATRIX_H
|
||||
#include "Matrix.h"
|
||||
namespace Ctain {
|
||||
#define Matrix Matrix<_Scalar>
|
||||
template<typename _Scalar>
|
||||
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){
|
||||
// }
|
||||
_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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
};//class Matrix end
|
||||
|
||||
template<typename _Scalar>
|
||||
_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<typename _Scalar>
|
||||
_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 = (float)sign*mid.determinant();
|
||||
return mid_result;
|
||||
}
|
||||
#undef Matrix
|
||||
|
||||
}//namespace Ctain end
|
||||
#endif //MATRIX_SQUAREMATRIX_H
|
||||
|
|
@ -103,11 +103,37 @@ void Camera::estimateExtrinsics(
|
|||
cv::Mat &tvec) const {
|
||||
std::vector<cv::Point2f> Ms(imagePoints.size());
|
||||
for (size_t i = 0; i < Ms.size(); ++i) {
|
||||
Eigen::Vector3d P;
|
||||
liftProjective(
|
||||
Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);
|
||||
Eigen::Vector3d P;
|
||||
|
||||
P /= P(2);
|
||||
liftProjective(
|
||||
Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);
|
||||
|
||||
P = P / P(2);
|
||||
|
||||
Ms.at(i).x = P(0);
|
||||
Ms.at(i).y = P(1);
|
||||
}
|
||||
|
||||
// assume unit focal length, zero principal point, and zero distortion
|
||||
cv::solvePnP(
|
||||
objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);
|
||||
}
|
||||
|
||||
//subEigen
|
||||
void Camera::estimateExtrinsics2(
|
||||
const std::vector<cv::Point3f> &objectPoints,
|
||||
const std::vector<cv::Point2f> &imagePoints, cv::Mat &rvec,
|
||||
cv::Mat &tvec) const {
|
||||
std::vector<cv::Point2f> Ms(imagePoints.size());
|
||||
for (size_t i = 0; i < Ms.size(); ++i) {
|
||||
// Eigen::Vector3d P;
|
||||
Ctain::Vectord P(3, 1), p(2, 1);
|
||||
p<< imagePoints.at(i).x << imagePoints.at(i).y;
|
||||
|
||||
// liftProjective(
|
||||
// Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);
|
||||
liftProjective(p, P);
|
||||
P = P / P(2);
|
||||
|
||||
Ms.at(i).x = P(0);
|
||||
Ms.at(i).y = P(1);
|
||||
|
|
@ -128,6 +154,17 @@ double Camera::reprojectionDist(
|
|||
return (p1 - p2).norm();
|
||||
}
|
||||
|
||||
//subEigen
|
||||
double Camera::reprojectionDist(
|
||||
const Ctain::Vectord &P1, const Ctain::Vectord &P2) const {
|
||||
Ctain::Vectord p1(2, 1), p2(2, 1);
|
||||
|
||||
spaceToPlane(P1, p1);
|
||||
spaceToPlane(P2, p2);
|
||||
|
||||
return (p1 - p2).norm();
|
||||
}
|
||||
|
||||
double Camera::reprojectionError(
|
||||
const std::vector<std::vector<cv::Point3f> > &objectPoints,
|
||||
const std::vector<std::vector<cv::Point2f> > &imagePoints,
|
||||
|
|
@ -178,6 +215,19 @@ double Camera::reprojectionError(
|
|||
return (p - observed_p).norm();
|
||||
}
|
||||
|
||||
//subEigen
|
||||
double Camera::reprojectionError(
|
||||
const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q,
|
||||
const Ctain::Vector3d &camera_t, const Ctain::Vector2d &observed_p) const {
|
||||
Ctain::Vector3d P_cam;
|
||||
P_cam = camera_q.toRotationMatrix() * P + camera_t;
|
||||
|
||||
Ctain::Vector2d p, res;
|
||||
spaceToPlane(P_cam, p);
|
||||
res = p - observed_p;
|
||||
return res.norm();
|
||||
}
|
||||
|
||||
void Camera::projectPoints(
|
||||
const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
|
||||
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const {
|
||||
|
|
@ -212,6 +262,41 @@ void Camera::projectPoints(
|
|||
}
|
||||
}
|
||||
|
||||
//subEigen
|
||||
void Camera::projectPoints2(
|
||||
const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
|
||||
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const {
|
||||
// project 3D object points to the image plane
|
||||
imagePoints.reserve(objectPoints.size());
|
||||
|
||||
// double
|
||||
cv::Mat R0;
|
||||
cv::Rodrigues(rvec, R0);
|
||||
|
||||
Ctain::MatrixXd R(3, 3);
|
||||
R << R0.at<double>(0, 0) << R0.at<double>(0, 1) << R0.at<double>(0, 2) <<
|
||||
R0.at<double>(1, 0) << R0.at<double>(1, 1) << R0.at<double>(1, 2) <<
|
||||
R0.at<double>(2, 0) << R0.at<double>(2, 1) << R0.at<double>(2, 2);
|
||||
|
||||
Ctain::Vectord t(3, 1);
|
||||
t << tvec.at<double>(0) << tvec.at<double>(1) << tvec.at<double>(2);
|
||||
|
||||
for (size_t i = 0; i < objectPoints.size(); ++i) {
|
||||
const cv::Point3f &objectPoint = objectPoints.at(i);
|
||||
|
||||
// Rotate and translate
|
||||
Ctain::Vectord P(3, 1);
|
||||
P << objectPoint.x << objectPoint.y << objectPoint.z;
|
||||
|
||||
P = R * P + t;
|
||||
|
||||
Ctain::Vector2d p;
|
||||
spaceToPlane(P, p);
|
||||
|
||||
imagePoints.push_back(cv::Point2f(p(0), p(1)));
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace models
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
#include <vector>
|
||||
#include <memory>
|
||||
#include "eigen3/Eigen/Dense"
|
||||
#include "Ctain/CtainBase.h"
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
#include "mynteye/mynteye.h"
|
||||
|
|
@ -70,21 +71,45 @@ class Camera {
|
|||
const cv::Size &boardSize,
|
||||
const std::vector<std::vector<cv::Point3f> > &objectPoints,
|
||||
const std::vector<std::vector<cv::Point2f> > &imagePoints) = 0;
|
||||
|
||||
// subEigen
|
||||
virtual void estimateIntrinsics2(
|
||||
const cv::Size &boardSize,
|
||||
const std::vector<std::vector<cv::Point3f> > &objectPoints,
|
||||
const std::vector<std::vector<cv::Point2f> > &imagePoints) = 0;
|
||||
|
||||
virtual void estimateExtrinsics(
|
||||
const std::vector<cv::Point3f> &objectPoints,
|
||||
const std::vector<cv::Point2f> &imagePoints,
|
||||
cv::Mat &rvec, cv::Mat &tvec) const; // NOLINT
|
||||
|
||||
virtual void estimateExtrinsics2(
|
||||
const std::vector<cv::Point3f> &objectPoints,
|
||||
const std::vector<cv::Point2f> &imagePoints,
|
||||
cv::Mat &rvec, cv::Mat &tvec) const; // NOLINT
|
||||
|
||||
// Lift points from the image plane to the projective space
|
||||
virtual void liftProjective(
|
||||
const Eigen::Vector2d &p, Eigen::Vector3d &P) const = 0; // NOLINT
|
||||
// %output P
|
||||
|
||||
//subEigen
|
||||
// Lift points from the image plane to the projective space
|
||||
virtual void liftProjective(
|
||||
const Ctain::Vector2d &p, Ctain::Vector3d &P) const = 0; // NOLINT
|
||||
// %output P
|
||||
|
||||
// Projects 3D points to the image plane (Pi function)
|
||||
virtual void spaceToPlane(
|
||||
const Eigen::Vector3d &P, Eigen::Vector2d &p) const = 0; // NOLINT
|
||||
// %output p
|
||||
|
||||
//subEigen
|
||||
// Projects 3D points to the image plane (Pi function)
|
||||
virtual void spaceToPlane(
|
||||
const Ctain::Vector3d &P, Ctain::Vector2d &p) const = 0; // NOLINT
|
||||
// %output p
|
||||
|
||||
// Projects 3D points to the image plane (Pi function)
|
||||
// and calculates jacobian
|
||||
// virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
|
||||
|
|
@ -116,20 +141,36 @@ class Camera {
|
|||
double reprojectionDist(
|
||||
const Eigen::Vector3d &P1, const Eigen::Vector3d &P2) const;
|
||||
|
||||
//subEigen
|
||||
double reprojectionDist(
|
||||
const Ctain::Vector3d &P1, const Ctain::Vector2d &P2) const;
|
||||
|
||||
double reprojectionError(
|
||||
const std::vector<std::vector<cv::Point3f> > &objectPoints,
|
||||
const std::vector<std::vector<cv::Point2f> > &imagePoints,
|
||||
const std::vector<cv::Mat> &rvecs, const std::vector<cv::Mat> &tvecs,
|
||||
cv::OutputArray perViewErrors = cv::noArray()) const;
|
||||
|
||||
|
||||
double reprojectionError(
|
||||
const Eigen::Vector3d &P, const Eigen::Quaterniond &camera_q,
|
||||
const Eigen::Vector3d &camera_t, const Eigen::Vector2d &observed_p) const;
|
||||
|
||||
//subEigen
|
||||
double reprojectionError(
|
||||
const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q,
|
||||
const Ctain::Vector3d &camera_t, const Ctain::Vector2d &observed_p) const;
|
||||
|
||||
|
||||
void projectPoints(
|
||||
const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
|
||||
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const; // NOLINT
|
||||
|
||||
//subEigen
|
||||
void projectPoints2(
|
||||
const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
|
||||
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const; // NOLINT
|
||||
|
||||
protected:
|
||||
cv::Mat m_mask;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -342,6 +342,102 @@ void EquidistantCamera::estimateIntrinsics(
|
|||
setParameters(params);
|
||||
}
|
||||
|
||||
// subEigen
|
||||
void EquidistantCamera::estimateIntrinsics2(
|
||||
const cv::Size &boardSize,
|
||||
const std::vector<std::vector<cv::Point3f> > &objectPoints,
|
||||
const std::vector<std::vector<cv::Point2f> > &imagePoints) {
|
||||
Parameters params = getParameters();
|
||||
|
||||
double u0 = params.imageWidth() / 2.0;
|
||||
double v0 = params.imageHeight() / 2.0;
|
||||
|
||||
double minReprojErr = std::numeric_limits<double>::max();
|
||||
|
||||
std::vector<cv::Mat> rvecs, tvecs;
|
||||
rvecs.assign(objectPoints.size(), cv::Mat());
|
||||
tvecs.assign(objectPoints.size(), cv::Mat());
|
||||
|
||||
params.k2() = 0.0;
|
||||
params.k3() = 0.0;
|
||||
params.k4() = 0.0;
|
||||
params.k5() = 0.0;
|
||||
params.u0() = u0;
|
||||
params.v0() = v0;
|
||||
|
||||
// Initialize focal length
|
||||
// C. Hughes, P. Denny, M. Glavin, and E. Jones,
|
||||
// Equidistant Fish-Eye Calibration and Rectification by Vanishing Point
|
||||
// Extraction, PAMI 2010
|
||||
// Find circles from rows of chessboard corners, and for each pair
|
||||
// of circles, find vanishing points: v1 and v2.
|
||||
// f = ||v1 - v2|| / PI;
|
||||
double f0 = 0.0;
|
||||
for (size_t i = 0; i < imagePoints.size(); ++i) {
|
||||
//std::vector<Eigen::Vector2d> center(boardSize.height);
|
||||
std::vector<Ctain::Vector2d> center(boardSize.height);
|
||||
int arrayLength = boardSize.height;
|
||||
double *radius = new double[arrayLength];
|
||||
for (int r = 0; r < boardSize.height; ++r) {
|
||||
std::vector<cv::Point2d> circle;
|
||||
for (int c = 0; c < boardSize.width; ++c) {
|
||||
circle.push_back(imagePoints.at(i).at(r * boardSize.width + c));
|
||||
}
|
||||
|
||||
fitCircle(circle, center[r](0), center[r](1), radius[r]);
|
||||
}
|
||||
|
||||
for (int j = 0; j < boardSize.height; ++j) {
|
||||
for (int k = j + 1; k < boardSize.height; ++k) {
|
||||
// find distance between pair of vanishing points which
|
||||
// correspond to intersection points of 2 circles
|
||||
std::vector<cv::Point2d> ipts;
|
||||
ipts = intersectCircles(
|
||||
center[j](0), center[j](1), radius[j], center[k](0), center[k](1),
|
||||
radius[k]);
|
||||
|
||||
if (ipts.size() < 2) {
|
||||
continue;
|
||||
}
|
||||
|
||||
double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;
|
||||
|
||||
params.mu() = f;
|
||||
params.mv() = f;
|
||||
|
||||
setParameters(params);
|
||||
|
||||
for (size_t l = 0; l < objectPoints.size(); ++l) {
|
||||
estimateExtrinsics(
|
||||
objectPoints.at(l), imagePoints.at(l), rvecs.at(l), tvecs.at(l));
|
||||
}
|
||||
|
||||
double reprojErr = reprojectionError(
|
||||
objectPoints, imagePoints, rvecs, tvecs, cv::noArray());
|
||||
|
||||
if (reprojErr < minReprojErr) {
|
||||
minReprojErr = reprojErr;
|
||||
f0 = f;
|
||||
}
|
||||
}
|
||||
}
|
||||
delete[] radius;
|
||||
}
|
||||
|
||||
if (f0 <= 0.0 && minReprojErr >= std::numeric_limits<double>::max()) {
|
||||
std::cout << "[" << params.cameraName() << "] "
|
||||
<< "# INFO: kannala-Brandt model fails with given data. "
|
||||
<< std::endl;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
params.mu() = f0;
|
||||
params.mv() = f0;
|
||||
|
||||
setParameters(params);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Lifts a point from the image plane to its projective ray
|
||||
*
|
||||
|
|
@ -363,6 +459,22 @@ void EquidistantCamera::liftProjective(
|
|||
P(2) = cos(theta);
|
||||
}
|
||||
|
||||
// subEigen
|
||||
void EquidistantCamera::liftProjective(
|
||||
const Ctain::Vector2d &p, Ctain::Vector3d &P) const {
|
||||
// Lift points to normalised plane
|
||||
Ctain::Vector2d p_u(2, 1);
|
||||
p_u << m_inv_K11 * p(0) + m_inv_K13 << m_inv_K22 * p(1) + m_inv_K23;
|
||||
|
||||
// Obtain a projective ray
|
||||
double theta, phi;
|
||||
backprojectSymmetric(p_u, theta, phi);
|
||||
|
||||
P(0) = sin(theta) * cos(phi);
|
||||
P(1) = sin(theta) * sin(phi);
|
||||
P(2) = cos(theta);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
|
||||
*
|
||||
|
|
@ -390,6 +502,31 @@ void EquidistantCamera::spaceToPlane(
|
|||
mParameters.mv() * p_u(1) + mParameters.v0();
|
||||
}
|
||||
|
||||
// subEigen
|
||||
void EquidistantCamera::spaceToPlane(
|
||||
const Ctain::Vector3d &P, Ctain::Vector2d &p) const {
|
||||
// double theta = acos(0.5);
|
||||
// double theta = 0.5;
|
||||
// double phi = 0.5;
|
||||
// Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
|
||||
// mParameters.k5(), theta) *
|
||||
// Eigen::Vector2d(cos(0.5), sin(0.5));
|
||||
|
||||
|
||||
double theta = acos(P(2) / P.norm());
|
||||
double phi = atan2(P(1), P(0));
|
||||
// double phi = ApproxAtan2(P(1), P(0));
|
||||
|
||||
double tmp[2] = {cos(phi), sin(phi)};
|
||||
Ctain::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
|
||||
mParameters.k5(), theta) *
|
||||
Ctain::Vector2d(tmp, 2, 1);
|
||||
|
||||
// Apply generalised projection matrix
|
||||
p << mParameters.mu() * p_u(0) + mParameters.u0()
|
||||
<< mParameters.mv() * p_u(1) + mParameters.v0();
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Project a 3D point to the image plane and calculate Jacobian
|
||||
*
|
||||
|
|
@ -411,6 +548,22 @@ void EquidistantCamera::spaceToPlane(
|
|||
mParameters.mv() * p_u(1) + mParameters.v0();
|
||||
}
|
||||
|
||||
// subEigen
|
||||
void EquidistantCamera::spaceToPlane(
|
||||
const Ctain::Vector3d &P, Ctain::Vector2d &p,
|
||||
Ctain::Matrix23d &J) const {
|
||||
double theta = acos(P(2) / 3.0);
|
||||
double phi = atan2(P(1), P(0));
|
||||
double tmp[2] = {cos(phi), sin(phi)};
|
||||
Ctain::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
|
||||
mParameters.k5(), theta) *
|
||||
Ctain::Vector2d(tmp, 2, 1);
|
||||
|
||||
// Apply generalised projection matrix
|
||||
p << mParameters.mu() * p_u(0) + mParameters.u0()
|
||||
<< mParameters.mv() * p_u(1) + mParameters.v0();
|
||||
}
|
||||
|
||||
void EquidistantCamera::initUndistortMap(
|
||||
cv::Mat &map1, cv::Mat &map2, double fScale) const {
|
||||
cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());
|
||||
|
|
@ -440,6 +593,36 @@ void EquidistantCamera::initUndistortMap(
|
|||
cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
|
||||
}
|
||||
|
||||
// subEigen
|
||||
void EquidistantCamera::initUndistortMap2(
|
||||
cv::Mat &map1, cv::Mat &map2, double fScale) const {
|
||||
cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());
|
||||
|
||||
cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);
|
||||
cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);
|
||||
|
||||
for (int v = 0; v < imageSize.height; ++v) {
|
||||
for (int u = 0; u < imageSize.width; ++u) {
|
||||
double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;
|
||||
double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;
|
||||
|
||||
double theta, phi;
|
||||
backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi);
|
||||
|
||||
Ctain::Vector3d P(3, 1);
|
||||
P << sin(theta) * cos(phi) << sin(theta) * sin(phi) << cos(theta);
|
||||
|
||||
Ctain::Vector2d p(2, 1);
|
||||
spaceToPlane(P, p);
|
||||
|
||||
mapX.at<float>(v, u) = p(0);
|
||||
mapY.at<float>(v, u) = p(1);
|
||||
}
|
||||
}
|
||||
|
||||
cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
|
||||
}
|
||||
|
||||
cv::Mat EquidistantCamera::initUndistortRectifyMap(
|
||||
cv::Mat &map1, cv::Mat &map2, float fx, float fy, cv::Size imageSize,
|
||||
float cx, float cy, cv::Mat rmat) const {
|
||||
|
|
@ -491,6 +674,68 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap(
|
|||
return K_rect_cv;
|
||||
}
|
||||
|
||||
|
||||
// subEigen
|
||||
cv::Mat EquidistantCamera::initUndistortRectifyMap2(
|
||||
cv::Mat &map1, cv::Mat &map2, float fx, float fy, cv::Size imageSize,
|
||||
float cx, float cy, cv::Mat rmat) const {
|
||||
if (imageSize == cv::Size(0, 0)) {
|
||||
imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());
|
||||
}
|
||||
|
||||
cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
|
||||
cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
|
||||
|
||||
Ctain::Matrix3f K_rect(3);
|
||||
|
||||
if (cx == -1.0f && cy == -1.0f) {
|
||||
K_rect << fx << 0 << imageSize.width / 2 << 0 << fy << imageSize.height / 2 << 0 << 0 << 1;
|
||||
} else {
|
||||
K_rect << fx << 0 << cx << 0 << fy << cy << 0 << 0 << 1;
|
||||
}
|
||||
|
||||
if (fx == -1.0f || fy == -1.0f) {
|
||||
K_rect(0, 0) = mParameters.mu();
|
||||
K_rect(1, 1) = mParameters.mv();
|
||||
}
|
||||
|
||||
Ctain::Matrix3f K_rect_inv = K_rect.inverse();
|
||||
|
||||
Ctain::Matrix3f R(3), R_inv(3);
|
||||
|
||||
for(int i = 0; i < 3; ++i) {
|
||||
for(int j = 0; j < 3; ++j) {
|
||||
R(i, j) = rmat.at<float>(i, j);
|
||||
}
|
||||
}
|
||||
R_inv = R.inverse();
|
||||
|
||||
for (int v = 0; v < imageSize.height; ++v) {
|
||||
for (int u = 0; u < imageSize.width; ++u) {
|
||||
Ctain::Vector3f xo(3, 1);
|
||||
xo << u << v << 1;
|
||||
|
||||
Ctain::Vector3f uo = R_inv * K_rect_inv * xo;
|
||||
|
||||
Ctain::Vector2d p;
|
||||
spaceToPlane(uo.cast<double>(), p);
|
||||
|
||||
mapX.at<float>(v, u) = p(0);
|
||||
mapY.at<float>(v, u) = p(1);
|
||||
}
|
||||
}
|
||||
|
||||
cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
|
||||
|
||||
cv::Mat K_rect_cv(3, 3, CV_32FC1);
|
||||
for(int i = 0; i < 3; ++i) {
|
||||
for(int j = 0; j < 3; ++j) {
|
||||
K_rect_cv.at<float>(i, j) = K_rect(i, j);
|
||||
}
|
||||
}
|
||||
return K_rect_cv;
|
||||
}
|
||||
|
||||
int EquidistantCamera::parameterCount(void) const {
|
||||
return 8;
|
||||
}
|
||||
|
|
@ -576,6 +821,33 @@ void EquidistantCamera::fitOddPoly(
|
|||
}
|
||||
}
|
||||
|
||||
// subEigen
|
||||
void EquidistantCamera::fitOddPoly2(
|
||||
const std::vector<double> &x, const std::vector<double> &y, int n,
|
||||
std::vector<double> &coeffs) const {
|
||||
std::vector<int> pows;
|
||||
for (int i = 1; i <= n; i += 2) {
|
||||
pows.push_back(i);
|
||||
}
|
||||
|
||||
Ctain::MatrixXd X(x.size(), pows.size());
|
||||
Ctain::MatrixXd Y(y.size(), 1);
|
||||
for (size_t i = 0; i < x.size(); ++i) {
|
||||
for (size_t j = 0; j < pows.size(); ++j) {
|
||||
X(i, j) = pow(x.at(i), pows.at(j));
|
||||
}
|
||||
Y(i, 0) = y.at(i);
|
||||
}
|
||||
Ctain::SMatrix<double> Tmp;
|
||||
Tmp = X.transpose() * X;
|
||||
Ctain::MatrixXd A = Tmp.inverse() * X.transpose() * Y;
|
||||
|
||||
coeffs.resize(A.rows());
|
||||
for (int i = 0; i < A.rows(); ++i) {
|
||||
coeffs.at(i) = A(i, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void EquidistantCamera::backprojectSymmetric(
|
||||
const Eigen::Vector2d &p_u, double &theta, double &phi) const {
|
||||
double tol = 1e-10;
|
||||
|
|
@ -657,6 +929,89 @@ void EquidistantCamera::backprojectSymmetric(
|
|||
}
|
||||
}
|
||||
|
||||
// subEigen
|
||||
void EquidistantCamera::backprojectSymmetric(
|
||||
const Ctain::Vector2d &p_u, double &theta, double &phi) const {
|
||||
double tol = 1e-10;
|
||||
double p_u_norm = p_u.norm();
|
||||
|
||||
if (p_u_norm < 1e-10) {
|
||||
phi = 0.0;
|
||||
} else {
|
||||
phi = atan2(p_u(1), p_u(0));
|
||||
}
|
||||
|
||||
int npow = 9;
|
||||
if (mParameters.k5() == 0.0) {
|
||||
npow -= 2;
|
||||
}
|
||||
if (mParameters.k4() == 0.0) {
|
||||
npow -= 2;
|
||||
}
|
||||
if (mParameters.k3() == 0.0) {
|
||||
npow -= 2;
|
||||
}
|
||||
if (mParameters.k2() == 0.0) {
|
||||
npow -= 2;
|
||||
}
|
||||
|
||||
Ctain::MatrixXd coeffs(npow + 1, 1);
|
||||
coeffs.setZero();
|
||||
coeffs(0) = -p_u_norm;
|
||||
coeffs(1) = 1.0;
|
||||
|
||||
if (npow >= 3) {
|
||||
coeffs(3) = mParameters.k2();
|
||||
}
|
||||
if (npow >= 5) {
|
||||
coeffs(5) = mParameters.k3();
|
||||
}
|
||||
if (npow >= 7) {
|
||||
coeffs(7) = mParameters.k4();
|
||||
}
|
||||
if (npow >= 9) {
|
||||
coeffs(9) = mParameters.k5();
|
||||
}
|
||||
|
||||
if (npow == 1) {
|
||||
theta = p_u_norm;
|
||||
} else {
|
||||
// Get eigenvalues of companion matrix corresponding to polynomial.
|
||||
// Eigenvalues correspond to roots of polynomial.
|
||||
Ctain::MatrixXd A(npow, npow);
|
||||
A.setZero();
|
||||
A.block(1, 0, npow - 1, npow - 1).setIdentity();
|
||||
A.col(npow - 1) = -coeffs.block(0, 0, npow, 1) / coeffs(npow);
|
||||
|
||||
Ctain::EigenSolver es(A);
|
||||
Ctain::MatrixXcd eigval = es.eigenvalues();
|
||||
|
||||
std::vector<double> thetas;
|
||||
for (int i = 0; i < eigval.rows(); ++i) {
|
||||
if (fabs(eigval(i, 1)) > tol) { //imag
|
||||
continue;
|
||||
}
|
||||
|
||||
double t = eigval(i, 0); //real
|
||||
|
||||
if (t < -tol) {
|
||||
continue;
|
||||
} else if (t < 0.0) {
|
||||
t = 0.0;
|
||||
}
|
||||
|
||||
thetas.push_back(t);
|
||||
}
|
||||
|
||||
if (thetas.empty()) {
|
||||
theta = p_u_norm;
|
||||
} else {
|
||||
theta = *std::min_element(thetas.begin(), thetas.end());
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
} // namespace models
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
|
|
|||
|
|
@ -115,20 +115,45 @@ class EquidistantCamera : public Camera {
|
|||
const std::vector<std::vector<cv::Point3f> > &objectPoints,
|
||||
const std::vector<std::vector<cv::Point2f> > &imagePoints);
|
||||
|
||||
//subEigen
|
||||
void estimateIntrinsics2(
|
||||
const cv::Size &boardSize,
|
||||
const std::vector<std::vector<cv::Point3f> > &objectPoints,
|
||||
const std::vector<std::vector<cv::Point2f> > &imagePoints);
|
||||
|
||||
// Lift points from the image plane to the projective space
|
||||
void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const; // NOLINT
|
||||
// %output P
|
||||
|
||||
//subEigen
|
||||
// Lift points from the image plane to the projective space
|
||||
void liftProjective(const Ctain::Vector2d &p, Ctain::Vector3d &P) const; // NOLINT
|
||||
// %output P
|
||||
|
||||
// Projects 3D points to the image plane (Pi function)
|
||||
void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const; // NOLINT
|
||||
// %output p
|
||||
|
||||
//subEigen
|
||||
// Projects 3D points to the image plane (Pi function)
|
||||
void spaceToPlane(const Ctain::Vector3d &P, Ctain::Vector2d &p) const; // NOLINT
|
||||
// %output p
|
||||
|
||||
// Projects 3D points to the image plane (Pi function)
|
||||
// and calculates jacobian
|
||||
void spaceToPlane(
|
||||
const Eigen::Vector3d &P, Eigen::Vector2d &p, // NOLINT
|
||||
Eigen::Matrix<double, 2, 3> &J) const; // NOLINT
|
||||
// %output p
|
||||
// %output J
|
||||
|
||||
// subEigen
|
||||
// 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; // NOLINT
|
||||
// %output p
|
||||
// %output J
|
||||
|
||||
template <typename T>
|
||||
|
|
@ -136,13 +161,29 @@ class EquidistantCamera : public Camera {
|
|||
const T *const params, const T *const q, const T *const t,
|
||||
const Eigen::Matrix<T, 3, 1> &P, Eigen::Matrix<T, 2, 1> &p); // NOLINT
|
||||
|
||||
// subEigen
|
||||
template <typename T>
|
||||
static void spaceToPlane(
|
||||
const T *const params, const T *const q, const T *const t,
|
||||
const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p); // NOLINT
|
||||
|
||||
void initUndistortMap(
|
||||
cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const; // NOLINT
|
||||
|
||||
cv::Mat initUndistortRectifyMap(
|
||||
cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f, // NOLINT
|
||||
cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f,
|
||||
cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
|
||||
|
||||
// subEigen
|
||||
void initUndistortMap2(
|
||||
cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const; // NOLINT
|
||||
|
||||
cv::Mat initUndistortRectifyMap2(
|
||||
cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f, // NOLINT
|
||||
cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f,
|
||||
cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
|
||||
|
||||
int parameterCount(void) const;
|
||||
|
||||
const Parameters &getParameters(void) const;
|
||||
|
|
@ -161,9 +202,18 @@ class EquidistantCamera : public Camera {
|
|||
const std::vector<double> &x, const std::vector<double> &y, int n,
|
||||
std::vector<double> &coeffs) const; // NOLINT
|
||||
|
||||
// subEigen
|
||||
void fitOddPoly2(
|
||||
const std::vector<double> &x, const std::vector<double> &y, int n,
|
||||
std::vector<double> &coeffs) const;
|
||||
|
||||
void backprojectSymmetric(
|
||||
const Eigen::Vector2d &p_u, double &theta, double &phi) const; // NOLINT
|
||||
|
||||
// subEigen
|
||||
void backprojectSymmetric(
|
||||
const Ctain::Vector2d &p_u, double &theta, double &phi) const; // NOLINT
|
||||
|
||||
Parameters mParameters;
|
||||
|
||||
double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
|
||||
|
|
@ -228,8 +278,56 @@ void EquidistantCamera::spaceToPlane(
|
|||
p(0) = mu * p_u(0) + u0;
|
||||
p(1) = mv * p_u(1) + v0;
|
||||
}
|
||||
|
||||
// subEigen
|
||||
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) {
|
||||
T P_w[3];
|
||||
P_w[0] = T(P(0));
|
||||
P_w[1] = T(P(1));
|
||||
P_w[2] = T(P(2));
|
||||
|
||||
// Convert quaternion from Eigen convention (x, y, z, w)
|
||||
// to Ceres convention (w, x, y, z)
|
||||
T q_ceres[4] = {q[3], q[0], q[1], q[2]};
|
||||
|
||||
T P_c[3];
|
||||
QuaternionRotatePoint(q_ceres, P_w, P_c);
|
||||
|
||||
P_c[0] += t[0];
|
||||
P_c[1] += t[1];
|
||||
P_c[2] += t[2];
|
||||
|
||||
// project 3D object point to the image plane;
|
||||
T k2 = params[0];
|
||||
T k3 = params[1];
|
||||
T k4 = params[2];
|
||||
T k5 = params[3];
|
||||
T mu = params[4];
|
||||
T mv = params[5];
|
||||
T u0 = params[6];
|
||||
T v0 = params[7];
|
||||
|
||||
T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);
|
||||
T theta = acos(P_c[2] / len);
|
||||
T phi = atan2(P_c[1], P_c[0]);
|
||||
|
||||
Ctain::Matrix<T> p_u(2, 1), tmp(2, 1);
|
||||
tmp(0) = cos(phi);
|
||||
tmp(1) = sin(phi);
|
||||
p_u = r(k2, k3, k4, k5, theta) * tmp;
|
||||
|
||||
p(0) = mu * p_u(0) + u0;
|
||||
p(1) = mv * p_u(1) + v0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
||||
#endif // MYNTEYE_CAMERA_MODELS_EQUIDISTANT_CAMERA_H_
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user