一、李代数上的位姿图优化
//利用 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(!fin)
{
cout<<" file "<<argv[1]<< "dose not exit!" <<endl;
return 1;
}
//设定g2o
// 使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数
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;//分别记录顶点和边的个数
while(!fin.eof())
{
string name;
fin>>name;
if(name=="")
{
//顶点
g2o::VertexSE3 *v = new g2o::VertexSE3();
int index=0;
fin>>index;//编号
v->setId(index);//设置顶点编号
v->read(fin);//读取边 就是setEstimate()
optimizer.addVertex(v);//加入顶点
vertexCnt++;//顶点个数++
//设置是否固定，第一帧固定
if(index==0)
v->setFixed(true);
}
else if(name=="")
{
//边
g2o::EdgeSE3 *e= new g2o::EdgeSE3();
int idx1,idx2;
fin>> idx1>> idx2;
e->setId(edgeCnt++);
//设置idx所对应的顶点
e->setVertex(0,optimizer.vertices()[idx1]);
e->setVertex(1,optimizer.vertices()[idx2]);
e->read(fin);//读取观测数据
optimizer.addEdge(e);//加入边
}
//
if(!fin.good())
break;
}
cout<<"real total "<<vertexCnt<<" vertices, "<< edgeCnt << " edges. " <<endl;
cout<<"optimizing .... "<<endl;
optimizer.initializeOptimization();//优化初始化
optimizer.optimize(30);//迭代次数
cout<<" saving optimization results "<<endl;
optimizer.save("result.g2o");//保存优化后的文件至result.g2o中
return 0;
}
运行结果如下：