refactor(src/api/camera_models):add independent Matrix functions to camera_models

include/Ctain/CtainBase.h

@@ -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
+
+

include/Ctain/Matrix.h

@@ -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

include/Ctain/MatrixSolver.h

@@ -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

include/Ctain/Quaternion.h

@@ -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

include/Ctain/SquareMatrix.h

@@ -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

src/mynteye/api/camera_models/camera.cc

@@ -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;
+  Eigen::Vector3d P;
-    liftProjective(
-        Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), 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

src/mynteye/api/camera_models/camera.h

@@ -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;
 };

src/mynteye/api/camera_models/equidistant_camera.cc

@@ -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

src/mynteye/api/camera_models/equidistant_camera.h

@@ -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_