基于Mutual Information的Active SLAM《Information-Theoretic Exploration with Bayesian Optimization》

ROS-based的源码来自国外大牛实验室：
https://github.com/RobustFieldAutonomyLab/turtlebot_exploration_3d/blob/master/include/exploration.h
本blog是对此源码的介绍
以下部分内容参考论文《Information-Theoretic Exploration with Bayesian Optimization》
感谢博主的引路https://blog.csdn.net/LOVE1055259415/article/details/80294714
我自己将其用python复现了，相对于C++版本更加简明易懂，欢迎点赞转载！：
https://github.com/BIT-MJY/Active_SLAM_Based_on_Information_Theory.git

开始正经介绍turtlebot_exploration_3d

关键步骤：

（1）提取前沿点

在turtlebot_exploration_3d.cpp的119行，函数实现位于include/exploration.h中

vector<vector<point3d>> frontier_groups=extractFrontierPoints(cur_tree);

extractFrontierPoints对已经扩展的栅格里面的，处于未占据状态的栅格进行前沿点提取。具体实现为：

首先判断这个点是否为未占据状态，若是，则观察其附近五个点是否存在为“未被八叉树地图扩展状态”，若是，则这个点将要被加入到前沿点group中。如果这个点距离此前加入到group中的前沿点距离较近（0.4m以内），则这个点归属于这个前沿点所在集合，否则，其自己单独成为一个集合，成为这一集合前沿点的中心（frontier centers），后续的点可能被加入到它引领的集合。

（2）提取候选移动点

在turtlebot_exploration_3d.cpp的160行，函数实现位于include/exploration.h中

