编译环境ubuntu20.04,vs code
(李群 李代数)
先是CMakeLists.txt
cmake_minimum_required(VERSION 3.0)
project(learning_sophus)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_BUILD_TYPE "Release")
find_package(Sophus REQUIRED)
include_directories("/usr/inlcude/eigen3")
include_directories("Panglin_INCLUDE_DIRS")
add_executable(use useSophus.cpp)
add_executable(RMSE trajectoryError.cpp)
target_link_libraries(use Sophus::Sophus)
find_package( Pangolin )
target_link_libraries(RMSE ${Pangolin_LIBRARIES})
target_link_libraries(RMSE Sophus::Sophus)
主要是一些简单的李群李代数计算(使用Sophus库),还有对轨迹误差作绝对轨迹误差求解
Sophus简单使用
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"
using namespace std;
using namespace Eigen;
int main (int argc ,char** argv)
{
Matrix3d R = AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix(); //旋转90度,先用轴角的形式声明一个旋转向量,然后转换成旋转矩阵赋值给R
Quaterniond q(R); //用四元数表达旋转
Sophus::SO3d SO3_R(R); //两种不同的方法构造李代数
Sophus::SO3d SO3_q(q);
cout<<"SO(3) from matrix:\n"<< SO3_R.matrix()<<endl;
cout<<"SO(3) from quaternion:\n"<<SO3_q.matrix()<<endl;
Vector3d so3 = SO3_R.log(); //对数映射
cout<<"so3 = "<<so3.transpose()<<endl; //旋转向量和旋转矩阵,指数映射就是罗德里格斯公式
cout<<"so3_hat =\n"<<Sophus::SO3d::hat(so3)<<endl; //hat将向量转换为反对称矩阵
cout<<"so3_hat vee= "<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;//vee为逆运算,即从反对称矩阵到向量
//扰动更新-----------------------------------------------------------------------------------------------------------------------------------
Vector3d update_so3(1e-4,0,0);
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3)*SO3_R;
cout<<"SO3_updated =\n"<<SO3_updated.matrix()<<endl;
Vector3d t(1,0,0); //平移一个单位(x轴)
Sophus::SE3d SE3_Rt(R,t); //两种不同的方式构造李群
Sophus::SE3d SE3_qt(q,t);
cout<<"SE3 from R,t=\n"<<SE3_Rt.matrix()<<endl;
cout<<"SE3 from q,t =\n"<<SE3_qt.matrix()<<endl;
typedef Eigen::Matrix<double ,6 ,1> Vector6d; //声明一个6*1的向量模板
Vector6d se3 = SE3_Rt.log(); //对数映射构造李代数
cout<<"se3 = "<<se3.transpose()<<endl;
cout<<"se3_hat =\n"<<Sophus::SE3d::hat(se3)<<endl;
cout<<"se3_hat_vee = "<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;
Vector6d update_se3; //更新量
update_se3.setZero();
update_se3(0,0) = 1e-4; //设置六维向量表示的李代数的第一个值
Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3)*SE3_Rt; //扰动对李群的更新(李代数用指数映射)
cout<<"SE3_updated = "<<endl
<<SE3_updated.matrix()<<endl;
return 0;
}
轨迹误差
#include <iostream>
#include <fstream>
#include <cmath>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>
using namespace std;
using namespace Sophus;
string groundtruth_file = "/home/martin/桌面/code/sophus/groundtruth.txt";
string estimated_file = "/home/martin/桌面/code/sophus/estimated.txt";
typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
void DrawTrajectory(const TrajectoryType >,const TrajectoryType &esti);
TrajectoryType ReadTrajectory(const string &path);
void RMSE(TrajectoryType &estimated,TrajectoryType &groundtruth);
TrajectoryType Read_Translatin(const string &path);
int main (int argc,char** argv)
{
TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
TrajectoryType estimated = ReadTrajectory(estimated_file);
assert(!groundtruth.empty()&&!estimated.empty()); //断言函数,如果为真不做任何操作,伪的话报错
assert(groundtruth.size() == estimated.size()); //和之前的未读取到文件cout一个声明同一个功能
RMSE(estimated,groundtruth);
DrawTrajectory(groundtruth,estimated);
TrajectoryType groundtruth1 = Read_Translatin(groundtruth_file); //相对平移误差(我自己的理解,可能不对)
TrajectoryType estimated1 = Read_Translatin(estimated_file);
RMSE(estimated1,groundtruth1);
return 0;
}
void RMSE(TrajectoryType &estimated,TrajectoryType &groundtruth) //RMSE中的轨迹误差
{
double rmse = 0;
for (size_t i = 0; i < estimated.size(); i++)
{
Sophus::SE3d p1 = estimated[i],p2 = groundtruth[i];
double error = (p2.inverse()*p1).log().norm(); //norm求范数
rmse += error*error;
}
rmse = rmse/double(estimated.size());
rmse = sqrt(rmse);
cout<<"RMSE = "<< rmse <<endl;
}
TrajectoryType ReadTrajectory(const string &path) //读取轨迹数据
{
ifstream fin(path);
TrajectoryType trajectory;
if(!fin)
{
cerr<<"trajectory "<< path << "not found. "<<endl; //功能上和cout类似,cerr是不缓冲的,用于输出错误信息,详细情况查api或者csdn上搜
return trajectory;
}
while (!fin.eof()) //文件读取和vector的尾插操作
{
double time,tx,ty,tz,qx,qy,qz,qw;
fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
Sophus::SE3d p1(Eigen::Quaterniond(qw,qx,qy,qz),Eigen::Vector3d(tx,ty,tz)); //书中有误四元数顺序是实部w,虚部x,y,z
trajectory.push_back(p1);
}
return trajectory;
}
TrajectoryType Read_Translatin(const string &path)
{
ifstream fin(path);
TrajectoryType trajectory;
if(!fin)
{
cerr<<"trajectory "<< path << "not found. "<<endl; //功能上和cout类似,cerr是不缓冲的,用于输出错误信息,详细情况查api或者csdn上搜
return trajectory;
}
while (!fin.eof()) //文件读取和vector的尾插操作
{
double time,tx,ty,tz,qx,qy,qz,qw;
fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
Sophus::SE3d p1(Eigen::Quaterniond(1,0,0,0),Eigen::Vector3d(tx,ty,tz)); //书中有误四元数顺序是实部w,虚部x,y,z
trajectory.push_back(p1);
}
return trajectory;
}
void DrawTrajectory(const TrajectoryType >,const TrajectoryType &esti) //pangolin显示轨迹
{
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024,768 ); //标题和窗口大小
glEnable(GL_DEPTH_TEST); //这三句直接写 这句是启用深度检测,下一句是启用混合
glEnable(GL_BLEND);
glBlendFunc(GL_SRC0_ALPHA,GL_ONE_MINUS_SRC_ALPHA); //表示颜色的混合方式
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000), //投影矩阵(前四个是尺寸,然后是near和far的边界值)
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,0.0,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); //激活相机s
glClearColor(0.0f,0.0f,0.0f,0.0f); //opencv类似设定颜色和线宽(背景颜色设置,我觉得黑色明显一点全是1.0f就是白色了)
//和上一次比少了那些姿态的刻画,在这次里姿态不是主要的,主要看两条轨迹之间的误差----------------------------------------------------------------------------------------
for (size_t i = 0; i < gt.size(); i++) //画线
{
glColor3f(1.0,0.0,0.0); //实际为红色
glBegin(GL_LINES);
auto p1 = gt[i], p2 = gt[i+1];
glVertex3d(p1.translation()[0],p1.translation()[1],p1.translation()[2]);
glVertex3d(p2.translation()[0],p2.translation()[1],p2.translation()[2]);
glColor3f(0.0,1.0,0.0); //估计为绿色
auto q1 = esti[i],q2 = esti[i+1];
glVertex3d(q1.translation()[0],q1.translation()[1],q1.translation()[2]);
glVertex3d(q2.translation()[0],q2.translation()[1],q2.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000);
}
}
还得继续看李代数这块的知识
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)