#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

using namespace std;

using namespace cv;

using namespace Sophus;

#define COMPARE 0

#define EXAMPLE 1

//pangolin画图

void DrawTrajectory(vector<:se3 eigen::aligned_allocator>> poses1,vector<:se3 eigen::aligned_allocator>> poses2) {

if (poses1.empty()||poses2.empty()) {

cerr << "Trajectory is empty!" << endl;

return;

}

// create pangolin window and plot the trajectory

pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);

glEnable(GL_DEPTH_TEST);

glEnable(GL_BLEND);

glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

pangolin::OpenGlRenderState s_cam(

pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),

pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)

);

pangolin::View &d_cam = pangolin::CreateDisplay()

.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)

.SetHandler(new pangolin::Handler3D(s_cam));

while (pangolin::ShouldQuit() == false) {

glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

d_cam.Activate(s_cam);

glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

glLineWidth(2);

for (size_t i = 0; i < poses1.size() - 1; i++) {

//glColor3f(1 - (float) i / poses1.size(), 0.0f, (float) i / poses1.size());

glColor3f(1.0f, 0.0f, 0.0f);

glBegin(GL_LINES);

auto p1 = poses1[i], p2 = poses1[i + 1];

glVertex2d(p1.translation()[0], p1.translation()[1]);//p1.translation()[2]

glVertex2d(p2.translation()[0], p2.translation()[1]);//p2.translation()[2]

glEnd();

}

for (size_t i = 0; i < poses2.size() - 1; i++) {

//glColor3f(1 - (float) i / poses2.size(), 0.0f, (float) i / poses2.size());

glColor3f(0.0f, 0.0f, 0.0f);

glBegin(GL_LINES);

auto p1 = poses2[i], p2 = poses2[i + 1];

glVertex2d(p1.translation()[0], p1.translation()[1]);//p1.translation()[2]

glVertex2d(p2.translation()[0], p2.translation()[1]);//p2.translation()[2]

glEnd();

}

pangolin::FinishFrame();

usleep(5000);   // sleep 5 ms

}

}

int main ( int argc, char** argv )

{

vector<:se3 eigen::aligned_allocator>> ps1,ps2;

#if COMPARE

ifstream file("../compare.txt");

string line;

vector pEstimated,pGroundTruth;

while(getline(file,line))

{

stringstream record(line);

vector vTmp(16,0);

for(auto &v:vTmp)

record>>v;

Point3d p1{vTmp[1],vTmp[2],vTmp[3]};//位置

Eigen::Quaterniond q1{vTmp[4],vTmp[5],vTmp[6],vTmp[7]};//旋转四元数

Sophus::SE3 SE3_qt1(q1,Eigen::Vector3d(p1.x,p1.y,p1.z));//组成李群(欧式变换群)--estimated

Point3d p2{vTmp[9],vTmp[10],vTmp[11]};

Eigen::Quaterniond q2{vTmp[12],vTmp[13],vTmp[14],vTmp[15]};

Sophus::SE3 SE3_qt2(q2,Eigen::Vector3d(p2.x,p2.y,p2.z));//组成李群(欧式变换群)--groundtruth

pEstimated.push_back(p1);//求估计到真实的变换矩阵只需要用到位置

pGroundTruth.push_back(p2);

ps1.push_back(SE3_qt1);

ps2.push_back(SE3_qt2);

}

#endif

#if EXAMPLE

ifstream ifileEstimated("../EstimatedAligned.txt");

ifstream ifileGround("../GroundAligned.txt");

string line;

vector pEstimated,pGroundTruth;//用于求取变换矩阵的相机位置

while(getline(ifileEstimated,line))

{

stringstream record(line);

double vtmp[8];

for (auto i = 0; i < 8; i++)

record >> vtmp[i];

Point3d p1{vtmp[1],vtmp[2],vtmp[3]};//位置

pEstimated.push_back(p1);

Eigen::Quaterniond q(vtmp[4], vtmp[5], vtmp[6], vtmp[7]);

Sophus::SE3 SE3_qt1(q,Eigen::Vector3d(p1.x,p1.y,p1.z));//组成李群(欧式变换群)--estimated

ps1.push_back(SE3_qt1);

}

while(getline(ifileGround,line))

{

stringstream record(line);

double vtmp[8];

for (auto i = 0; i < 8; i++)

record >> vtmp[i];

Point3d p2{vtmp[1],vtmp[2],vtmp[3]};//位置

pGroundTruth.push_back(p2);

Eigen::Quaterniond q(vtmp[4], vtmp[5], vtmp[6], vtmp[7]);

Sophus::SE3 SE3_qt2(q,Eigen::Vector3d(p2.x,p2.y,p2.z));//组成李群(欧式变换群)--groundtruth

ps2.push_back(SE3_qt2);

}

#endif

//下面是ICP过程，p1=Rp2+t，即为真实到估计之间的变换，之后取逆即为估计到真实之间的变换

Point3d pMid1, pMid2;     // center of mass

int N = pEstimated.size();

for ( int i=0; i

{

pMid1 += pEstimated[i];

pMid2 += pGroundTruth[i];

}

pMid1 = Point3d( pMid1/ N);

pMid2 = Point3d( pMid2/ N);

vector     q1 ( N ), q2 ( N ); // remove the center

for ( int i=0; i

{

q1[i] = pEstimated[i] - pMid1;

q2[i] = pGroundTruth[i] - pMid2;

}

// compute q1*q2^T

Eigen::Matrix3d W = Eigen::Matrix3d::Zero();

for ( int i=0; i

{

W=W+Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();

}

cout<

// SVD on W

Eigen::JacobiSVD<:matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );

Eigen::Matrix3d U = svd.matrixU();

Eigen::Matrix3d V = svd.matrixV();

cout<

cout<

Eigen::Matrix3d R_ = U* ( V.transpose() );

Eigen::Vector3d t_ = Eigen::Vector3d ( pMid1.x, pMid1.y, pMid1.z ) - R_ * Eigen::Vector3d ( pMid2.x, pMid2.y, pMid2.z );

cout<

cout<

//取逆

Eigen::Matrix3d R=R_.inverse();

Eigen::Vector3d t=-R*t_;

//    Eigen::Quaterniond q(R_);

//    cout<

//DrawTrajectory(ps1,ps2);

SE3 SE3_T(R,t);

#if EXAMPLE

ofstream ofileEst("../AfterAligned.txt");//保存经过转换后的相机位姿的文件

#endif

for(auto &p:ps1)

{

p=SE3_T*p;

#if EXAMPLE

Eigen::Quaterniond q(p.rotation_matrix());//把李群变换成位置向量和四元数形式保存

Eigen::Vector3d vp(p.translation());

//a按格式写入文件，直接以转置的形式写入，格式有些混乱

ofileEst<

ofileEst<

q.y()<

}

ofileEst.close();

#endif

DrawTrajectory(ps1,ps2);

return 0;

}