因为想要做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<int> id(1);
std::vector<float> 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删除的就是周围没有近邻点的点云，那么这段代码就是删除点云地图中的孤立点
########################################################################
#################20210408，最近要补充一部分实验，又开始看代码该代码了，看一点写一点##########
raycast.h是判断在某个位置的某个激光点云（转到地图世界坐标系下）是与地图中的点云（栅格范围内）重合
######################################################################
讲真，粒子滤波在进行6维状态估计时不经济，也不准确，尤其是z方向效果很差，这个代码作为学习是可以的，但是效果一般，还不如ukf，我用图优化模型写了代码，效果确实比pf和ukf好很多，实时性也可以，每帧都能处理，我用的雷达是20帧/s