Skip to content

Efficiency improvements and customizable occupancy 2D map projection. #9

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

Efficiency improvements and customizable occupancy 2D map projection. #9

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
29d32c1
Efficiency improvements and customizable occupancy 2D map projection.
carlosmccosta Jun 16, 2014
cfe8293
Improved efficiency of pointcloud integration and better parametrization
carlosmccosta Jun 18, 2014
dbb4055
Configuration of occupancy grid projection (min and max z) using dynamic
carlosmccosta Aug 13, 2014
03f1c1c
Added option to load the initial OctoMap from nav_msgs/OccupancyGrid msg
carlosmccosta Feb 18, 2015
87d6004
Added service to trigger the publish of octomap information into ros
carlosmccosta Mar 4, 2015
e102f8b
Improved integration of nav_msgs::OccupancyGrid msgs.
carlosmccosta Jul 9, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Improved efficiency of pointcloud integration and better parametrization 
of sensor frame id.

- Line 310 of OctomapServer.cpp was doing an unnecessary internal copy
(pc_nonground = pc), that can be avoided by using shared pointers.
- Improved assembly of pointcloud from octree by using vector reserve
instead of resize.
- Improved parametrization of sensor frame id. This way the input cloud
can be sent in any frame id (map, sensor, other), and the parameter
sensor_frame_id can be used to specify which is the sensor frame to
compute the origin for the raytracing algorithms (used to integrate the
pointcloud into the octree).
  • Loading branch information
@carlosmccosta
carlosmccosta committed Jun 18, 2014
commit cfe8293df2cbcaa0747abd4154d2ca6f40e18abe
1 change: 0 additions & 1 deletion octomap_server/include/octomap_server/OctomapServer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -237,7 +237,6 @@ class OctomapServer{
double m_groundFilterPlaneDistance;

bool m_compressMap;
size_t m_numberCloudsPublished;

// downprojected 2D map:
bool m_incrementalUpdate;
Expand Down
93 changes: 50 additions & 43 deletions octomap_server/src/OctomapServer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_)
: m_nh(),
m_pointCloudSub(NULL),
m_tfPointCloudSub(NULL),
m_tfListener(ros::Duration(30)),
m_tfListener(ros::Duration(60)),
m_octree(NULL),
m_maxRange(-1.0),
m_worldFrameId("/map"), m_baseFrameId("base_footprint"),
Expand All @@ -62,7 +62,6 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_)
m_filterSpeckles(false), m_filterGroundPlane(false),
m_groundFilterDistance(0.04), m_groundFilterAngle(0.15), m_groundFilterPlaneDistance(0.07),
m_compressMap(true),
m_numberCloudsPublished(0),
m_incrementalUpdate(false)
{
ros::NodeHandle private_nh(private_nh_);
Expand Down Expand Up @@ -248,35 +247,42 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr
//
// ground filtering in base frame
//
PCLPointCloud pc; // input cloud for filtering and ground-detection
pcl::fromROSMsg(*cloud, pc);
PCLPointCloud::Ptr pc(new PCLPointCloud()); // input cloud for filtering and ground-detection
pcl::fromROSMsg(*cloud, *pc);

bool cloudAlreadyInWorldFrameId = cloud->header.frame_id == m_worldFrameId;
std::string targetFrame = cloud->header.frame_id;
if (cloudAlreadyInWorldFrameId && !m_sensorFrameId.empty()) {
targetFrame = m_sensorFrameId;
}

tf::StampedTransform sensorToWorldTf;
tf::StampedTransform cloudToWorldTf;
try {
m_tfListener.lookupTransform(m_worldFrameId, cloudAlreadyInWorldFrameId ? m_sensorFrameId : cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);
m_tfListener.waitForTransform(m_worldFrameId, targetFrame, cloud->header.stamp, ros::Duration(0.2));
m_tfListener.lookupTransform(m_worldFrameId, targetFrame, cloud->header.stamp, cloudToWorldTf);
} catch(tf::TransformException& ex){
ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
return;
}

Eigen::Matrix4f sensorToWorld;
pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);
Eigen::Matrix4f cloudToWorld;
pcl_ros::transformAsMatrix(cloudToWorldTf, cloudToWorld);


// set up filter for height range, also removes NANs:
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setFilterFieldName("z");
pass.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ);

PCLPointCloud pc_ground; // segmented ground plane
PCLPointCloud pc_nonground; // everything else
PCLPointCloud::Ptr pc_ground(new PCLPointCloud()); // segmented ground plane
PCLPointCloud::Ptr pc_nonground(new PCLPointCloud()); // everything else

tf::StampedTransform cloudToBaseTf, baseToWorldTf;

if (m_filterGroundPlane){
tf::StampedTransform sensorToBaseTf, baseToWorldTf;
try{
m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2));
m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf);
m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, cloudToBaseTf);
m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf);


Expand All @@ -286,38 +292,43 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr
}


Eigen::Matrix4f sensorToBase, baseToWorld;
pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase);
Eigen::Matrix4f cloudToBase, baseToWorld;
pcl_ros::transformAsMatrix(cloudToBaseTf, cloudToBase);
pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld);

// transform pointcloud from sensor frame to fixed robot frame
pcl::transformPointCloud(pc, pc, sensorToBase);
pass.setInputCloud(pc.makeShared());
pass.filter(pc);
filterGroundPlane(pc, pc_ground, pc_nonground);
// transform pointcloud from cloud frame to fixed robot frame
pcl::transformPointCloud(*pc, *pc, cloudToBase);
pass.setInputCloud(pc);
pass.filter(*pc);
filterGroundPlane(*pc, *pc_ground, *pc_nonground);

