点云地图的2维投影-1

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

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

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

typedef pcl::PointCloud PointCloud;

typedef pcl::PointCloud NormalCloud;

typedef pcl::PointCloud XYZ; //xyz

/* Global */

PointCloud::ConstPtr currentPC;

pcl::RadiusOutlierRemoval outrem;

XYZ:: Ptr Cloudxyz1;

pcl::PointCloud:: Ptr cloud_filtered ( new pcl::PointCloud); //ptr

//pcl::PointCloud::Ptr cloud (new pcl::PointCloud ());

//pcl::PointCloud::Ptr Cloudxyzrgb (new pcl::PointCloud ());

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;

void loadDefaults(ros::NodeHandle& nh) {

nh. param( "frame", param. frame, "map");

nh. param( "search_radius", param. searchRadius, 0.05);

nh. param( "deviation", param. deviation, 0.78539816339);

nh. param( "buffer", param. buffer, 5);

nh. param( "loop_rate", param. loopRate, 10.0);

nh. param( "cell_resolution", param. cellResolution, 0.02);

}

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

// -----Update current PointCloud if msg is received-----

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

void callback( const PointCloud::ConstPtr& msg) {

boost::unique_lock(mutex);

currentPC = msg; //this msg must contain by a point cloudof xyzrgb

// Cloudxyz1 = msg;

newPointCloud = true;

}

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

// -----Callback for Dynamic Reconfigure-----

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

void callbackReconfig(cloud_to_map::cloud_to_map_nodeConfig &config, uint32_t level) {

ROS_INFO( "Reconfigure Request: %s %f %f %f %f", config. frame. c_str(), config. deviation,

config. loop_rate, config. cell_resolution, config. search_radius);

boost::unique_lock(mutex);

param. frame = config. frame. c_str();

param. searchRadius = config. search_radius;

param. deviation = config. deviation;

param. buffer = config. buffer;

param. loopRate = config. loop_rate;

param. cellResolution = config. cell_resolution;

reconfig = true;

}

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

//Cloudxyz1 currentPC cloud_filtered

/*

void pointcloud_filter(){

Cloudxyz1->points.resize(currentPC->size());

for (size_t i = 0; i < Cloudxyz1->points.size(); i++) {

Cloudxyz1->points[i].x = currentPC->points[i].x;

Cloudxyz1->points[i].y = currentPC->points[i].y;

Cloudxyz1->points[i].z = currentPC->points[i].z;

}

outrem.setInputCloud(Cloudxyz1); // 创建滤波器

outrem.setRadiusSearch(1); //设置在0.8半径的范围内找邻近点

outrem.setMinNeighborsInRadius (2); //设置查询点的邻近点集数小于2的删除

outrem.filter (*cloud_filtered); // 应用滤波器

}*/

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

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

/*

*/

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

// -----Calculate surface normals with a search radius of 0.05-----

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

void calcSurfaceNormals(PointCloud::ConstPtr& cloud, pcl::PointCloud:: Ptr normals) { //first one is ptr

pcl::NormalEstimationOMP ne;

ne. setInputCloud(cloud);

pcl::search::KdTree:: Ptr tree( new pcl::search::KdTree());

ne. setSearchMethod(tree);

ne. setRadiusSearch(param. searchRadius);

ne. compute(*normals);

}

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

// -----Initialize Occupancy Grid Msg-----

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

void initGrid(nav_msgs::OccupancyGridPtr grid) {

grid-> header. seq = 1;

grid-> header. frame_id = param. frame;

grid-> info. origin. position. z = 0;

grid-> info. origin. orientation. w = 1;

grid-> info. origin. orientation. x = 0;

grid-> info. origin. orientation. y = 0;

grid-> info. origin. orientation. z = 0;

}

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

// -----Update Occupancy Grid Msg-----

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

void updateGrid(nav_msgs::OccupancyGridPtr grid, double cellRes, int xCells, int yCells,

double originX, double originY, std::vector< signed char> *ocGrid) {

grid-> header. seq++;

grid-> header. stamp. sec = ros::Time::now(). sec;

grid-> header. stamp. nsec = ros::Time::now(). nsec;

grid-> info. map_load_time = ros::Time::now();

grid-> info. resolution = cellRes; //cellRes=cellResolution=param.cellResolution;

grid-> info. width = xCells; //xCells = ((int) ((xMax - xMin) / cellResolution)) + 1;

grid-> info. height = yCells;

grid-> info. origin. position. x = originX; //xMin

grid-> info. origin. position. y = originY;

grid-> data = *ocGrid; //&ocGrid liner save

}

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

// -----Calculate size of Occupancy Grid-----

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

void calcSize( double *xMax, double *yMax, double *xMin, double *yMin) {

for ( size_t i = 0; i < currentPC-> size(); i++) {

double x = currentPC-> points[i]. x;

double y = currentPC-> points[i]. y;

if (*xMax < x) {

*xMax = x;

}

if (*xMin > x) {

*xMin = x;

}

if (*yMax < y) {

*yMax = y;

}

if (*yMin > y) {

*yMin = y;

}

}

}

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

// -----Populate map with cost values-----

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

