1. Sophus库基础应用
//
// Created by g214-j on 18-7-22.
//
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/so3.h"
#include "sophus/se3.h"
int main(int argc, char** argv){
// 沿Z轴转90度的旋转矩阵(由旋转轴得到旋转矩阵)
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
// SO3特殊正交群变量的创建
Sophus::SO3 SO3_R(R); // 由旋转矩阵直接构建SO3特殊正交群
Sophus::SO3 SO3_v(0,0,M_PI/2); // 也可以直接由旋转向量构建SO3特殊正交群
Eigen::Quaterniond q(R); // 四元数
Sophus::SO3 SO3_q(q); // 也可以直接由四元数构建SO3特殊正交群
// SO3李群的输出,输出将会以构建李群时的原始方式so3表示,要注意:这里不是矩阵
cout<<"SO(3) from matrix: "<<SO3_R<<endl;
cout<<"SO(3) from vector: "<<SO3_v<<endl;
cout<<"SO(3) from quaternion: "<<SO3_q<<endl;
// 使用对数映射获得相应的李代数(特殊正交群对应的李代数其实是旋转向量,即三维向量)
Eigen::Vector3d so3 = SO3_R.log();
cout<<"so3 = "<<so3.transpose()<<endl;
// 向量的反对称形式
cout<<"so3 hat = \n"<<Sophus::SO3::hat(so3)<<endl;
// 反对称回到向量
cout<<"so3 hat vee = \n"<<Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose()<<endl;
// 使用扰动模型进行微小的变动
Eigen::Vector3d update_so3(1e-4,0,0); // 假设更新量为这么多
Sophus::SO3 SO3_update = Sophus::SO3::exp(update_so3)*SO3_R;
cout<<"SO3 update = "<<SO3_update<<endl; // 输出还是so3的形式
cout<<"SO3 update(in matrix model) = "<<endl<<SO3_update.matrix()<<endl;
/********************萌萌的分割线*****************************/
cout<<"************我是分割线*************"<<endl;
// SO3操作完了,对SE3进行类比操作
Eigen::Vector3d t(1,0,0); // 平移量,沿x轴平移1
Sophus::SE3 SE3_Rt(R,t); // 从R,t构造SE(3)
Sophus::SE3 SE3_qt(q,t); // 从q,t构造SE(3)
cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl; // 好奇怪,这里的输出形式和se3的输出形式不一样,这里旋转在前,平移在后
cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
// SE3对应的李代数为6维的向量
typedef Eigen::Matrix<double,6,1> Vector6d;
Vector6d se3 = SE3_Rt.log();
cout<<"se3 = "<<se3.transpose()<<endl;
// 观察输出后,会发现在Sophus中,se(3)的平移在前,旋转在后!!!
// 同样的,有hat和vee两个算符
cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl; // se3对应的hat为一个四维的矩阵,这里的hat和vee和李代数对应的群有关,所以就能理解hat和vee函数要放在对应的李群类中调用了
cout<<"se3 hat vee = "<<endl<<Sophus::SE3::vee(Sophus::SE3::hat(se3))<<endl;
// 最后,演示一下扰动更新
Vector6d update_se3; // 创建李代数上的微小更新量
update_se3.setZero();
update_se3(0,0) = 1e-4; // 确定微小更新量的值
Sophus::SE3 SE3_update = Sophus::SE3::exp(update_se3)*SE3_Rt;
cout<<"SE3 update = "<<endl<<SE3_update.matrix()<<endl; // 这里输出要调用变量自身的matrix()函数转换成矩阵形式打印输出,猜测可能是变量类内部实现的时候的存储方式不是矩阵,所以要转换一下
return 0;
}
2. 轨迹的描绘
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Geometry>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
// path to trajectory file
string trajectory_file = "./trajectory.txt";
// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
int main(int argc, char **argv) {
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;
/// implement pose reading code
// start your code here (5~10 lines)
fstream fin("./trajectory.txt");
string line;
double t, tx, ty, tz, qx, qy, qz, qw;
while( getline(fin,line) ){
stringstream lineStream(line);
lineStream>>t>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
Eigen::Vector3d t(tx,ty,tz);
Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized();
Sophus::SE3 SE3_qt(q,t);
poses.push_back(SE3_qt);
}
// end your code here
// draw trajectory in pangolin
DrawTrajectory(poses);
return 0;
}
/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
if (poses.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);
(qw,qx,qy,qz)
d_cam.Activate(s_cam);
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
glLineWidth(2);
for (size_t i = 0; i < poses.size() - 1; i++) {
glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
glBegin(GL_LINES);
auto p1 = poses[i], p2 = poses[i + 1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
}
3. 计算轨迹的误差
//
// Created by g214-j on 18-7-22.
//
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pangolin/pangolin.h>
using namespace std;
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1);
void ReadData(string fileName, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>& pose);
double error_Twc(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> Tg, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> Te);
// 路径变量
string estimated_file = "./estimated.txt";
string groundtruth_file = "./groundtruth.txt";
string trajectory_file = "./trajectory.txt";
int main(int argc, char** argv){
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_estimated;
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_groundtruth;
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_trajectory;
ReadData(estimated_file, pose_estimated);
ReadData(groundtruth_file, pose_groundtruth);
ReadData(trajectory_file, pose_trajectory);
cout<<"The size of estimated data is "<<pose_estimated.size()<<endl;
cout<<"The size of groundtruth data is "<<pose_groundtruth.size()<<endl;
cout<<"The size of trajectory data is "<<pose_trajectory.size()<<endl;
double trajectory_error_RMSE = 0;
trajectory_error_RMSE = error_Twc(pose_groundtruth, pose_estimated);
cout<<"trajectory_error_RMSE = "<<trajectory_error_RMSE<<endl;
DrawTrajectory(pose_estimated,pose_groundtruth);
return 0;
}
/*******************************************************************************************/
// 绘制两条曲线
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1)
{
if (poses.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 < poses.size() - 1; i++) {
glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
glBegin(GL_LINES);
auto p1 = poses[i], p2 = poses[i + 1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
// 第二条线绘制
glColor3f(0.f, 0.8f, 0.f);
glBegin(GL_LINES);
auto p3 = poses1[i], p4 = poses1[i + 1];
glVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);
glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
}
/*******************************************************************************************/
// 读取文件数据,转换成SE3数据类型,存进pose向量中
void ReadData(string fileName, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>& pose){
ifstream trajectory(fileName);
if(!trajectory.is_open()){
cout<<"No "<<fileName<<endl;
return;
}
double t, tx, ty, tz, qx, qy, qz, qw;
string line;
while(getline(trajectory,line)){
stringstream lineStream(line);
lineStream>>t>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
Eigen::Vector3d t(tx,ty,tz);
Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized();
Sophus::SE3 SE3_qt(q,t);
pose.push_back(SE3_qt);
}
return;
}
/*******************************************************************************************/
// 计算两条曲线的RMSE误差,输入为两条曲线的SE3向量,输出为RMSE误差值
double error_Twc(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> Tg, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> Te){
vector<double> error;
Eigen::Matrix<double,6,1> se3;
// 计算路径上每一对对应点的误差
for(int i=0;i<Tg.size();i++){
se3 = Sophus::SE3::log(Tg[i].inverse() * Te[i]);
cout<<"se3 = "<<se3.transpose()<<endl;
error.push_back(se3.squaredNorm()); // 二范数,已经是每一对误差的平方
cout<<"se3 squarNorm is "<<error[i]<<endl;
}
double RMSE = 0;
for(int i =0;i<error.size();i++){
RMSE+=error[i];
}
RMSE/=(double)error.size();
RMSE = sqrt(RMSE);
return RMSE;
}