diff --git a/src/mynteye/api/camera_models/CtainBase.h b/src/mynteye/api/camera_models/CtainBase.h
index 89f7f0a..2fd040d 100644
--- a/src/mynteye/api/camera_models/CtainBase.h
+++ b/src/mynteye/api/camera_models/CtainBase.h
@@ -1,8 +1,17 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
 //
-// Created by 顾涵彬 on 2019-08-28.
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
 //
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
 
-#include <cstdint>
 #include "Matrix.h"
 #include "SquareMatrix.h"
 #include "MatrixSolver.h"
@@ -12,28 +21,26 @@
 
 
 namespace Ctain {
-    typedef SMatrix<double> Matrixd;
-    typedef Matrix<double> MatrixXd;
-    typedef Matrix<double> Matrix23d;
-    typedef SMatrix<double> Matrix3d;
-    typedef SMatrix<double> Matrix4d;
+  typedef SMatrix<double> Matrixd;
+  typedef Matrix<double> MatrixXd;
+  typedef Matrix<double> Matrix23d;
+  typedef SMatrix<double> Matrix3d;
+  typedef SMatrix<double> Matrix4d;
 
-    typedef SMatrix<float> Matrixf;
-    typedef Matrixf Matrix2f;
-    typedef Matrixf Matrix3f;
+  typedef SMatrix<float> Matrixf;
+  typedef Matrixf Matrix2f;
+  typedef Matrixf Matrix3f;
 
-    typedef Matrix<float> Vectorf;
-    typedef Vectorf Vector2f;
-    typedef Vectorf Vector3f;
+  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 Matrix<double> Vectord;
+  typedef Matrix<double> Vector2d;
+  typedef Matrix<double> Vector3d;
+  typedef Matrix<double> MatrixXcd;
 
-    typedef Quaternion<double> Quaterniond;
-} // end namespace Ctain
+  typedef Quaternion<double> Quaterniond;
+}
 
 #endif // Ctain_CtainBASE_H
-
-
diff --git a/src/mynteye/api/camera_models/Matrix.h b/src/mynteye/api/camera_models/Matrix.h
index 598674b..28bca00 100644
--- a/src/mynteye/api/camera_models/Matrix.h
+++ b/src/mynteye/api/camera_models/Matrix.h
@@ -1,415 +1,421 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
 //
-// Created by 顾涵彬 on 2019-08-28.
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
 //
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
 
-#ifndef MATRIX_MATRIX_H
-#define MATRIX_MATRIX_H
+#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_
+#define SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_
 
 #include <cstring>
 #include <iostream>
 #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));
-        }
+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, double a) {
-            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(0, _Cols-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) {
-            if(_isSub) {
-                for (int i = 0; i < m._Rows; i++) {
-                    for (int j = 0; j < m._Cols; j++) {
-                        Data(i,j) = m.cData(i, j);
-                    }
-                }
-                return;
-            }
-            _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);
-            }
-        }
+  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 _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 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;
+  }
 
-    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];
+  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;
         }
-        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;
+  }
+  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; }
 
-    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++) {
-                _Scalar sum = 0;
-                for (int k = 0; k < _Cols; k++) {
-                    sum += cData(i, k) * m.cData(k, j);
-                }
-                res.Data(i,j) = sum;
-            }
-        }
-        return res;
+  inline int rows() const { return _Rows; }
+
+  inline int size() const { return cols() * rows(); }
+  inline _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) {
+    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;
+  }
 
-    template<typename _Scalar>
-    Matrix<_Scalar> Matrix<_Scalar>::operator /(double m) const {
-        Matrix<_Scalar> res;
-        res = *this;
-        for (int i = 0; i < _Rows; i++) {
-            for (int j = 0; j < _Cols; j++) {
-                res.Data(i,j) /= m;
-            }
-        }
-        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;
+  }
 
