点云地图的2维投影-1

最近在做点云投影到2维平面gridmap的东西：

#include <iostream>

#include <fstream>

#include <stdint.h>

#include <math.h>

#include <ros/ros.h>

#include <pcl_ros/point_cloud.h>

#include <nav_msgs/OccupancyGrid.h>

#include <pcl_conversions/pcl_conversions.h>

#include <pcl/point_cloud.h>

#include <pcl/point_types.h>

#include <pcl/common/common_headers.h>

#include <pcl/common/impl/io.hpp>

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/features/normal_3d.h>

#include <pcl/features/normal_3d_omp.h>

#include <pcl/console/parse.h>

#include <dynamic_reconfigure/server.h>

#include <cloud_to_map/cloud_to_map_nodeConfig.h>

#include <pcl/filters/radius_outlier_removal.h>

#include <pcl/filters/conditional_removal.h>

#include <pcl/filters/voxel_grid.h>

#include <stdio.h>

#include <sensor_msgs/PointCloud2.h>

#include <pcl/filters/voxel_grid.h>

/* Define the two point cloud types used in this code */

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

typedef pcl::PointCloud<pcl::Normal> NormalCloud;

typedef pcl::PointCloud<pcl::PointXYZRGB> XYZ; //xyz

/* Global */

PointCloud::ConstPtr currentPC;

pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> outrem;

XYZ:: Ptr Cloudxyz1;

pcl::PointCloud<pcl::PointXYZRGB>:: Ptr cloud_filtered ( new pcl::PointCloud<pcl::PointXYZRGB>); //ptr

//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());

//pcl::PointCloud<pcl::PointXYZRGB>::Ptr Cloudxyzrgb (new pcl::PointCloud<pcl::PointXYZRGB> ());

bool newPointCloud = false;

bool reconfig = false;

//Cloudxyz1,cloud_filtered,currentPC

// -----------------------------------------------

// -----Define a structure to hold parameters-----

// -----------------------------------------------

struct Param {

std::string frame;

double searchRadius;

double deviation;

int buffer;

double loopRate;

double cellResolution;

};

// -----------------------------------

// -----Define default parameters-----

// -----------------------------------

Param param;

boost::mutex mutex;