1、章节:
1、激光SLAM理论与实践-第五期 第一次作业(矩阵坐标变换)
2、激光SLAM理论与实践-第五期 第二次作业(里程计标定)
3、激光SLAM理论与实践-第五期 第三次作业(去运动畸变)
4、激光SLAM理论与实践-第五期 第四次作业(-帧间匹配算法,imls-icp和csm)
5、激光SLAM理论与实践-第五期 第五次作业(高斯牛顿法优化)
6、激光SLAM理论与实践-第五期 第六次作业 (g2o优化方法)
7、激光SLAM理论与实践-第五期 第七次作业 (mapping)
2、课程PPt和源码
https://download.csdn.net/download/weixin_44023934/85491811
3、代码链接:https://download.csdn.net/download/weixin_44023934/14881521第六章优化代码
定义边:
实验结果
全部代码:
gaussian_newton.cpp
#include "gaussian_newton.h"
#include <eigen3/Eigen/Jacobi>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Householder>
#include <eigen3/Eigen/Cholesky>
#include <eigen3/Eigen/LU>
#include <iostream>
const double GN_PI = 3.1415926;
//进行角度正则化.
double GN_NormalizationAngles(double angle)
{
if(angle > GN_PI)
angle -= 2*GN_PI;
else if(angle < -GN_PI)
angle += 2*GN_PI;
return angle;
}
Eigen::Matrix3d PosetoTran(Eigen::Vector3d x)
{
Eigen::Matrix3d T ;
T << cos(x(2)),-sin(x(2)),x(0),
sin(x(2)),cos(x(2)),x(1),
0.0,0.0,1;
return T;
}
//位姿-->转换矩阵
Eigen::Matrix3d PoseToTrans(Eigen::Vector3d x)
{
Eigen::Matrix3d trans;
trans << cos(x(2)),-sin(x(2)),x(0),
sin(x(2)), cos(x(2)),x(1),
0, 0, 1;
return trans;
}
//转换矩阵-->位姿
Eigen::Vector3d TransToPose(Eigen::Matrix3d trans)
{
Eigen::Vector3d pose;
pose(0) = trans(0,2);
pose(1) = trans(1,2);
pose(2) = atan2(trans(1,0),trans(0,0));
return pose;
}
//计算整个pose-graph的误差
double ComputeError(std::vector<Eigen::Vector3d>& Vertexs,
std::vector<Edge>& Edges)
{
double sumError = 0;
for(int i = 0; i < Edges.size();i++)
{
Edge tmpEdge = Edges[i];
Eigen::Vector3d xi = Vertexs[tmpEdge.xi];
Eigen::Vector3d xj = Vertexs[tmpEdge.xj];
Eigen::Vector3d z = tmpEdge.measurement;
Eigen::Matrix3d infoMatrix = tmpEdge.infoMatrix;
Eigen::Matrix3d Xi = PosetoTran(xi);
Eigen::Matrix3d Xj = PosetoTran(xj);
Eigen::Matrix3d Z = PosetoTran(z);
Eigen::Matrix3d Ei = Z.inverse() * Xi.inverse() * Xj;
Eigen::Vector3d ei = TransToPose(Ei);
sumError += ei.transpose() * infoMatrix * ei;
}
return sumError;
}
/**
* @brief CalcJacobianAndError
* 计算jacobian矩阵和error
* @param xi fromIdx
* @param xj toIdx
* @param z 观测值:xj相对于xi的坐标
* @param ei 计算的误差
* @param Ai 相对于xi的Jacobian矩阵
* @param Bi 相对于xj的Jacobian矩阵
*/
void CalcJacobianAndError(Eigen::Vector3d xi,Eigen::Vector3d xj,Eigen::Vector3d z,
Eigen::Vector3d& ei,Eigen::Matrix3d& Ai,Eigen::Matrix3d& Bi)
{
//TODO--Start
Eigen::Matrix2d Ri,Ri_t,Rj,Rj_t,Rij ,Rij_t ,d_Ri_T,e1;
Eigen::Vector2d ti,tj,tij,e2;
Eigen::Matrix3d Ti = PosetoTran(xi);
Eigen::Matrix3d Tj = PosetoTran(xj);
Eigen::Matrix3d Tij = Ti.transpose()*Tj;
Ri = Ti.block(0,0,2,2);
Rj = Tj.block(0,0,2,2);
Rij = Tij.block(0,0,2,2);
Ri_t = Ti.transpose().block(0,0,2,2);
Rj_t = Tj.transpose().block(0,0,2,2);
Rij_t = Tij.transpose().block(0,0,2,2);
ti =Eigen::Vector2d(xi(0),xi(1));
tj =Eigen::Vector2d(xj(0),xj(1));
tij=Eigen::Vector2d(z(0),z(1));
d_Ri_T = Ti.transpose().block(0,0,2,2);
Eigen::Vector2d error = Rij_t*(Ri_t*(tj- ti)-tij);
double col = GN_NormalizationAngles(xj(2)-xi(2)-z(2));
ei = Eigen::Vector3d(error(0),error(1),col);
e1 = Rij_t*Ri_t;
e2 = Rij_t* d_Ri_T*( tj- ti);
Ai.block(0,0,2,2) = -e1;
Ai.block(0,2,2,1) = e2;
Ai.block(2,0,1,3) = Eigen::Vector3d(0.0,0.0,-1.0).transpose();
Bi.setIdentity();
Bi.block(0,0,2,2) = e1;
// Ri << cos(xi(2)),-sin(xi(2)),
// sin(xi(2)),cos(xi(2));
// Rj << cos(xj(2)),-sin(xj(2)),
// sin(xj(2)),cos(xj(2));
// Rij << cos(z(2)),-sin(z(2)),
// sin(z(2)),cos(z(2));
// //先转置再求导数
// detc_Ri_T << -sin(xi(2)),cos(xi(2)),
// -cos(xi(2)),-sin(xi(2));
// Eigen::Vector2d ti,tj,tij,e2;
// ti << xi(0),xi(1);
// tj << xj(0),xj(1);
// tij << z(0),z(1);
// ei.block(0,0,2,1) = Rij.transpose()*(Ri.transpose()*(tj-ti)-tij);
// ei(2) = GN_NormalizationAngles(xj(2)-xi(2)-z(2));//角度归一化
// e1 = -Rij.transpose()*Ri.transpose();
// e2 = Rij.transpose()*detc_Ri_T*(tj-ti);
// Ai = -Ai.setIdentity();
// Ai << e1(0,0),e1(0,1),e2(0),
// e1(1,0),e1(1,1),e2(1),
// 0.0, 0.0, -1.0;
//TODO--end
}
/**
* @brief LinearizeAndSolve
* 高斯牛顿方法的一次迭代.
* @param Vertexs 图中的所有节点
* @param Edges 图中的所有边
* @return 位姿的增量
*/
Eigen::VectorXd LinearizeAndSolve(std::vector<Eigen::Vector3d>& Vertexs,
std::vector<Edge>& Edges)
{
//申请内存
Eigen::MatrixXd H(Vertexs.size() * 3,Vertexs.size() * 3);
Eigen::VectorXd b(Vertexs.size() * 3);
H.setZero();
b.setZero();
//固定第一帧
Eigen::Matrix3d I;
I.setIdentity();
H.block(0,0,3,3) += I;
//构造H矩阵 & b向量
// std::cout <<"计算H和B的值++++++++++++++++"<<std::endl;
for(int i = 0; i < Edges.size();i++)
{
//提取信息
Edge tmpEdge = Edges[i];
Eigen::Vector3d xi = Vertexs[tmpEdge.xi];
Eigen::Vector3d xj = Vertexs[tmpEdge.xj];
Eigen::Vector3d z = tmpEdge.measurement;
Eigen::Matrix3d infoMatrix = tmpEdge.infoMatrix;
//计算误差和对应的Jacobian
Eigen::Vector3d ei;
Eigen::Matrix3d Ai;
Eigen::Matrix3d Bi;
// std::cout <<"计算雅可比值JJJJJJJJJJJJJJJJJJ"<<std::endl;
CalcJacobianAndError(xi,xj,z,ei,Ai,Bi);
// std::cout <<"计算雅可比值JJJJJJJJJJJJJJJJJJ完毕"<<std::endl;
//TODO--Start
H.block(3*tmpEdge.xi,3*tmpEdge.xi,3,3) += Ai.transpose()*infoMatrix*Ai;
H.block(3*tmpEdge.xi,3*tmpEdge.xj,3,3) += Ai.transpose()*infoMatrix*Bi;
H.block(3*tmpEdge.xj,3*tmpEdge.xi,3,3) += Bi.transpose()*infoMatrix*Ai;
H.block(3*tmpEdge.xj,3*tmpEdge.xj,3,3) += Bi.transpose()*infoMatrix*Bi;
b.block(3*tmpEdge.xi,0,3,1) += Ai.transpose()*infoMatrix*ei;
b.block(3*tmpEdge.xj,0,3,1) += Bi.transpose()*infoMatrix*ei;
//TODO--End
}
// std::cout <<"计算H和B的值++++++++++++++++完毕"<<std::endl;
//求解
Eigen::VectorXd dx(Vertexs.size()*3);
//TODO--Start
// Eigen::MatrixXd ATA = H.transpose()*H;
// Eigen::VectorXd ATb = H.transpose()*(-b);
//最小二乘
//(A.transpose()*A).inverse()*A.transpose()*b;
// dx = -ATA.inverse()*ATb;
// std::cout <<"最小二乘求解****************"<<std::endl;
// dx = ATA.llt().solve(ATb);
dx = (H.transpose()*H).llt().solve(H.transpose()*(-b));
// dx.setZero();
// typedef Eigen::SparseMatrixBase<double> spMat;
// spMat A = H.sparseView();
// Eigen::Solv<spMat> solver;
// solver.compute(A);
// if(solver.info() == Eigen::Success)
// dx = solver.solve(-b);
// std::cout <<"最小二乘求解****************完毕"<<std::endl;
{
// std::cout <<"elseXXXXXXXXXXXXXXXX"<<std::endl;
// dx = ATA.lu().solve(ATb);
// dx = ATA.llt().solve(ATb);
// dx = H.colPivHouseholderQr().solve(b);
// dx = H.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV ).solve(b);
// Eigen::JacobiSVD<Eigen::MatrixXd> svd(H,Eigen::ComputeFullU);
// Eigen::VectorXd evals = svd.singularValues();
// Eigen::MatrixXd evecs = svd.matrixU();
// Eigen::MatrixXd lambds(evals.size(),evals.size());
// lambds.setIdentity();
// for(int i ; i<evals.size();i++)
// {
// lambds(i,i)=1.0/evals(i);
// }
// dx = -1*(evecs*(lambds*(evecs.transpose()*b)));
}
//TODO-End
return dx;
}
main.cpp
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <gaussian_newton.h>
#include <readfile.h>
#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
//#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
// #include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/factory.h>
#include <g2o/core/hyper_graph.h>
#include <Eigen/Core>
#include <cmath>
const double GN_PI = 3.1415926;
double GN_NormalizationAngle(double angle)
{
if(angle > GN_PI)
angle -= 2*GN_PI;
else if(angle < -GN_PI)
angle += 2*GN_PI;
return angle;
}
Eigen::Matrix3d PosetoTrans(Eigen::Vector3d x)
{
Eigen::Matrix3d T ;
T << cos(x(2)),-sin(x(2)),x(0),
sin(x(2)),cos(x(2)),x(1),
0.0,0.0,1;
return T;
}
// 定义定点
class PoseVertex: public g2o::BaseVertex<3,Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PoseVertex(){}
PoseVertex( Eigen::Vector3d org ){
_org = org;
}
virtual void setToOriginImpl()
{
_estimate = _org;
}
virtual void oplusImpl(const double* update)
{
_estimate +=Eigen::Vector3d(update);
_estimate(2) = GN_NormalizationAngle(_estimate(2));
}
virtual bool read(std::istream& in) {}//return true;
virtual bool write(std::ostream& out) const {}
private:
Eigen::Vector3d _org;
};
//维度 类型 顶点类型
class PoseEdge: public g2o::BaseBinaryEdge<3, Eigen::Vector3d ,PoseVertex ,PoseVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//放在public 下
// 在生成定长的Matrix或Vector对象时,需要开辟内存,调用默认构造函数,
// 通常x86下的指针是32位,内存位数没对齐就会导致程序运行出错。
// 而对于动态变量(例如Eigen::VectorXd)会动态分配内存,因此会自动地进行内存对齐。
PoseEdge(){}
void computeError()
{
const PoseVertex* v0 = static_cast<const PoseVertex*>(_vertices[0]);
const PoseVertex* v1 = static_cast<const PoseVertex*>(_vertices[1]);
const Eigen::Vector3d xi = v0->estimate();
const Eigen::Vector3d xj = v1->estimate();
const Eigen::Vector3d z = _measurement;
Eigen::Matrix3d Ti = PosetoTrans(xi);
Eigen::Matrix3d Tj = PosetoTrans(xj);
Eigen::Matrix3d Tij = Ti.transpose()*Tj;
_Ri = Ti.block(0,0,2,2);
_Rj = Tj.block(0,0,2,2);
_Rij = Tij.block(0,0,2,2);
_Ri_t = Ti.transpose().block(0,0,2,2);
_Rj_t = Tj.transpose().block(0,0,2,2);
_Rij_t = Tij.transpose().block(0,0,2,2);
_ti =Eigen::Vector2d(xi(0),xi(1));
_tj =Eigen::Vector2d(xj(0),xj(1));
_tij=Eigen::Vector2d(z(0),z(1));
_d_Ri_T = Ti.transpose().block(0,0,2,2);
// _Ri << cos(xi(2)),-sin(xi(2)),
// sin(xi(2)),cos(xi(2));
// _Rj << cos(xj(2)),-sin(xj(2)),
// sin(xj(2)),cos(xj(2));
// _Rij << cos(z(2)),-sin(z(2)),
// sin(z(2)),cos(z(2));
// _detc_Ri_T << -sin(xi(2)),cos(xi(2)),
// -cos(xi(2)),-sin(xi(2));
// _ti << xi(0),xi(1);
// _tj << xj(0),xj(1);
// _tij << z(0),z(1);
Eigen::Vector2d error = _Rij_t*(_Ri_t*(_tj-_ti)-_tij);
double col =GN_NormalizationAngle(xj(2)-xi(2)-z(2));
_error = Eigen::Vector3d(error(0),error(1),col);
}
void linearizeOplus()
{
_e1 = _Rij_t*_Ri_t;
_e2 = _Rij_t*_d_Ri_T*(_tj-_ti);
_jacobianOplusXi.block(0,0,2,2) = -_e1;//-_Rij_t*_Ri_t;
_jacobianOplusXi.block(0,2,2,1) = _e2;
_jacobianOplusXi.block(2,0,1,3) = Eigen::Vector3d(0.0,0.0,-1.0).transpose();
_jacobianOplusXj.block(0,0,2,2) = _e1;
_jacobianOplusXj.block(0,2,2,1) =Eigen::Vector2d(0.0,0.0);
_jacobianOplusXj.block(2,0,1,3) = Eigen::Vector3d(0.0,0.0,1.0).transpose();
}
virtual bool read(std::istream& in) {}
virtual bool write(std::ostream& out) const {}
private:
Eigen::Matrix2d _Ri ,_Ri_t,_Rj,_Rj_t,_Rij ,_Rij_t ,_d_Ri_T,_e1;
Eigen::Vector2d _ti,_tj,_tij,_e2;
};
//for visual
void PublishGraphForVisulization(ros::Publisher* pub,
std::vector<Eigen::Vector3d>& Vertexs,
std::vector<Edge>& Edges,
int color = 0)
{
visualization_msgs::MarkerArray marray;
//point--red
visualization_msgs::Marker m;
m.header.frame_id = "map";
m.header.stamp = ros::Time::now();
m.id = 0;
m.ns = "ls-slam";
m.type = visualization_msgs::Marker::SPHERE;
m.pose.position.x = 0.0;
m.pose.position.y = 0.0;
m.pose.position.z = 0.0;
m.scale.x = 0.1;
m.scale.y = 0.1;
m.scale.z = 0.1;
if(color == 0)
{
m.color.r = 1.0;
m.color.g = 0.0;
m.color.b = 0.0;
}
else
{
m.color.r = 0.0;
m.color.g = 1.0;
m.color.b = 0.0;
}
m.color.a = 1.0;
m.lifetime = ros::Duration(0);
//linear--blue
visualization_msgs::Marker edge;
edge.header.frame_id = "map";
edge.header.stamp = ros::Time::now();
edge.action = visualization_msgs::Marker::ADD;
edge.ns = "karto";
edge.id = 0;
edge.type = visualization_msgs::Marker::LINE_STRIP;
edge.scale.x = 0.1;
edge.scale.y = 0.1;
edge.scale.z = 0.1;
if(color == 0)
{
edge.color.r = 0.0;
edge.color.g = 0.0;
edge.color.b = 1.0;
}
else
{
edge.color.r = 1.0;
edge.color.g = 0.0;
edge.color.b = 1.0;
}
edge.color.a = 1.0;
m.action = visualization_msgs::Marker::ADD;
uint id = 0;
//加入节点
for (uint i=0; i<Vertexs.size(); i++)
{
m.id = id;
m.pose.position.x = Vertexs[i](0);
m.pose.position.y = Vertexs[i](1);
marray.markers.push_back(visualization_msgs::Marker(m));
id++;
}
//加入边
for(int i = 0; i < Edges.size();i++)
{
Edge tmpEdge = Edges[i];
edge.points.clear();
geometry_msgs::Point p;
p.x = Vertexs[tmpEdge.xi](0);
p.y = Vertexs[tmpEdge.xi](1);
edge.points.push_back(p);
p.x = Vertexs[tmpEdge.xj](0);
p.y = Vertexs[tmpEdge.xj](1);
edge.points.push_back(p);
edge.id = id;
marray.markers.push_back(visualization_msgs::Marker(edge));
id++;
}
pub->publish(marray);
}
bool G2oOptimize(std::vector<Eigen::Vector3d>& Vertexs, std::vector<Edge>& Edges,const int maxIteration)
{
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,3> >Block;
Block::LinearSolverType* linearSolverType = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolverType));
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);//打开调试输出
std::vector< PoseVertex* > vertice;
for(int i = 0;i<Vertexs.size();i++)
{
PoseVertex* v=new PoseVertex(Vertexs[i]) ;//Vertexs[i]
if(i == 0)
v->setFixed(true);
v->setEstimate(Vertexs[i]);
v->setId(i);
optimizer.addVertex(v);
vertice.push_back(v);
}
for(int i= 0;i<Edges.size();i++)
{
PoseEdge* e = new PoseEdge();
e->setId(i);
e->setVertex(0,vertice[Edges[i].xi]);
e->setVertex(1,vertice[Edges[i].xj]);
e->setMeasurement(Edges[i].measurement);
e->setInformation(Edges[i].infoMatrix);
optimizer.addEdge(e);
}
optimizer.initializeOptimization();
optimizer.optimize(maxIteration);
for(int i =0;i<vertice.size();i++)
Vertexs[i] = vertice[i]->estimate();
std::cout <<"finish" <<std::endl;
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ls_slam");
ros::NodeHandle nodeHandle;
// beforeGraph
ros::Publisher beforeGraphPub,afterGraphPub;
beforeGraphPub = nodeHandle.advertise<visualization_msgs::MarkerArray>("beforePoseGraph",1,true);
afterGraphPub = nodeHandle.advertise<visualization_msgs::MarkerArray>("afterPoseGraph",1,true);
std::string VertexPath = "/home/lcp/ws/shenlan2d_ws/src/ls_slam/data/intel-v.dat";
std::string EdgePath = "/home/lcp/ws/shenlan2d_ws/src/ls_slam/data/intel-e.dat";
std::vector<Eigen::Vector3d> Vertexs;
std::vector<Edge> Edges;
//从路径读取文件存到 Vertexs 和Edges
ReadVertexInformation(VertexPath,Vertexs);
ReadEdgesInformation(EdgePath,Edges);
PublishGraphForVisulization(&beforeGraphPub,
Vertexs,
Edges);
double initError = ComputeError(Vertexs,Edges);
std::cout <<"initError:"<<initError<<std::endl;
int maxIteration = 100;
double epsilon = 1e-4;
#if 1
for(int i = 0; i < maxIteration;i++)
{
std::cout <<"Iterations:"<<i<<std::endl;
Eigen::VectorXd dx = LinearizeAndSolve(Vertexs,Edges);
//进行更新
//TODO--Start
std::cout <<"更新值----------------- " <<std::endl;
for(int j = 0;j<Vertexs.size();j++)
Vertexs[j] += Eigen::Vector3d(dx(3*j),dx(3*j+1),dx(3*j+2));
//TODO--End
double maxError = -1;
for(int k = 0; k < 3 * Vertexs.size();k++)
{
if(maxError < std::fabs(dx(k)))
{
maxError = std::fabs(dx(k));
}
}
if(maxError < epsilon)
break;
}
#else
if(! G2oOptimize( Vertexs, Edges, maxIteration))
{
std::cout <<"优化出错" <<std::endl;
return 0;
}
#endif
double finalError = ComputeError(Vertexs,Edges);
std::cout <<"FinalError:"<<finalError<<std::endl;
PublishGraphForVisulization(&afterGraphPub,
Vertexs,
Edges,1);
ros::spin();
return 0;
}