27 lines
1.2 KiB
C++
27 lines
1.2 KiB
C++
//
|
|
// 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;
|
|
}
|
|
|