#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;
}