沉默杀手HomeArchivesTagsCategories
(七) 源码分析5 updateMap函数
|激光SLAMgmapping|
Word count: 1k|Reading time: 5 min

第1部分

这里的更新地图，只是为了可视化。因为真正的地图存储在粒子里面，这里会拿一个权值最大的粒子的地图发布出来.

得到权值最大的粒子，然后遍历这个粒子的整个轨迹，根据轨迹上记录的信息来进行建图，然后把得到的地图发布出去。

每次addScan()成功了，就会调用这个函数来生成地图，并发布出去。

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
ROS_DEBUG("Update map");
// 更新地图的时候，加了一把锁
boost::mutex::scoped_lock map_lock (map_mutex_);
// 构造函数的解释在下一篇
GMapping::ScanMatcher  matcher;

/*设置scanmatcher的各个参数*/
matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),
        gsp_laser_->getPose() );
matcher.setlaserMaxRange(maxRange_);
matcher.setusableRange(maxUrange_);
matcher.setgenerateMap(true);

// 得到累计权重(weightSum)最大的粒子，不是当前的最大权重的粒子
// 累计权重即该粒子在各个时刻的权重之和(轨迹上的各个节点的权重之和)
GMapping::GridSlamProcessor::Particle best =
        gsp_->getParticles()[gsp_->getBestParticleIndex()];

std_msgs::Float64 entropy;
// computePoseEntropy 遍历粒子集合计算熵
entropy.data = computePoseEntropy();
//发布位姿的熵
if(entropy.data > 0.0)
    entropy_publisher_.publish(entropy);

//如果没有地图，则初始化一个地图
if(!got_map_)
{
    // nav_msgs::GetMap::Response   map_
    map_.map.info.resolution = delta_;
    map_.map.info.origin.position.x = 0.0;
    map_.map.info.origin.position.y = 0.0;
    map_.map.info.origin.position.z = 0.0;
    map_.map.info.origin.orientation.x = 0.0;
    map_.map.info.origin.orientation.y = 0.0;
    map_.map.info.origin.orientation.z = 0.0;
    map_.map.info.origin.orientation.w = 1.0;
}

计算位姿的信息熵

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
double SlamGMapping::computePoseEntropy()
{
    double weight_total=0.0;
    for( std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
        it != gsp_->getParticles().end();
        ++it)
    {
        weight_total += it->weight;
    }
    double entropy = 0.0;
    for( std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
        it != gsp_->getParticles().end();
        ++it)
    {
        if(it->weight/weight_total > 0.0)
            entropy += it->weight/weight_total * log(it->weight/weight_total);
    }
    return -entropy;
}

第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
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
/*地图的中点*/
GMapping::Point center;
center.x=(xmin_ + xmax_) / 2.0;
center.y=(ymin_ + ymax_) / 2.0;

/* 初始化一个scanmatcherMap 创建一个地图 */
GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_,
                              delta_);

/*更新地图*/
//遍历最优粒子的整条轨迹树， 按照轨迹上各个节点存储的信息，计算激活区域更新地图
ROS_DEBUG("Trajectory tree:");
for(GMapping::GridSlamProcessor::TNode* n = best.node;n;n = n->parent)
{
    ROS_DEBUG("  %.3f %.3f %.3f",
              n->pose.x,
              n->pose.y,
              n->pose.theta);
    if(!n->reading)
    {
        ROS_DEBUG("Reading is NULL");
        continue;
    }
    //进行地图更新
    //matcher.invalidateActiveArea();
    //matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
    //matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
    matcher.registerScan(smap, n->pose, &(n->reading->m_dists[0]));
}

// the map may have expanded, so resize ros message as well
// 扩充地图的大小
if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY())
{

    // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
    //       so we must obtain the bounding box in a different way
    GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
    GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
    xmin_ = wmin.x; ymin_ = wmin.y;
    xmax_ = wmax.x; ymax_ = wmax.y;

    ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
              xmin_, ymin_, xmax_, ymax_);

    map_.map.info.width = smap.getMapSizeX();
    map_.map.info.height = smap.getMapSizeY();
    map_.map.info.origin.position.x = xmin_;
    map_.map.info.origin.position.y = ymin_;
    map_.map.data.resize(map_.map.info.width * map_.map.info.height);

    ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
}

//根据地图的信息计算出来各个点的情况:occ、free、noinformation
//这样对地图进行标记主要是方便用RVIZ显示出来
for(int x=0; x < smap.getMapSizeX(); x++)
{
    for(int y=0; y < smap.getMapSizeY(); y++)
    {
        /// @todo Sort out the unknown vs. free vs. obstacle thresholding
        /// 得到.xy被占用的概率
        GMapping::IntPoint p(x, y);
        double occ=smap.cell(p);
        assert(occ <= 1.0);

        //unknown
        if(occ < 0)
            map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = GMAPPING_UNKNOWN;

        //占用
        else if(occ > occ_thresh_)
        {
            //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
            map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = GMAPPING_OCC;
        }

        //freespace
        else
            map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = GMAPPING_FREE;
    }
}

//到了这一步，肯定是有地图了。
got_map_ = true;

//make sure to set the header information on the map
//把计算出来的地图发布出去
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = tf_.resolve( map_frame_ );

sst_.publish(map_.map);
sstm_.publish(map_.map.info);