vector<pair<point3d, point3d>> extractCandidateViewPoints(vector<vector<point3d>> frontier_groups, point3d sensor_orig, int n ) {

根据上一步提取的前沿点，在前沿点的各个中心点周围寻找候选点。以中心点为圆心，半径1m~5m的范围内枚举每一个点，进行四个条件的筛选：

• 是否被octomap扩展过（即地图中是否存在该点）
• 距离机器人当前位置不能太近（0.25米之外才行）
• 不能距离其它前沿点太近（0.3米之外）这里代码是否有bug？？？？感觉前沿点中心点自己把自己过滤掉了？？是否可以直接将这一部分筛选条件去掉219-227行
• 附近0.6m的矩形内不应有被占据状态的栅格
  注意候选点包含：x,y,z坐标以及yaw
  重点关注：
  
  由上述截图也可以看出机器人位姿必须朝向前沿点的方向

（3）为每个候选点初始化mutual information

首先计算未被占据空间地体积：

double before = countFreeVolume(cur_tree);

然后对每个候选点进行raycasting：

octomap::Pointcloud hits = castSensorRays(cur_tree, c.first, c.second);

raycasting真正起作用的语句是include/exploration.h中的

    if(octree->castRay(position, RaysToCast.getPoint(i), end, true, Kinect_360.max_range)) {
            hits.push_back(end);
        } else {
            end = RaysToCast.getPoint(i) * Kinect_360.max_range;
            end += position;
            hits.push_back(end);
        }

octree->castRay的api官方文档存在于：
https://octomap.github.io/octomap/doc/classoctomap_1_1OccupancyOcTreeBase.html#afc368d6ff93f40ee33eca3df0b8d1967
可以发现此函数在raycasting击中占据栅格时返回true，击中未知栅格、或传感器达到最大范围依然没有击中占据栅格、或激光束达到八叉树边界的时候均返回false。

castSensorRays函数目的就是为了采集如果机器人位于候选点处，激光束射出去，那么激光束的endpoint是如何进行分布的。

接下来计算初始化的MI：

// Normalize the MI with distance
MIs[i] = calc_MI(cur_tree, c.first, hits, before) / 
    sqrt(pow(c.first.x()-kinect_orig.x(),2) + pow(c.first.y()-kinect_orig.y(),2));

那么具体如何计算MI呢？非常重要！所以贴在下面：

double calc_MI(const octomap::OcTree *octree, const point3d &sensor_orig, const octomap::Pointcloud &hits, const double before) {
    auto octree_copy = new octomap::OcTree(*octree);

    octree_copy->insertPointCloud(hits, sensor_orig, Kinect_360.max_range, true, true);
    double after = countFreeVolume(octree_copy);
    delete octree_copy;
    return after - before;
}

先用目前的地图octree复制一份，得到octree_copy，接着将（hits, sensor_orig）组成的当前观测插入octree，这个过程隐含了Bresenham画线算法。然后使用countFreeVolume计算空闲空间体积。用现在的体积 - 此前的体积，可得到互信息，对应论文中：

所以为什么不是before-after？因为论文中的H指的是占据空间的体积，而在代码中计算的是空闲空间的体积。
得到的MI需要除以sqrt(pow(c.first.x()-kinect_orig.x(),2) + pow(c.first.y()-kinect_orig.y(),2))即当前未知到候选点之间的距离。候选点越远，这个值越小，意思是我们更偏向于更小的移动。

最终，所有的互信息存储在MIs这个容器当中。

（4）进行贝叶斯优化

        // Bayesian Optimization for actively selecting candidate
        double train_time, test_time;
        GPRegressor g(100, 3, 0.01);
        for (int bay_itr = 0; bay_itr < num_of_bay; bay_itr++) {
            //Initialize gp regression
            
            MatrixXf gp_train_x(candidates.size(), 3), gp_train_label(candidates.size(), 1), gp_test_x(gp_test_poses.size(), 3);

            for (int i=0; i< candidates.size(); i++){
                gp_train_x(i,0) = candidates[i].first.x();
                gp_train_x(i,1) = candidates[i].first.y();
                gp_train_x(i,2) = candidates[i].second.z();
                gp_train_label(i) = MIs[i];
            }

            for (int i=0; i< gp_test_poses.size(); i++){
                gp_test_x(i,0) = gp_test_poses[i].first.x();
                gp_test_x(i,1) = gp_test_poses[i].first.y();
                gp_test_x(i,2) = gp_test_poses[i].second.z();
            }

            // Perform GP regression
            MatrixXf gp_mean_MI, gp_var_MI;
            train_time = ros::Time::now().toSec();
            g.train(gp_train_x, gp_train_label);
            train_time = ros::Time::now().toSec() - train_time;

            test_time = ros::Time::now().toSec();
            g.test(gp_test_x, gp_mean_MI, gp_var_MI);
            test_time = ros::Time::now().toSec() - test_time;

            // Get Acquisition function
            double beta = 2.4;
            vector<double>  bay_acq_fun(gp_test_poses.size());
            for (int i = 0; i < gp_test_poses.size(); i++) {
                bay_acq_fun[i] = gp_mean_MI(i) + beta*gp_var_MI(i);
            }
            vector<int> idx_acq = sort_MIs(bay_acq_fun);

            // evaluate MI, add to the candidate
            auto c = gp_test_poses[idx_acq[0]];
            octomap::Pointcloud hits = castSensorRays(cur_tree, c.first, c.second);
            candidates.push_back(c);
            MIs.push_back(calc_MI(cur_tree, c.first, hits, before));
            gp_test_poses.erase(gp_test_poses.begin()+idx_acq[0]);
        }
        
        end_mi_eva_secs = ros::Time::now().toSec();
        ROS_INFO("Mutual Infomation Eva took:  %3.3f Secs.", end_mi_eva_secs - begin_mi_eva_secs);

首先建立高斯过程回归器：GPRegressor g(100, 3, 0.01); 一共三个参数，分别为sf2、ell、noise。然后迭代三次完成贝叶斯优化。

gp_test_poses 来源于

vector<pair<point3d, point3d>> gp_test_poses = candidates;

gp_test_poses 是未经过下采样的候选点。而目前的candidates是被下采样的，来源于：

candidates.resize(min(num_of_samples_eva,temp_size));

gp_train_x存放了所有candidates，gp_train_label存放了train所有的互信息，gp_test_x存放了所有的gp_test_poses。

接下来进行训练

g.train(gp_train_x, gp_train_label);

训练后进行测试，获得均值和方差gp_mean_MI, gp_var_MI

g.test(gp_test_x, gp_mean_MI, gp_var_MI);

接下来计算获取函数Acquisition function相关，使用的是upper confidence bound（UCB）

然后对获取函数进行排序：

 vector<int> idx_acq = sort_MIs(bay_acq_fun);

找到拥有最大获取函数的候选点：

 auto c = gp_test_poses[idx_acq[0]];

然后计算他的MI，作为最佳选取点之一，然后在gp_test_poses将其抹去，避免下一次仍然选他。每次迭代都能在所有test候选点里面获取一个不同的最佳选取点。值得注意的是，这里的MI仍然加入MIs的容器中，符合论文中：

所以最后，对MIs进行排序：

vector<int> idx_MI = sort_MIs(MIs);

接下来用move base走过去：

arrived = goToDest(next_vp, Goal_heading);

这里之所以用到了move base就是因为用到了他的局部规划避障的功能。因为真实情况下确实有很多运动物体。但我们自己做仿真的时候可以考虑不使用move base

接下来更新八叉树地图，即使用回调函数回调一次。

// Update Octomap
ros::spinOnce();：

即exploration.h中的

void kinectCallbacks( const sensor_msgs::PointCloud2ConstPtr& cloud2_msg ) {
....
    cur_tree->insertPointCloud(hits, kinect_orig, Kinect_360.max_range);
....

至此，基于互信息的active slam讲解到此结束。

介绍的较为粗糙，为了简（省）洁（事）一些细节没有介绍，欢迎感兴趣的读者在评论区进行交流。