//
// Created by ivan on 22.06.2022.
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>
#include <Eigen/Dense>
#include <sophus/se3.hpp>

int main(){
    std::vector<float> data1 = {-0.0000000006805052125, -0.999999999976638075, -0.000000000230382531,  0.259021424000,
                            -0.00000000015321616298,  0.000000000334626986, -0.999999999882124089, -0.089722543900,
                            0.999999999868921883, -0.0000000006800724144, -0.00000000015323560249, -1.712258880000,
                            0.000000000000,  0.000000000000,  0.000000000000,  1.000000000000};

    std::vector<float> data2 = {-0.9995250378696743, 0.0075019185074052044, -0.02989013031643309, 0.045574835649698026,
                                0.029615343885863205, -0.03439736061393144, -0.998969345370175, -0.071161801837997044,
                                -0.008522328211654736, -0.9993800792498829, 0.03415885127385616, -0.044681254117144367,
                                0.0, 0.0, 0.0, 1.0};
    cv::Mat test(4, 4, CV_32F, &data1[0]);
    Eigen::Matrix<float,4,4,Eigen::RowMajor> eigTbc(test.ptr<float>(0));
    std::cout << eigTbc << std::endl;
    Sophus::SE3f Tbc(eigTbc);
    std::cout << Tbc.rotationMatrix() << std::endl;
}