// transform clouds to world frame for insertion
pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld);
pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld);
pcl::transformPointCloud(*pc_ground, *pc_ground, baseToWorld);
pcl::transformPointCloud(*pc_nonground, *pc_nonground, baseToWorld);
} else {
// directly transform to map frame:
if (!cloudAlreadyInWorldFrameId) pcl::transformPointCloud(pc, pc, sensorToWorld);
if (!cloudAlreadyInWorldFrameId) pcl::transformPointCloud(*pc, *pc, cloudToWorld);

// just filter height range:
pass.setInputCloud(pc.makeShared());
pass.filter(pc);
pass.setInputCloud(pc);
pass.filter(*pc);

pc_nonground = pc;
pc_nonground = pc; // switch pointers
// pc_nonground is empty without ground segmentation
pc_ground.header = pc.header;
pc_nonground.header = pc.header;
pc_ground->header = pc->header;
pc_nonground->header = pc->header;
}

// change sensor origin to the frame specified
if (!m_sensorFrameId.empty() && m_sensorFrameId != targetFrame) {
m_tfListener.waitForTransform(m_worldFrameId, m_sensorFrameId, cloud->header.stamp, ros::Duration(0.2));
m_tfListener.lookupTransform(m_worldFrameId, m_sensorFrameId, cloud->header.stamp, cloudToWorldTf);
}

insertScan(sensorToWorldTf.getOrigin(), pc_ground, pc_nonground);
insertScan(cloudToWorldTf.getOrigin(), *pc_ground, *pc_nonground);

double total_elapsed = (ros::WallTime::now() - startTime).toSec();
ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground), %f sec)", pc_ground.size(), pc_nonground.size(), total_elapsed);
ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground), %f sec)", pc_ground->size(), pc_nonground->size(), total_elapsed);

publishAll(cloud->header.stamp);
}
Expand Down Expand Up @@ -469,10 +480,8 @@ void OctomapServer::publishAll(const ros::Time& rostime){
// init pointcloud:
sensor_msgs::PointCloud2Ptr cloud = sensor_msgs::PointCloud2Ptr(new sensor_msgs::PointCloud2());
Copy link
Member

@ahornung ahornung Jun 18, 2014

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

cloud is only used locally here, so no need to create on the Heap + Ptr.

Copy link
Author

@carlosmccosta carlosmccosta Jun 18, 2014

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tend to use shared ptrs because if in the future the nodes are converted to nodelets, the transition is easier (and the overhead of the Ptr is negligible).

size_t numberPointsInCloud = 0;
size_t numberReservedPoints = m_octree->size();
float* pointcloudDataPosition = NULL;
size_t numberReservedPoints = m_octree->getNumLeafNodes();
if (publishPointCloud) {
cloud->header.seq = m_numberCloudsPublished++;
cloud->header.stamp = rostime;
cloud->header.frame_id = m_worldFrameId;
cloud->height = 1;
Expand All @@ -493,9 +502,7 @@ void OctomapServer::publishAll(const ros::Time& rostime){
cloud->fields[2].count = 1;
cloud->point_step = 12;

if (numberReservedPoints < 1) numberReservedPoints = 8;
cloud->data.resize(numberReservedPoints * cloud->point_step); // resize to fit all points and avoid memory moves
pointcloudDataPosition = (float*)(&cloud->data[0]);
cloud->data.reserve(numberReservedPoints * cloud->point_step); // reserve memory to avoid reallocations
}

// call pre-traversal hook:
Expand Down Expand Up @@ -554,15 +561,13 @@ void OctomapServer::publishAll(const ros::Time& rostime){

// insert into pointcloud:
if (publishPointCloud) {
if (numberPointsInCloud >= numberReservedPoints) {
numberReservedPoints += 16;
cloud->data.resize(numberReservedPoints * cloud->point_step);
pointcloudDataPosition = (float*)(&cloud->data[numberPointsInCloud * cloud->point_step]);
}
float pointData[3];
pointData[0] = x;
pointData[1] = y;
pointData[2] = z;
unsigned char* pointDataBytes = (unsigned char*)(&pointData[0]);
cloud->data.insert(cloud->data.end(), &pointDataBytes[0], &pointDataBytes[3 * sizeof(float)]);

*pointcloudDataPosition++ = (float)x;
*pointcloudDataPosition++ = (float)y;
*pointcloudDataPosition++ = (float)z;
++numberPointsInCloud;
}

Expand Down Expand Up @@ -658,6 +663,8 @@ void OctomapServer::publishAll(const ros::Time& rostime){
cloud->row_step = cloud->width * cloud->point_step;
cloud->data.resize(cloud->height * cloud->row_step); // resize to shrink the vector size to the real number of points inserted
Copy link
Member

@ahornung ahornung Jun 18, 2014

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Resizing back and forth is a bad idea. Best leave that to the actual vector implementation and instead use data.reserve from the beginning.

Copy link
Author

@carlosmccosta carlosmccosta Jun 18, 2014

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With large pointclouds, letting the vector resize automatically may lead to lots of memory reallocations, and since we have an estimate of the desired size, the efficiency gains will outweigh the memory consumption overhead.

Initially i used resize because i wanted to track the size of the internal vector (cloud->points), and reserve doesnt affect the vector size, but then i decided to track it manually with
size_t numberPointsInCloud
This way i avoided a division by the cloud->point_step.

Nevertheless reserve is more efficient than resize, because doesn't insert default value elements, so i will changed it.

m_pointCloudPub.publish(cloud);

ROS_DEBUG_STREAM("Published pointcloud with " << numberPointsInCloud << " points");
}

if (publishBinaryMap)
Expand Down