-
-    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;
-            }
-        }
+  friend Matrix<_Scalar> operator *(
+      const Matrix<_Scalar> &m, double a) {
+    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;
+  }
 
-    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) * Matrix::cData(i, j);
-            }
-        }
-        sum = sqrt(sum);
-
-        return sum;
+  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;
+      }
     }
-} //namespace Ctain end
-#endif //MATRIX_MATRIX_H
\ No newline at end of file
+    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;
+  }
+
+  virtual void operator =(Matrix<_Scalar> m);
+  Matrix<_Scalar> operator +(const Matrix<_Scalar> &m) const;
+  Matrix<_Scalar> operator -(const Matrix<_Scalar> &m) const;
+  Matrix<_Scalar> operator *(const Matrix<_Scalar> &m) const;
+
+  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() {
+    Matrix<_Scalar> sub;
+    sub = *this;
+    sub.setSub(0, 0, Rows, Cols, data);
+    return sub;
+  }
+
+  template<int Rows, int Cols>
+  Matrix<_Scalar> topRightCorner() {
+    Matrix<_Scalar> sub;
+    sub = *this;
+    sub.setSub(0, _Cols-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;
+  }
+
+  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
+
+template<typename _Scalar>
+void Matrix<_Scalar>::operator =(Matrix<_Scalar> m) {
+  if (m._isSub) {
+    if (_isSub) {
+      for (int i = 0; i < m._Rows; i++) {
+        for (int j = 0; j < m._Cols; j++) {
+          Data(i, j) = m.cData(i, j);
+        }
+      }
+      return;
+    }
+    _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++) {
+      _Scalar 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;
+  res = *this;
+  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) * Matrix::cData(i, j);
+    }
+  }
+  sum = sqrt(sum);
+
+  return sum;
+}
+}   //  namespace Ctain
+#endif  //  SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_
diff --git a/src/mynteye/api/camera_models/MatrixSolver.h b/src/mynteye/api/camera_models/MatrixSolver.h
index 205ba56..9254fc4 100644
--- a/src/mynteye/api/camera_models/MatrixSolver.h
+++ b/src/mynteye/api/camera_models/MatrixSolver.h
@@ -2,249 +2,241 @@
 // Created by 顾涵彬 on 2019-08-30.
 //
 
-#ifndef MATRIX_MATRIXSOLVER_H
-#define MATRIX_MATRIXSOLVER_H
+#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
+#define SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
 #include <cmath>
 #include <complex>
