#pragma omp parallel for
for(int x=0; x < smap.getMapSizeX(); x++)
{
    for(int y=0; y < smap.getMapSizeY(); y++)
    {

#pragma omp parallel for
    for(int i = 0; i<tmp_size;i++)
    {
        //对保留下来的粒子数据进行更新
        //每个粒子的权重都设置为相同的值
        temp[i].setWeight(0);
        //为每个粒子更新running_scans
        //增加了一帧激光数据 因此需要更新地图
        m_matcher.registerScan(temp[i].map,temp[i].pose,plainReading);
        //m_matcher.registerScan(temp[i].lowResolutionMap,temp[i].pose,plainReading);
    }

inline void GridSlamProcessor::scanMatch(const double* plainReading)
{
    // sample a new pose from each scan in the reference
    /* 每个粒子都要进行 scan-match */
    double sumScore=0;
    int particle_number = m_particles.size();
    //可以用openMP的方式来进行并行化，因此这里不能用迭代器，只能用下标的方式
    //并行化之后，里面的循环会均匀分到各个不同的核里面
    for (int i = 0; i < particle_number; i++)
    {
        OrientedPoint corrected;
        double score, l, s;
        /*进行scan-match 计算粒子的最优位姿，这是gmapping本来的做法*/
        score = m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading);
        /* 匹配成功则更新最优位姿 */
	    if (score>m_minimumScore)
	    {
	      m_particles[i].pose = corrected;
	    }
	    /*  扫描匹配不上,则使用里程计的数据,使用里程计数据不进行更新.
	          因为在进行扫描匹配之前 里程计已经更新过了*/
	    else
	    {
	        if (m_infoStream)
	        {
	            m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;
	            m_infoStream << "lp:" << m_lastPartPose.x << " "  << m_lastPartPose.y << " "<< m_lastPartPose.theta <<std::endl;
	            m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta;
	        }
	    }
	    // 粒子的最优位姿计算了之后，重新计算粒子的权重,相当于粒子滤波器中的观测步骤
	    /* 计算p(z|x,m)，粒子的权重由粒子的似然来表示, 计算粒子的得分和权重(似然)	
	        注意粒子的权重经过ScanMatch之后已经更新了
	     * 在论文中   粒子的权重不是用最优位姿的似然值来表示的
	     * 是用所有的似然值的和来表示的, s是得分  l是似然,也就是权重 */
	    m_matcher.likelihoodAndScore(s, l, m_particles[i].map, m_particles[i].pose, plainReading);
	    sumScore+=score;
	    m_particles[i].weight+= l;
	    m_particles[i].weightSum+= l;

	    //set up the selective copy of the active area
	    //by detaching the areas that will be updated
	    /*计算出来最优的位姿之后，进行地图的扩充  这里不会进行内存分配
	     *不进行内存分配的原因是这些粒子进行重采样之后有可能会消失掉，因此在后面进行重采样的时候统一进行内存分配。
	     *理论上来说，这里的操作是没有必要的，因为后面的重采样的时候还会进行一遍
	     */
	    m_matcher.invalidateActiveArea();
	    m_matcher.computeActiveArea(m_particles[i].map, m_particles[i].pose, plainReading);
    }
    if (m_infoStream)
        m_infoStream << "Average Scan Matching Score of all particles=" << sumScore/m_particles.size();
}

/*矫正成功则更新位姿*/
if (score>m_minimumScore)
{
    m_particles[i].pose = corrected;
}
// 扫描匹配不上,则使用里程计的数据,使用里程计数据不进行更新,因为扫描匹配之前,里程计已经更新过了
else
{
    //输出信息 这个在并行模式下可以会出现错位
    if (m_infoStream)
    {
        m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<endl;
        m_infoStream << "lp:" << m_lastPartPose.x << " "  << m_lastPartPose.y << " "<< m_lastPartPose.theta <<endl;
        m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta <<endl;
    }
}
/* 粒子的最优位姿计算了之后，重新计算粒子的权重(相当于粒子滤波器中的观测步骤，
   计算p(z|x,m))，粒子的权重由粒子的似然来表示。
* 计算粒子的得分和权重(似然)   注意粒子的权重经过ScanMatch之后已经更新了
* 在论文中,例子的权重不是用最有位姿的似然值来表示的。
* 是用所有的似然值的和来表示的。
*/
m_matcher.likelihoodAndScore(s, l, m_particles[i].map, m_particles[i].pose, plainReading);

sumScore+=score;
m_particles[i].weight+=l;
m_particles[i].weightSum+=l;
//set up the selective copy of the active area
//by detaching the areas that will be updated
/*计算出来最优的位姿之后，进行地图的扩充  这里不会进行内存分配
*不进行内存分配的原因是这些粒子进行重采样之后有可能会消失掉，因此在后面进行冲采样的时候统一进行内存分配。
*理论上来说，这里的操作是没有必要的，因为后面的重采样的时候还会进行一遍
*/
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(m_particles[i].map, m_particles[i].pose, plainReading);

// 简化声明
int resampleIndexes(const std::vector<Particle>& particles, int nparticles) const
{
	Numeric cweight=0;
	/*计算总的权重*/
	unsigned int n=0;
	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it)
	{
		cweight+=(Numeric)*it;
		n++;
	}
	if (nparticles>0)
		n=nparticles;
	//compute the interval
	Numeric interval=cweight/n;

	// drand48 返回服从均匀分布的·[0.0, 1.0) 之间的double随机数
	Numeric target=interval*::drand48();
    // 如果某个粒子的区间为4*interval。那么它至少被采样3次。
	cweight=0;  n=0;
	std::vector<unsigned int> indexes(n);
	unsigned int i=0;
	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i)
	{
		cweight+=(Numeric)* it;
		while(cweight>target)
		{
			indexes[n++]=i;
			target+=interval;
		}
	}
	return indexes;
}

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;
}

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;
}

