i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-13 04:59:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:introlab/rtabmap_ros into devel

Browse Source
This commit is contained in:
matlabbe
2016-06-17 15:11:21 -04:00
parent 9724c40876 e6135d70ee
commit f0e2badb7e
1 changed files with 49 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapsManager.cpp
+21 -9
View File
@@ -577,21 +577,19 @@ void MapsManager::publishMaps(
} }
} }
if(assembledCloud->size())
{
if(cloudFrustumCulling_ && negativePoses.size())
{
for(std::list<std::pair<int, Transform> >::reverse_iterator iter=negativePoses.rbegin(); iter!=negativePoses.rend(); ++iter) for(std::list<std::pair<int, Transform> >::reverse_iterator iter=negativePoses.rbegin(); iter!=negativePoses.rend(); ++iter)
{ {
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first); std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
if(jter != clouds_.end() && jter->second->size())
{
std::map<int, std::vector<CameraModel> >::iterator kter = cameraModels_.find(iter->first); std::map<int, std::vector<CameraModel> >::iterator kter = cameraModels_.find(iter->first);
if(jter != clouds_.end() && kter != cameraModels_.end()) if(cloudFrustumCulling_ && kter != cameraModels_.end() && assembledCloud->size())
{ {
for(unsigned int i=0; i<kter->second.size(); ++i) for(unsigned int i=0; i<kter->second.size(); ++i)
{ {
if(kter->second[i].isValidForProjection()) if(kter->second[i].isValidForProjection())
{ {
int size = assembledCloud->size();
assembledCloud = util3d::frustumFiltering( assembledCloud = util3d::frustumFiltering(
assembledCloud, assembledCloud,
iter->second, // FIXME: should include camera local transform iter->second, // FIXME: should include camera local transform
@@ -601,13 +599,25 @@ void MapsManager::publishMaps(
cloudMaxDepth_>0.0?cloudMaxDepth_:999999., cloudMaxDepth_>0.0?cloudMaxDepth_:999999.,
true); true);
//ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size()); //ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size());
if(jter->second->size())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed; *assembledCloud+=*transformed;
++count;
} }
} }
} }
else
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
if(assembledCloud->size())
{
*assembledCloud+=*transformed;
}
else
{
assembledCloud = transformed;
}
++count;
} }
} }
} }
@@ -626,13 +636,15 @@ void MapsManager::publishMaps(
ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks()); ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks());
if(assembledCloud->size())
{
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*assembledCloud, *cloudMsg); pcl::toROSMsg(*assembledCloud, *cloudMsg);
cloudMsg->header.stamp = stamp; cloudMsg->header.stamp = stamp;
cloudMsg->header.frame_id = mapFrameId; cloudMsg->header.frame_id = mapFrameId;
cloudMapPub_.publish(cloudMsg); cloudMapPub_.publish(cloudMsg);
} }
else if(poses.size()) else if(poses.size() - negativePoses.size())
{ {
ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size()); ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size());
} }