-static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,double *Ret);
-static void Matrix_Hessenberg(double *A1,int n,double *ret);
+
+static bool Matrix_EigenValue(double *K1, int n,
+    int LoopNumber, double Error1, double *Ret);
+static void Matrix_Hessenberg(double *A1, int n, double *ret);
+
 namespace Ctain {
-    class EigenSolver {
-    public:
-        EigenSolver(SMatrix<double> s) {
-            double *A = new double[s.rows()*2];
-            double *B = new double[s.size()];
-            for (int i = 0; i < s.size(); i++)
-                B[i] = s(i);
-            memset(A, 0, sizeof(s.rows()*2));
-            Matrix_EigenValue(B, s.rows(),1000,1e-10,A);
-            Matrix<double> tt(A, s.rows(), 2);
-            t=tt;
-            std::cout<<"s:"<<s;
-            SMatrix<double> s2(A, s.rows());
-            std::cout<<"tt:"<<tt;
-            std::cout<<"s2:"<<s2;
-            delete []A;
-            delete []B;
-        }
-        Matrix<double> eigenvalues() {
-            return t;
-        }
-    private:
-        Matrix<double> t;
-    };
-
-
-} //namespace Ctain end
-
-static void Matrix_Hessenberg(double *A1,int n,double *ret)
-{
-
-    int MaxNumber;
-    double temp,*A;
-    A=new double[n*n];
-    memset(A, 0, sizeof(double)*n*n);
-    for (int i=0;i<n;i++) {
-        int k=i*n;
-        for (int j=0;j<n;j++)
-        {
-            A[k+j]=A1[k+j];
-        }
-    }
-    for (int k=1;k<n-1;k++) {
-        int i=k-1;
-        MaxNumber=k;
-        temp=fabs(A[k*n+i]);
-        for (int j=k+1;j<n;j++) {
-            if (fabs(A[j*n+i])>temp) {
-                temp=fabs(A[j*n+i]);
-                MaxNumber=j;
-            }
-        }
-        ret[0]=A[MaxNumber*n+i];
-        
-        if (ret[0]!=0) {
-            if (MaxNumber!=k) {
-                for (int 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 (int j=0;j<n;j++) {
-                    temp=A[j*n+i];
-                    A[j*n+i]=A[j*n+k];
-                    A[j*n+k]=temp;
-                }
-            }
-            for (int i=k+1;i<n;i++) {
-                temp=A[i*n+k-1]/ret[0];
-                A[i*n+k-1]=0;
-                for (int j=k;j<n;j++) {
-                    A[i*n+j]-=temp*A[k*n+j];
-                }
-                for (int j=0;j<n;j++) {
-                    A[j*n+k]+=temp*A[j*n+i];
-                }
-            }
-        }
-    }
-    for (int i=0;i<n;i++) {
-        int k=i*n;
-        for (int j=0;j<n;j++) {
-            ret[k+j]=A[k+j];
-        }
-    }
+class EigenSolver {
+ public:
+  explicit EigenSolver(SMatrix<double> s) {
+    double *A = new double[s.rows()*2];
+    double *B = new double[s.size()];
+    for (int i = 0; i < s.size(); i++)
+      B[i] = s(i);
+    memset(A, 0, sizeof(s.rows()*2));
+    Matrix_EigenValue(B, s.rows(), 1000, 1e-10, A);
+    Matrix<double> tt(A, s.rows(), 2);
+    t = tt;
+    SMatrix<double> s2(A, s.rows());
     delete []A;
+    delete []B;
+}
+  Matrix<double> eigenvalues() {
+    return t;
+  }
+
+ private:
+  Matrix<double> t;
+};
+
+}  // namespace Ctain
+
+static void Matrix_Hessenberg(double *A1, int n, double *ret) {
+  int MaxNumber;
+  double temp, *A;
+  A = new double[n*n];
+  memset(A, 0, sizeof(double)*n*n);
+  for (int i = 0; i < n; i++) {
+    int k = i * n;
+    for (int j = 0; j < n; j++) {
+      A[k + j] = A1[k + j];
+    }
+  }
+  for (int k = 1; k < n-1; k++) {
+    int i = k-1;
+    MaxNumber = k;
+    temp = fabs(A[k*n+i]);
+    for (int j = k+1; j < n; j++) {
+      if (fabs(A[j * n + i]) > temp) {
+        temp = fabs(A[j * n + i]);
+        MaxNumber = j;
+      }
+    }
+    ret[0] = A[MaxNumber * n + i];
+    if (ret[0] != 0) {
+      if (MaxNumber != k) {
+        for (int 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 (int j = 0; j < n; j++) {
+          temp = A[j * n + i];
+          A[j * n + i] = A[j * n + k];
+          A[j * n + k] = temp;
+        }
+      }
+      for (int i = k + 1; i < n; i++) {
+        temp = A[i * n + k - 1] / ret[0];
+        A[i * n + k - 1] = 0;
+        for (int j = k; j < n; j++) {
+          A[i * n + j] -= temp * A[k * n + j];
+        }
+        for (int j = 0; j < n; j++) {
+          A[j * n + k] += temp * A[j * n + i];
+        }
+      }
+    }
+  }
+  for (int i = 0; i < n; i++) {
+    int k = i * n;
+    for (int 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];
-    memset(A, 0, sizeof(double)*n*n);
-    Matrix_Hessenberg(K1,n,A);
-    m=n;
-    Loop1=LoopNumber;
-    while(m!=0) {
-        t=m-1;
-        while(t>0) {
-            temp=fabs(A[(t-1)*n+t-1]);
-            temp+=fabs(A[t*n+t]);
-            temp=temp*Error1;
-            if (fabs(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(fabs(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++;
-            }
-        }
+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];
+  memset(A, 0, sizeof(double) * n * n);
+  Matrix_Hessenberg(K1, n, A);
+  m = n;
+  Loop1 = LoopNumber;
+  while (m != 0) {
+    t = m - 1;
+    while (t > 0) {
+      temp = fabs(A[(t - 1) * n + t - 1]);
+      temp += fabs(A[t * n + t]);
+      temp = temp * Error1;
+      if (fabs(A[t * n + t - 1]) > temp) {
+        t--;
+      } else {
+        break;
+      }
     }
-    delete []A;
-    return true;
+    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(fabs(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) {
+        delete []A;
+        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
+#endif  // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
diff --git a/src/mynteye/api/camera_models/Quaternion.h b/src/mynteye/api/camera_models/Quaternion.h
index 9b37a9c..4c7dcfa 100644
--- a/src/mynteye/api/camera_models/Quaternion.h
+++ b/src/mynteye/api/camera_models/Quaternion.h
@@ -1,57 +1,66 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
 //
-// Created by 顾涵彬 on 2019-08-30.
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
 //
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
 
-#ifndef MATRIX_QUATERNION_H
-#define MATRIX_QUATERNION_H
+#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_
+#define SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_
 
 #include "SquareMatrix.h"
 #include <cmath>
 namespace Ctain {
-//    using SMatrix<double>;
-    template<typename T>
-    class Quaternion {
-    public:
-        Quaternion(){}
-        Quaternion(SMatrix<double> m){
-            //double f = 1e-10;
-            _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
+template<typename T>
+class Quaternion {
+ public:
+  Quaternion() {}
+  explicit Quaternion(SMatrix<double> m) {
+    _w = sqrt(m(0) + m(4) + m(8) + 1) / 2;
+    _x = (m(5) - m(7)) / (4 * _w);
+    _y = (m(6) - m(2)) / (4 * _w);
+    _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;
+  }
+  inline T x() {return _x;}
+  inline T y() {return _y;}
+  inline T z() {return _z;}
+  inline T w() {return _w;}
+  SMatrix<double> toRotationMatrix() const {
+    SMatrix<double> r(3);
+    double q0 = _w, q1 = _x, q2 = _y, q3 = _z;
+    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;
+};
+}   // namespace Ctain
+#endif   // SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_
diff --git a/src/mynteye/api/camera_models/SquareMatrix.h b/src/mynteye/api/camera_models/SquareMatrix.h
index f6d0a36..d7b0d2b 100644
--- a/src/mynteye/api/camera_models/SquareMatrix.h
+++ b/src/mynteye/api/camera_models/SquareMatrix.h
@@ -1,91 +1,96 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
 //
-// Created by 顾涵彬 on 2019-08-29.
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
 //
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
 
-#ifndef MATRIX_SQUAREMATRIX_H
-#define MATRIX_SQUAREMATRIX_H
+#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
+#define SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
 #include "Matrix.h"
 namespace Ctain {
-#define Matrix Matrix<_Scalar>
-    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;
-
+#define Matrix_ Matrix<_Scalar>
+template<typename _Scalar>
+class SMatrix: public Matrix_{
+ public:
+  explicit SMatrix(int D) : Matrix_(D, D) {}
+  SMatrix() : Matrix_(0, 0) {}
+  SMatrix(_Scalar _data[], int D) : Matrix_(_data, D, D) {}
+  SMatrix(_Scalar **_data, int D) : Matrix_(_data, D, D) {}
+  explicit SMatrix(Matrix_ m) : Matrix_(m) {}
+  _Scalar determinant();
+  _Scalar M(int m, int n);
+  SMatrix<_Scalar> inverse() {
+    SMatrix<_Scalar> res(Matrix_::_Rows);
+    _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;
         }
-
-
-    };//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;
     }
+    return res;
+  }
+  void operator =(Matrix<_Scalar> m) {
+    SMatrix t(m);
+    *this = t;
+  }
+};
 
-    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);
-                }
-            }
+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));
         }
-        sign = (m+n)%2 == 0 ? 1 : -1;
-        mid_result = (float)sign*mid.determinant();
-        return mid_result;
+        result += mid;
     }
-#undef Matrix
+    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;
+}
 
-}//namespace Ctain end
-#endif //MATRIX_SQUAREMATRIX_H
+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 = static_cast<_Scalar>(sign) * mid.determinant();
+  return mid_result;
+}
+#undef Matrix_
+
+}   //  namespace Ctain
+#endif  // SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
diff --git a/src/mynteye/api/camera_models/camera.cc b/src/mynteye/api/camera_models/camera.cc
index 7ebb654..b8ac430 100644
--- a/src/mynteye/api/camera_models/camera.cc
+++ b/src/mynteye/api/camera_models/camera.cc
@@ -172,7 +172,7 @@ double Camera::reprojectionError(
 
 double Camera::reprojectionError(
     const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q,
-    const Ctain::Vector3d &camera_t, 
+    const Ctain::Vector3d &camera_t,
     const Ctain::Vector2d &observed_p) const {
   Ctain::Vector3d P_cam;
   P_cam = camera_q.toRotationMatrix() * P + camera_t;
diff --git a/src/mynteye/api/camera_models/camera.h b/src/mynteye/api/camera_models/camera.h
index c4179a6..57868f3 100644
--- a/src/mynteye/api/camera_models/camera.h
+++ b/src/mynteye/api/camera_models/camera.h
@@ -127,7 +127,8 @@ class Camera {
 
   void projectPoints(
       const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
-      const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const;
+      const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const;  // NOLINT
+
  protected:
   cv::Mat m_mask;
 };
diff --git a/src/mynteye/api/camera_models/equidistant_camera.cc b/src/mynteye/api/camera_models/equidistant_camera.cc
index 16fe855..a336769 100644
--- a/src/mynteye/api/camera_models/equidistant_camera.cc
+++ b/src/mynteye/api/camera_models/equidistant_camera.cc
@@ -478,7 +478,7 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap(
 
   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);
@@ -587,8 +587,7 @@ void EquidistantCamera::fitOddPoly(
     }
     Y(i, 0) = y.at(i);
   }
-  Ctain::SMatrix<double> Tmp;
-  Tmp = X.transpose() * X;
+  Ctain::SMatrix<double> Tmp(X.transpose() * X);
   Ctain::MatrixXd A = Tmp.inverse() * X.transpose() * Y;
 
   coeffs.resize(A.rows());
@@ -679,7 +678,7 @@ void EquidistantCamera::backprojectSymmetric(
         theta = p_u_norm;
       } else {
         theta = *std::min_element(thetas.begin(), thetas.end());
-      } 
+      }
   }
 }
 