/*地图的中点*/
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);

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//滤波的头文件
#include <pcl/filters/voxel_grid.h>

ros::Publisher pub;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // 声明存储原始数据与滤波后的数据的点云的格式
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; //原始的点云的数据格式
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);

  // ROS消息转化为PCL中的点云数据格式
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  pcl::PCLPointCloud2 cloud_filtered;   //存储滤波后的数据格式
  // 进行一个滤波处理
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor; //创建滤波对象
  sor.setInputCloud (cloudPtr);  //设置输入的滤波，将需要过滤的点云给滤波对象
  sor.setLeafSize (0.1, 0.1, 0.1);  //设置滤波时创建的体素大小为1cm立方体
  sor.filter (cloud_filtered);//执行滤波处理，存储输出cloud_filtered

  // 再将滤波后的点云的数据格式转换为ROS下的数据格式发布
  sensor_msgs::PointCloud2 output;
  pcl_conversions::moveFromPCL(cloud_filtered, output);//第一个参数是输入，后面的是输出
  pub.publish (output);
}

int main (int argc, char** argv)
{
  ros::init (argc, argv, "filter_cloud");//声明节点的名称
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/camera/depth/points", 1, cloud_cb);
  pub = nh.advertise<sensor_msgs::PointCloud2> ("filtered_cloud", 1);

  ros::spin ();
}

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>

int main(int argc, char** argv)
{
    
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  pcl::PCDReader reader;
  reader.read("16line.pcd", *cloud);

  std::cerr << "Cloud before filtering: " << cloud->points.size() << std::endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered3(new pcl::PointCloud<pcl::PointXYZ>);

  // Create the filtering object 直通滤波
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud(cloud);
  pass.setFilterFieldName("x");   // 设置操作的坐标轴
  pass.setFilterLimits(-5.0, 5.0);
  pass.filter(*cloud_filtered2);
  // filter range Y-axis
  // 注意多次滤波需要再次输入点云和调用filter函数
  pass.setInputCloud(cloud_filtered2);
  pass.setFilterFieldName("y");
  pass.setFilterLimits(-5.0, 5.0);
  pass.filter(*cloud_filtered3);

  std::cerr << "Cloud after filtering: " << cloud_filtered3->points.size() << std::endl;

  // 读取pcd点云数据进行操作，最后保存为pcd文件
  pcl::PCDWriter writer;
  writer.write("16line_filtered.pcd", *cloud_filtered3, false);

  return 0;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);      //待滤波点云
if (pcl::io::loadPCDFile("test.pcd", *cloud) < 0)
{
  PCL_ERROR("\a点云文件不存在！\n");
  system("pause");
  return -1;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); //滤波后点云
pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror; //创建滤波器对象
ror.setInputCloud(cloud);           //设置待滤波点云
ror.setRadiusSearch(0.02);            //设置查询点的半径邻域范围
ror.setMinNeighborsInRadius(5);         //设置判断是否为离群点的阈值，即半径内至少包括的点数
//ror.setNegative(true);            //默认false，保存内点；true，保存滤掉的外点
ror.filter(*cloud_filtered);          //执行滤波，保存滤波结果于cloud_filtered

void radiusremoval(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, double radius, int min_pts)
{
  pcl::KdTreeFLANN<pcl::PointXYZ> tree;
  tree.setInputCloud(cloud);
  vector<float> avg;
  for (int i = 0; i < cloud->points.size(); ++i)
  {
    vector<int> id(min_pts);
    vector<float> dist(min_pts);
    tree.nearestKSearch(cloud->points[i], min_pts, id, dist);
    // 是否是平方
    if (dist[dist.size() - 1] < radius)
    {
      cloud_filtered->push_back(cloud->points[i]);
    }
  }
}

#include <pcl/io/pcd_io.h>  //文件输入输出
#include <pcl/point_types.h>  //点类型相关定义
#include <pcl/visualization/cloud_viewer.h>  //点云可视化相关定义
#include <pcl/filters/statistical_outlier_removal.h>  //滤波相关
#include <pcl/common/common.h>  

// 1.读取点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader r;
r.read<pcl::PointXYZ>("table_scene_lms400.pcd", *cloud);
cout << "there are " << cloud->points.size() << " points before filtering." << endl;

// 2.统计滤波
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(50);             // K近邻搜索点个数
sor.setStddevMulThresh(1.0); // 标准差倍数
sor.setNegative(false);     // 保留未滤波点（内点）
sor.filter(*cloud_filter);  // 保存滤波结果到cloud_filter

// 3.滤波结果保存
pcl::PCDWriter w;
w.writeASCII<pcl::PointXYZ>("table_scene_lms400_filter.pcd", *cloud_filter);
cout << "there are " << cloud_filter->points.size() << " points after filtering." << endl;

system("pause");
return 0;