void populateMap(NormalCloud:: Ptr cloud_normals, std::vector< int> &map, double xMin, double yMin, double cellResolution, int xCells, int yCells) {

double deviation = param. deviation;

for ( size_t i = 0; i < currentPC-> size(); i++) { // currentPC->size()???????????????????????????????????????????????????????????

double x = currentPC-> points[i]. x;

double y = currentPC-> points[i]. y;

double z = cloud_normals-> points[i]. normal_z; //give the normal of z axis to z

double zheight = currentPC-> points[i]. z;

double phi = acos( fabs(z)); // arccos<0 , 90degree>=<0 , 1.57>

int xCell, yCell;

if (zheight == zheight) { //TODO implement cutoff zheight

xCell = ( int) ((x - xMin) / cellResolution);

yCell = ( int) ((y - yMin) / cellResolution);

if ((yCell * xCells + xCell) > (xCells * yCells)) { //xCell and yCell is current point

std::cout << "x: " << x << ", y: " << y << ", xCell: " << xCell << ", yCell: " << yCell << "\n";

}

if (phi > deviation) { // if the normal is larger than a certian degree it gets occupied

map[yCell * xCells + xCell]++; // map = countGrid vector是一个能够存放任意类型的动态数组，能够增加和压缩数据。

} else {

map[yCell * xCells + xCell]--;

}

}

}

}

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

// -----Generate Occupancy Grid-----

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

void genOccupancyGrid(std::vector< signed char> &ocGrid, std::vector< int> &countGrid, int size) {

int buf = param. buffer; // buffer=param.buffer can be adjust by rosparam set, buffer is the threshold

for ( int i = 0; i < size; i++) {

if (countGrid[i] < buf) {

ocGrid[i] = 0;

} else if (countGrid[i] > buf) {

ocGrid[i] = 100;

} else if (countGrid[i] == 0) {

ocGrid[i] = 0; // (unknown)TODO Should be -1

}

}

}

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

// -----Main-----

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

int main( int argc, char** argv) {

/* Initialize ROS */

ros::init(argc, argv, "cloud_to_map_node");

ros::NodeHandle nh;

ros::Subscriber sub = nh. subscribe( "/camera/depth/points_filter", 1, callback); //get the point cloud message

ros::Publisher pub = nh. advertise( "grid", 1);

/* Initialize Dynamic Reconfigure */

dynamic_reconfigure::Server server;

dynamic_reconfigure::Server::CallbackType f;

f = boost::bind(&callbackReconfig, _1, _2);

server. setCallback(f);

/* Initialize Grid */

nav_msgs::OccupancyGridPtr grid( new nav_msgs::OccupancyGrid);

initGrid(grid);

/* Finish initializing ROS */

mutex. lock();

ros::Rate loop_rate(param. loopRate);

mutex. unlock();

/* Wait for first point cloud */

while( ros::ok() && newPointCloud == false){

ros::spinOnce();

loop_rate. sleep();

}

/* Begin processing point clouds */

while ( ros::ok()) {

ros::spinOnce();

boost::unique_lock lck(mutex);

if (newPointCloud || reconfig) {

/* Update configuration status */

reconfig = false;

newPointCloud = false;

/* Record start time */

uint32_t sec = ros::Time::now(). sec;

uint32_t nsec = ros::Time::now(). nsec;

/* Point cloud filter */

//pointcloud_filter();

/* Calculate Surface Normals */

NormalCloud:: Ptr cloud_normals( new NormalCloud);

calcSurfaceNormals(currentPC, cloud_normals); //cloud_filtered

/* Figure out size of matrix needed to store data. */

double xMax = 0, yMax = 0, xMin = 0, yMin = 0;

calcSize(&xMax, &yMax, &xMin, &yMin);

std::cout << "xMax: " << xMax << ", yMax: " << yMax << ", xMin: " << xMin << ", yMin: "

<< yMin << "\n";

/* Determine resolution of grid (m/cell) */

double cellResolution = param. cellResolution;

int xCells = (( int) ((xMax - xMin) / cellResolution)) + 1;

int yCells = (( int) ((yMax - yMin) / cellResolution)) + 1;

std::cout << "xCells: " << xCells << ", yCells: " << yCells << "\n";

/* Populate Map */

std::vector< int> countGrid(yCells * xCells); //countGrid is a vector

populateMap(cloud_normals, countGrid, xMin, yMin, cellResolution, xCells, yCells);

/* Generate OccupancyGrid Data Vector */

std::vector< signed char> ocGrid(yCells * xCells);

genOccupancyGrid(ocGrid, countGrid, xCells * yCells);

/* Update OccupancyGrid Object */

updateGrid(grid, cellResolution, xCells, yCells, xMin, yMin, &ocGrid);

/* Publish the filted cloud

ros::init(argc, argv, "realcam_node");

ros::NodeHandle nh;

ros::Subscriber sub = nh.subscribe (pointstopic, 1, pointsdo);

points_pub=nh.advertise("/camera/depth/points_filter", 1);

ros::spin();

return 0;

/* Release lock */

lck. unlock();

/* Record end time */

uint32_t esec = ros::Time::now(). sec;

uint32_t ensec = ros::Time::now(). nsec;

std::cout << "Seconds: " << esec - sec << "\nNSeconds: " << ensec - nsec << "\n";

/* Publish Occupancy Grid */

pub. publish(grid);

}

loop_rate. sleep();

}

}

代码是这个，现在的问题就是点云滤波，当地图为高分辨率的时候会产生很多路面噪音。

主要是采用求点云normal，做一个阈值，然后用vector存储点云地图，发布。