diff --git a/src/mynteye/api/camera_models/equidistant_camera.h b/src/mynteye/api/camera_models/equidistant_camera.h
index 7077454..46924be 100644
--- a/src/mynteye/api/camera_models/equidistant_camera.h
+++ b/src/mynteye/api/camera_models/equidistant_camera.h
@@ -127,8 +127,8 @@ class EquidistantCamera : public Camera {
   // Projects 3D points to the image plane (Pi function)
   // and calculates jacobian
   void spaceToPlane(
-      const Ctain::Vector3d &P, Ctain::Vector2d &p,
-      Ctain::Matrix23d &J) const;  
+      const Ctain::Vector3d &P,Ctain::Vector2d &p,  // NOLINT
+      Ctain::Matrix23d &J) const;  // NOLINT
   // %output p
   // %output J
 
@@ -161,7 +161,7 @@ class EquidistantCamera : public Camera {
 
   void fitOddPoly(
       const std::vector<double> &x, const std::vector<double> &y, int n,
-      std::vector<double> &coeffs) const;
+      std::vector<double> &coeffs) const;  // NOLINT
 
   void backprojectSymmetric(
       const Ctain::Vector2d &p_u, double &theta, double &phi) const;  // NOLINT
@@ -172,7 +172,7 @@ class EquidistantCamera : public Camera {
 };
 
 typedef std::shared_ptr<EquidistantCamera> EquidistantCameraPtr;
