initMapper 第2部分
1 | // 激光的名称必须是"FLASER" |
getOdomPose
获得centered_laser_pose_ (laser坐标系的原点, 方向(0,0,angle_center)) 在odom坐标系中的坐标gmap_pose，即scan消息的时间戳时，激光雷达在里程计坐标系中的位姿
调用： getOdomPose(initialPose, scan.header.stamp)1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24bool
SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{
// Get the pose of the centered laser at the right time
centered_laser_pose_.stamp_ = t;
tf::Stamped<tf::Transform> odom_pose;
// 得到centered_laser_pose_ 在odom坐标系中的坐标 odom_pose
try
{
tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
double yaw = tf::getYaw(odom_pose.getRotation());
ROS_INFO("get centered_laser_pose_'s odom pose: yaw: %f", yaw);
ROS_INFO("centered_laser_pose_'s odom pose:: %f %f %f",odom_pose.getOrigin().x(), odom_pose.getOrigin().y(), odom_pose.getOrigin().z());
gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(), yaw);
return true;
}
sampleGaussian
S是随机数种子，保证每次的高斯分布不同。AMCL源码中是pf_pdf_gaussian_sample函数中调用pf_ran_gaussian1
2
3
4
5
6
7
8
9
10double sampleGaussian(double sigma, unsigned int S)
{
if (S!=0)
{
srand(S);
}
if (sigma==0)
return 0;
return pf_ran_gaussian (sigma);
}
GridSlamProcessor::init
GridFastSLAM初始化主要是用来初始化各个粒子的一些信息。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
37void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose)
{
//设置地图大小和分辨率
m_xmin=xmin;
m_ymin=ymin;
m_xmax=xmax;
m_ymax=ymax;
m_delta=delta;
/*设置每个粒子的初始值*/
m_particles.clear(); // vector<Particle>
TNode* node=new TNode(initialPose, 0, 0, 0);
//粒子对应的地图进行初始化 用两个地图来进行初始化 一个高分辨率地图 一个低分辨率地图
//高分辨率地图由自己指定 低分辨率地图固定为0.1m
//定义最终使用的地图数据类型 cell类型为 PointAccumulator 存储数据类型为HierarchicalArray2D<PointAccumulator>
// typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
ScanMatcherMap lowMap(Point(xmin+xmax,ymin+ymax)*0.5,xmax-xmin,ymax-ymin, 0.1);
for (unsigned int i=0; i<size; i++) // size是粒子数
{
m_particles.push_back(Particle(lmap,lowMap));
//m_particles.push_back(Particle(lmap));
m_particles.back().pose = initialPose;
// 开始时，前一帧的粒子位姿也是 initialPose
m_particles.back().previousPose = initialPose;
m_particles.back().setWeight(0);
m_particles.back().previousIndex=0;
// we use the root directly
m_particles.back().node= node;
}
m_neff=(double)size;
m_count=0;
m_readingCount=0;
m_linearDistance=m_angularDistance=0;
}
