1.优化李代数上的位置图
//利用 g2o对sphere.g2o优化文件 优化前 用g20——viewer显示为椭球 //用g2o的话 需要定义顶点和边缘 ///位置图优化只优化位置 路标点不优化 ///顶点应该是相机的位置 ///边是相邻两个位置的变化 //error误差是相邻相机位置变化的逆转 * 待优化的相邻相机的位置变化 ///我们希望这个误差能接近I矩阵 给误差取ln后 误差接近 0 ///该程序用李代数描述误差 // ///把J矩阵的计算放在这里JRInv(const SE3d & e)函数里 ///这里的J矩阵不是雅克比矩阵 雅克比见书公式 p272页 公式10.9 10.10 //李代数应该是向量形式 ///李代数hat 也就是说，李代数的向量变成了反对称矩阵 #include <iostream> #include <fstream> #include <string> #include <Eigen/Core> #include <g2o/core/base_vertex.h> #include <g2o/core/base_binary_edge.h> #include <g2o/core/block_solver.h> #include <g2o/core/optimization_algorithm_levenberg.h> #include <g2o/solvers/eigen/linear_solver_eigen.h> #include <sophus/se3.hpp> using namespace std; using namespace Eigen; using Sophus::SE3d; using Sophus::SO3d; typedef Matrix<double, 6, 6> Matrix6d; typedef Matrix<double, 6, 1> Vector6d; Matrix6d JRInv(const SE3d & e) { Matrix6d J; J.block(0,0,3,3)=SO3d::hat(e.so3().log()); J.block(0,3,3,3)=SO3d::hat(e.translation()); J.block(3,0,3,3)=Matrix3d::Zero(3,3); J.block(3,3,3,3)=SO3d::hat(e.so3().log()); //经过上面得到的是公式10.11哟面的矩阵 // J = J * 0.5 + Matrix6d::Identity(); //P272页 公式10.11 J = Matrix6d::Identity(); // try Identity if you want return J; } //这里的SE3d我之前很纠结 我一直以为这个类型就是李群 其实不然 //个人觉得 李群和李代数的定义都可直接由SE3d定义 //定义顶点 相机位姿势 李代数 class VertexSE3LieAlgebra :public g2o::BaseVertex<6,SE3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW //读取数据 virtual bool read(istream &is) override { double data[7]; for(int i=0;i<7;i++) { is>>data[i]; setEstimate(SE3d(Quaterniond(data[6], data[3], data[4], data[5]),Vector3d(data[0], data[1], data[2]))); } } //将优化的位姿存入内存 virtual bool write(ostream &os) const override { os << id() << " "; Quaterniond q= _estimate.unit_quaternion(); os << _estimate.translation().transpose() << " "; //coeffs顺序是 x y z w ,w是实部 os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << endl; return true; } virtual void setToOriginImpl() override { _estimate=SE3d();//李代数 } //更新 virtual void oplusImpl(const double *update) override { Vector6d upd;//六维向量 upd接收 update upd << update[0], update[1], update[2], update[3], update[4], update[5]; _estimate=SE3d::exp(upd) * _estimate;//更新位姿 } }; // 定义边 两个李代数顶点的边 边就是两个顶点之间的变换 即位姿之间的变换 class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3d, VertexSE3LieAlgebra, VertexSE3LieAlgebra> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW //读取观测值和构造信息矩阵 virtual bool read(istream &is) override { //这里观测值是位子之间的变换，当然包括旋转和平移 所以 data[]是7维 平移加四元数 double data[7]; for(int i=0;i<7;i++) is>>data[i];//流入data[] Quaterniond q(data[6], data[3], data[4], data[5]); q.normalize();//归一化 setMeasurement(SE3d(q,Vector3d(data[0], data[1], data[2]))); //信息矩阵 代表不确定性 //信息矩阵的一些小知识 从另一个博主copy过来的 原文链接 ：https://blog.csdn.net/fly1ng_duck/article/details/101236559?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522160518749619195264726280%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=160518749619195264726280&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~baidu_landing_v2~default-1-101236559.first_rank_ecpm_v3_pc_rank_v2&utm_term=g2o%E4%BF%A1%E6%81%AF%E7%9F%A9%E9%98%B5&spm=1018.2118.3001.4449 //信息矩阵是协方差矩阵的一个逆矩阵 // 信息矩阵在计算条件概率分布明显比协方差矩阵要方便，显然，协方差矩阵要求逆矩阵，所以时间复杂度是O(n^3). 之后我们可以在图优化slam中可以看到， // 因为图优化优化后的解是无穷多个的，比如说x1->x2->x3, 每个xi相隔1m这是我们实际观测出来的，优化后，我们会得出永远得不出x1 x2 x3的唯一解，因为他们有可能123 可能是234 blabla // 但是如果我们提供固定值比如说x2 坐标是3那么解那么就有唯一解234，提供固定值x2这件事情其实就是个先验信息，提供先验信息，求分布，那就是条件分布，也就是这里我们要用到信息矩阵。 //为什么我们需要information matrix 去表征这个uncertainty呢？ // 原因就是我们的系统可能有很多传感器，比如说有两个传感器，一个传感器很精确，另外一个很垃圾。那么精确那个对应的information matrix里面的系数可能是很大， // 记住！！，这里是越大越好，因为它是协方差矩阵的逆矩阵。 //信息矩阵转置后不变，为什么呢？因为这是通常假定我们的传感器之间是独立的。所以中间两项可以合在一起。 //在stream流类型中，有一个成员函数good().用来判断当前流的状态（读写正常（即符合读取和写入的类型)，没有文件末尾） //对于类 读写文件 fstream ifstream ofstream 以及读写字符串流stringstream istringstream ostringstream等类型。都用good()成员函数来判断当前流是否正常。 //不知道下面的信息矩阵为什么要这样构造 //从sphere.g2o文件中可知 // 信息矩阵=[ 10000 0 0 0 0 0 // 0 10000 0 0 0 0 // 0 0 10000 0 0 0 // 0 0 0 40000 0 0 // 0 0 0 0 40000 0 // 0 0 0 0 0 40000]6*6 for (int i = 0; i < information().rows() && is.good(); i++) for (int j = i; j < information().cols() && is.good(); j++) { is >> information()(i, j); if (i != j)//不是对角线的地方 information()(j, i) = information()(i, j); } return true; } //这个函数就是为了把优化好的相机位姿放进指定文件中去 virtual bool write(ostream &os) const override { //v1 v2分别指向两个顶点 VertexSE3LieAlgebra *v1 = static_cast<VertexSE3LieAlgebra *> (_vertices[0]); VertexSE3LieAlgebra *v2 = static_cast<VertexSE3LieAlgebra *> (_vertices[1]); os << v1->id() << " " << v2->id() << " ";//把两个顶点的编号流入os SE3d m =_measurement; Eigen::Quaterniond q = m.unit_quaternion();//unit_quaternion()获取单位四元数 //先传入平移 再传入四元数 os << m.translation().transpose() << " "; os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " "; //信息矩阵 information matrix for(int i=0;i<information().rows();i++) for(int j=i;j<information().cols();j++) { //把信息矩阵的对角线的位置 os << information()(i, j) << " "; } os << endl; return true; } //计算误差 virtual void computeError() override { //v1和v2分别指向两顶点的位姿(待优化) SE3d v1 =(static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate(); SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate(); //_measurment.inverse()*v1.inverse()*v2 = 观测的相邻相机的位姿变换的逆 * 待优化的相邻相机的位姿变换 _error=(_measurement.inverse()*v1.inverse()*v2).log();//李代数 } virtual void linearizeOplus() override { SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate(); SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate(); Matrix6d J= JRInv(SE3d::exp(_error)); //计算J // 尝试把J近似为I？ //雅克比有两个，一个是误差对相机i位姿的雅克比 ，另一个是误差对相机j位姿的雅克比 _jacobianOplusXi= -J*v2.inverse().Adj(); _jacobianOplusXj= J*v2.inverse().Adj(); } }; int main(int argc,char **argv) { //判断运行时候 用法是否正确 if (argc != 2) { cout << "Usage: pose_graph_g2o_SE3_lie sphere.g2o" << endl; return 1; } //将sphere.g2o文件流入 fin //sphere.g2o文件在g2o_viewer中是椭球，也就是我们要优化的东西 ifstream fin(argv[1]); if (!fin) { cout << "file " << argv[1] << " does not exist." << endl; return 1; } // 设定g2o typedef g2o::BlockSolver <g2o::BlockSolverTraits<6, 6>> BlockSolverType;//6,6是顶点和边的维度 typedef g2o::LinearSolverEigen <BlockSolverType::PoseMatrixType> LinearSolverType;//线性求解器类型 //设置梯度下降的方法 auto solver = new g2o::OptimizationAlgorithmLevenberg( g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()) ); g2o::SparseOptimizer optimizer;//图模型 optimizer.setAlgorithm(solver);//设置求解器 optimizer.setVerbose(true);//打开调试输出 int vertexCnt = 0, edgeCnt = 0;//顶点和边的数量 //容器vectices 和 edges 存放各个顶点和边 vector < VertexSE3LieAlgebra * > vectices; vector < EdgeSE3LieAlgebra * > edges; //这里要注意 sphere.g2o文件里面的格式 比如： // VERTEX_SE3:QUAT 0 -0.125664 -1.53894e-17 99.9999 0.706662 4.32706e-17 0.707551 -4.3325e-17 //索引号（编号） 四元数 平移 // //该循环结束后 就把所有的顶点加入图模型中了 //同时容器vectices也指向各个顶点 方便后面优化后进行写入的操作 while (!fin.eof()) { string name; fin >> name; //将文件中的顶点数据流入 顶点就是各个相机的位姿 if (name == "VERTEX_SE3:QUAT") { // 顶点 VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra(); int index = 0; fin>>index; v->setId(index); v->read(fin);//这里其实就是setEstimate optimizer.addVertex(v); vertexCnt++; vectices.push_back(v); if (index == 0) v->setFixed(true); } else if (name == "EDGE_SE3:QUAT") { EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra(); int idx1, idx2; fin >> idx1 >> idx2;//顶点的ID e->setId(edgeCnt++);//设置边的ID //设置顶点 e->setVertex(0, optimizer.vertices()[idx1]); e->setVertex(1, optimizer.vertices()[idx2]); e->read(fin);//读取观测值 optimizer.addEdge(e); edges.push_back(e); } if (!fin.good()) break; } //输出边的顶点的合个数 cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl; cout << "optimizing ..." << endl; optimizer.initializeOptimization();//优化初始化 optimizer.optimize(30);//迭代次数 cout << "saving optimization results ..." << endl; ofstream fout("result_lie.g2o");//把文件流入到result_lie.g2o //下面是把优化的顶点 和 边流入到result_lie.g2o的操作 for (VertexSE3LieAlgebra *v:vectices) { fout << "VERTEX_SE3:QUAT "; v->write(fout);//把优化的顶点放进result_lie.g2o } for (EdgeSE3LieAlgebra *e:edges) { fout << "EDGE_SE3:QUAT "; e->write(fout);//这时候把边也流入到result_lie.g2o } fout.close(); return 0; }
运行结果如下面视频所示： 优化前的sphere.g2o 利用g2o_viewer打开sphere.g2o：
slambook2-ch10-sphere.g2o演示实验
优化后的result_lie.g2o 利用g2o_viewer打开result_lie.g2o：
slambook2-ch10-result
二、g2o原声位姿图优化
这里面的顶点和边的类型选用的是g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数。 源码如下：
// // Created by nnz on 2020/11/14. // #include <iostream> #include <string> #include <fstream> #include <g2o/types/slam3d/types_slam3d.h> #include <g2o/core/block_solver.h> #include <g2o/core/optimization_algorithm_levenberg.h> #include <g2o/solvers/eigen/linear_solver_eigen.h> using namespace std; /************************************************ * 本程序演示如何用g2o solver进行位姿图优化 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更>深刻的理解 * 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数. * * sphere.g2o本身就是g2o文件 直接数据放进去优化就完事了！！！！ * * **********************************************/ int main(int argc, char **argv) { //流程很简单 //不用定义顶点和边 if( argc !=2 ) { cout<<" usge : pose_graph_SE3 sphere.g2o "<<endl; return 1; } ifstream fin(argv[1]); if 标签：ch10传感器