-
Notifications
You must be signed in to change notification settings - Fork 5
/
debug.cpp
38 lines (35 loc) · 1.1 KB
/
debug.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
#include<Eigen/Core>
#include<Eigen/Eigen>
#include<fstream>
#include <iostream>
using namespace std;
Eigen::Isometry3d readPose(string path)
{
double pose_tem[12];
ifstream fin(path);
for(int i=0;i<12;i++)
{
fin>>pose_tem[i];
}
Eigen::Vector3d trans;
Eigen::Matrix3d rotate;
rotate<<pose_tem[0],pose_tem[1],pose_tem[2],
pose_tem[4],pose_tem[5],pose_tem[6],
pose_tem[8],pose_tem[9],pose_tem[10];
trans<<pose_tem[3],pose_tem[7],pose_tem[11];
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
T.rotate(rotate);
T.pretranslate(trans);
return T;
}
int main()
{
string left="/home/mameng/dataset/scannet/LPI_evaviewpoint/label/scene0591_02/000900.txt";
string right="/home/mameng/dataset/scannet/LPI_evaviewpoint/label/scene0591_02/001700.txt";
Eigen::Isometry3d lT=readPose(left);
Eigen::Isometry3d rT=readPose(right);
Eigen::Isometry3d lr=lT.inverse()*rT;
Eigen::AngleAxisd rotation_vector(lr.rotation());
float angle=rotation_vector.angle()*180.0/M_PI;
std::cout<<angle<<" "<<lr.translation().norm()<<endl;
}