-typedef std::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr;
+typedef std::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr;  // NOLINT
 
 template <typename T>
 T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
@@ -193,7 +193,7 @@ T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
 template <typename T>
 void spaceToPlane(
     const T *const params, const T *const q, const T *const t,
-    const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p) {
+    const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p) {  // NOLINT
   T P_w[3];
   P_w[0] = T(P(0));
   P_w[1] = T(P(1));
diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc
index aa20265..63817d4 100644
--- a/src/mynteye/api/processor/rectify_processor.cc
+++ b/src/mynteye/api/processor/rectify_processor.cc
@@ -109,11 +109,15 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
   // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
   double fc_new = DBL_MAX;
   CvPoint2D64f cc_new[2] = {{0, 0}, {0, 0}};
-  newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;
-  const double ratio_x = (double)newImgSize.width / imageSize.width / 2;
-  const double ratio_y = (double)newImgSize.height / imageSize.height / 2;
+  newImgSize = newImgSize.width * newImgSize.height != 0 ?
+      newImgSize : imageSize;
+  const double ratio_x = static_cast<double>(newImgSize.width) /
+      imageSize.width / 2;
+  const double ratio_y = static_cast<double>(newImgSize.height) /
+      imageSize.height / 2;
   const double ratio = idx == 1 ? ratio_x : ratio_y;
-  fc_new = (cvmGet(K1, idx ^ 1, idx ^ 1) + cvmGet(K2, idx ^ 1, idx ^ 1)) * ratio;
+  fc_new = (cvmGet(K1, idx ^ 1, idx ^ 1) +
+      cvmGet(K2, idx ^ 1, idx ^ 1)) * ratio;
 
   for (k = 0; k < 2; k++) {
     CvPoint2D32f _pts[4];
@@ -126,8 +130,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
     Ctain::Vector3d b(3, 1);
     for (i = 0; i < 4; i++) {
       int j = (i < 2) ? 0 : 1;
-      a(0) = (float)((i % 2)*(nx));
-      a(1) = (float)(j*(ny));
+      a(0) = static_cast<float>((i % 2)*(nx));
+      a(1) = static_cast<float>(j*(ny));
       if (0 == k) {
         leftOdo->liftProjective(a, b);
       } else {
@@ -290,7 +294,8 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
   // Eigen::Matrix3d R = T.topLeftCorner<3, 3>();
   // Eigen::Vector3d t = T.topRightCorner<3, 1>();
   Ctain::Matrix4d T = loadT(ex_right_to_left);
-  Ctain::Matrix3d R = T.topLeftCorner<3, 3>();
+  Ctain::Matrix3d R;
+  R = T.topLeftCorner<3, 3>();
   Ctain::Vector3d t = T.topRightCorner<3, 1>();
   // cv::Mat cv_R, cv_t;
   // cv::eigen2cv(R, cv_R);
@@ -444,7 +449,7 @@ void RectifyProcessor::InitParams(
   camera_odo_ptr_right->initUndistortRectifyMap(
       map21, map22, right_f[0], right_f[1],
       cv::Size(0, 0), right_center[0],
-      right_center[1], rect_R_r); 
+      right_center[1], rect_R_r);
 }
 
 const char RectifyProcessor::NAME[] = "RectifyProcessor";