基于三维点云地图定位导航（更新中。。。。。）

因为想要做6D localization，因此先尝试跑了下lego_loam  

1 安装好lego-loam以后，按照如下命令跑了下

roslaunch lego_loam run.launch

rosbag play 2018-05-18-14-57-12_8.bag --clock --topic /velodyne_points /imu/data

2 下一步要做的，保存三维地图，然后尝试用蒙特卡罗做定位，先读懂3d_mcl的代码

void cbCloud()中的accum_->push（）应该就是个滑窗，超过一定数量以后，强制process（根据观测模型进行测量）

 

全局定位部分：

#####################删除点云中的孤立点，即周围没有近邻点的点#################################

auto pc_filter = [this, kdtree](const PointType& p)   //lambda 表达式
    {
      std::vector id(1);
      std::vector sqdist(1);
      auto p2 = p;
      p2.z += 0.01 + params_.global_localization_grid_;

      return kdtree->radiusSearch(
          p2, params_.global_localization_grid_, id, sqdist, 1);
    };                           //返回的是半径范围内近邻点的个数
    points->erase(
        std::remove_if(points->begin(), points->end(), pc_filter),    //remove_if返回的是没有近邻点的点的指针
        points->end());     //那么erase删除的就是周围没有近邻点的点云，那么这段代码就是删除点云地图中的孤立点

########################################################################