Converter.h

#pragma once
#include "Base/Headers/Buffer.hpp"
#include "Base/Headers/Matrix.hpp"
#include <Eigen/Dense>
#include "GraphOptimization/Headers/GeometricCamera.h"
namespace cv
{
    class Mat;
    template<typename _Tp> 
    class Point3_;
}
 
namespace Sophus
{
    template <class Scalar_, int Options>
    class SE3;
    template <class Scalar_, int Options>
    class Sim3;
}
namespace NDEVR
{
    struct Sim3;
    class SE3Quat;
    class Converter
    {
    public:
        static Buffer<cv::Mat, BufferAllocator<cv::Mat, 64, false>> toDescriptorVector(const cv::Mat& Descriptors);

        static SE3Quat toSE3Quat(const cv::Mat& cvT);
        static SE3Quat toSE3Quat(const Sophus::SE3<float, 0>& T);
        static SE3Quat toSE3Quat(const Sim3& gSim3);

        static cv::Mat toCvMat(const SE3Quat& SE3);
        static cv::Mat toCvMat(const Sim3& Sim3);
        static cv::Mat toCvMat(const Eigen::Matrix<double, 4, 4>& m);
        static cv::Mat toCvMat(const Eigen::Matrix<float, 4, 4>& m);
        static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 4>& m);
        static cv::Mat toCvMat(const Eigen::Matrix<double, 3, 3>& m);
        static cv::Mat toCvMat(const Eigen::Matrix<double, 3, 1>& m);
        static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 1>& m);
        static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 3>& m);
 

        static Matrix<fltp04> ToNDVMat(const Eigen::Matrix<float, 4, 4>& m)
        {
            Matrix<fltp04> mat;
            for (uint01 i = 0; i < 4; i++)
                for (uint01 j = 0; j < 4; j++)
                    mat[j][i] = cast<fltp04>(m(i, j));
            return mat;
        }

        static cv::Mat toCvMat(const Eigen::Matrix<float, -1, -1>& m);
        static cv::Mat toCvMat(const Eigen::Matrix<double, -1, -1>& m);

        static cv::Mat toCvSE3(const Eigen::Matrix<double, 3, 3>& R, const Eigen::Matrix<double, 3, 1>& t);
        static cv::Mat toCvSE3(const Eigen::Matrix<fltp04, 3, 3>& R, const Eigen::Matrix<fltp04, 3, 1>& t);
        static cv::Mat tocvSkewMatrix(const cv::Mat& v);

        static Eigen::Matrix<double, 3, 1> toVector3d(const cv::Mat& cvVector);
        static Eigen::Matrix<float, 3, 1> toVector3f(const cv::Mat& cvVector);
        static Eigen::Matrix<double, 3, 1> toVector3d(const cv::Point3_<float>& scvPoint);
        static Eigen::Matrix<double, 3, 3> toMatrix3d(const cv::Mat& cvMat3);
        static Eigen::Matrix<double, 4, 4> toMatrix4d(const cv::Mat& cvMat4);
        static Eigen::Matrix<float, 3, 3> toMatrix3f(const cv::Mat& cvMat3);
        static Eigen::Matrix<float, 4, 4> toMatrix4f(const cv::Mat& cvMat4);

        static bool isRotationMatrix(const cv::Mat& R);
        static Buffer<float> toEuler(const cv::Mat& R);

        static Sophus::SE3<g_type, 0> toSophus(const cv::Mat& T);
        static Sophus::Sim3<g_type, 0> toSophus(const Sim3& S);
    };
 
}