沉默杀手
(四) 源码分析2 initMapper 第2部分
|激光SLAMgmapping|
Word count: 819|Reading time: 3 min

initMapper 第2部分

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
38
39
40
41
42
43
44
45
46
47
48
// 激光的名称必须是"FLASER"
// 传入计算的angle increment绝对值, 因为gmapping接受正的angle increment
// 根据上面得到的激光雷达的数据信息， 初始化一个激光传感器
gsp_laser_ = new GMapping::RangeSensor("FLASER",
                                       gsp_laser_beam_count_,
                                       fabs(scan.angle_increment),
                                       gmap_pose,   // (0,0,0)
                                       0.0,
                                       maxRange_);
ROS_ASSERT(gsp_laser_);
// std::map<std::string, Sensor*>
GMapping::SensorMap  smap;
// getName()就是 FLASER
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);   // GMapping::GridSlamProcessor*
// 构造函数里只有变量的赋值
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
ROS_ASSERT(gsp_odom_);

///得到里程计的初始位姿，如果没有，则把初始位姿设置为(0,0,0)
GMapping::OrientedPoint  initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
{
  ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
  initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
}
// 以下全是参数的设置，纯粹变量赋值，不多不少
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
                            kernelSize_, lstep_, astep_, iterations_,
                            lsigma_, ogain_, lskip_);

gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
gsp_->setUpdatePeriod(temporalUpdate_);
gsp_->setgenerateMap(false);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
                              delta_, initialPose);
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
gsp_->setminimumScore(minimum_score_);

// 生成1作为方差，均值为0的高斯分布
GMapping::sampleGaussian(1, seed_);

ROS_INFO("Mapper Initialization complete");
return true;

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
24
bool
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_gaussian

1
2
3
4
5
6
7
8
9
10
double 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
37
void 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;
  }