激光SLAM:图优化---基本概念+Rviz显示位姿图
激光SLAM:图优化---基本概念+Rviz显示位姿图
图优化本身有成形的 开源的库 例如
- g2o
- ceres
- gtsam
搞SLAM的话,最好可以理解其概念,然后再用好现成的工具.本篇主要介绍图优化的基本概念.以及在RVIZ中显示位姿图的代码
Pose Graph的概念
用一个图(Graph 图论)来表示SLAM问题
图中的节点来表示机器人的位姿 二维的话即为 (x,y,yaw)
两个节点之间的边表示两个位姿的空间约束(相对位姿关系以及对应方差或线性矩阵)
边分为了两种边
- 帧间边:连接的前后,时间上是连续的
- 回环边:连接的前后,时间上是不连续的,但是直接也是两个位姿的空间约束
构建了回环边才会有误差出现,没有回环边是没有误差的
图优化的基本思想:
出现回环边,有了误差之后.构建图,并且找到一个最优的配置(各节点的位姿),让预测与观测的误差最小
一旦形成回环即可进行优化消除误差
里程积分的相对位姿视为预测值 图上的各个节点就是通过里程(激光里程计\轮速里程计)积分得到的
回环计算的相对位姿视为观测值 图上就是说通过 X2和X8的帧间匹配作为观测值
图优化要干的事:
构建图并调整各节点的位姿,让预测与观测的误差最小
图的构建
帧间边
里程测量 得到
相邻节点之间的相对位姿关系,可以由里程计、IMU、帧间匹配计算得到
回环边
通过回环检测得到
节点 i 和节点 j 在空间上相邻(观测到同样的数据),但是时间上不相邻(中间有其它节点)
用帧间匹配算法计算一个相对位姿 即为回环边的信息
一个简单的回环检测方法
针对当前节点,如图上的X8,标为红色
把其它节点为active (黄色)和 inactive(蓝色和绿色)两部分
找到当前节点周围一定范围内所有inactive节点,作为回环候选帧(绿色节点)
周围一定范围可以认为设定一个距离比如10m,或者根据位置不确定度了设置一个距离
当前节点和回环候选帧进行匹配,根据得分(匹配的相似度)判断是否形成回环
图优化简单例程
用一个最简单的例子,走一遍图优化的过程,加深整体理解.
Pose Graph建立(机器人移动情况):
- 一个机器人,初始点在X0处,位置为0.
- 然后机器人向前移动,通过编码器测得移动距离为1,节点为X1
- 机器人向后移动,通过编码器测得移动距离为0.8,节点为X2
- 通过闭环检测,X2与X0的偏差为0
位姿图(Pose Graph)就是这样的:
上面的例子,可以认为 编码器的测量存在误差,导致和闭环检测的结果,不一致.
那么到底机器人到了哪里,我们假如更相信闭环检测的结果,那么应该如何调整X0 X1 X2的位姿呢
就可以通过图优化的方式来进行求解.
图优化求解过程:
建立关系:
为了使总体的误差最小,使用最小二乘如下:
对上面的函数每个变量求偏导(雅克比矩阵),并使得偏导等于0
这里既可以算出 x0 x1 和x2的值为:
即对整体进行了优化,使得误差项最小.
Pose Graph rivz显示
Code
下面的代码通过rviz将 建立的 Pose Graph 显示出来 ,方便进行 算法的调试
是通过 rviz的 MarkerArray 形式 来 展示 的. 写成了一个函数的形式,方便使用
/***
****函数名称: PublishGraphForVisulization
****函数形参: pub 要发布的topic句柄 Vertexs 存的顶点向量 Edges 存的边的向量 color 颜色设置
****函数返回: 无
****函数功能: 在rviz中 查看 POSE graph 以MarkerArray的形式
****/
void PublishGraphForVisulization(ros::Publisher* pub,
std::vector<Eigen::Vector3d>& Vertexs,
std::vector<Edge>& Edges,
int color = 0)
{
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
函数名称: PublishGraphForVisulization
函数形参: pub 要发布的topic句柄 Vertexs 存的顶点向量 Edges 存的边的向量 color 颜色设置
函数返回: 无
函数功能: 在rviz中 查看 POSE graph 以MarkerArray的形式
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 分割线
visualization_msgs::MarkerArray marray; //声明要发布的 MarkerArray
//point--red
visualization_msgs::Marker m; //声明一个maker 用来 画出 顶点
//赋值顶点的 marker的 基本信息 初始化位置 为 0
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);
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
声明要发布的 MarkerArray
声明一个maker 用来 画出 顶点
赋值顶点的 marker的 基本信息 初始化位置 为 0
根据传入的形参 设置 顶点的颜色
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 分割线
//linear--blue
visualization_msgs::Marker edge; //声明一个marker 用来画出边
//赋值边的 marker的 基本信息 初始化位置 为 0
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;
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
声明一个marker 用来画出边
赋值边的 marker的 基本信息 初始化位置 为 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));//puse 该 maker
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);//发布 以MarkerArray
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
遍历每个顶点 将位置赋值
遍历每个边 将位置赋值
发布 以MarkerArray
函数完毕,调用方式如下
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 分割线
int main(int argc, char **argv)
{
ros::init(argc, argv, "pose_graph");//初始化节点
ros::NodeHandle nodeHandle;//声明句柄
// beforeGraph 声明一个publisher
ros::Publisher beforeGraphPub;
beforeGraphPub = nodeHandle.advertise<visualization_msgs::MarkerArray>("beforePoseGraph",1,true);
// 设置 路径
std::string VertexPath = "/home/jone/ros_slam_ws/src/data/test_v.dat";
std::string EdgePath = "/home/jone/ros_slam_ws/src/data/test_e.dat";
std::vector<Eigen::Vector3d> Vertexs;//声明顶点
std::vector<Edge> Edges;//声明边
ReadVertexInformation(VertexPath,Vertexs);//读取顶点
ReadEdgesInformation(EdgePath,Edges);//读取边
//调用 rivz poes graph 显示功能函数
PublishGraphForVisulization(&beforeGraphPub,
Vertexs,
Edges);
ros::spin();
return 0;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
Result
顶点的设置是这样的
边的设置是这样的
上面是一个用来测试的,例子
下面是实际工程中的数据的样子.
有了pose graph 的显示功能,可以很直观的 看到哪里建立了回环.
文章来源: blog.csdn.net，作者：月照银海似蛟龙，版权归原作者所有，如需转载，请联系作者。
原文链接：blog.csdn.net/qq_32761549/article/details/123394976
- 点赞
- 收藏
- 关注作者
评论（0）