i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

cloud_map: Showing negative clouds even if map is empty

Browse Source
This commit is contained in:
matlabbe
2016-06-17 15:03:29 -04:00
parent 99eb696e36
commit e6135d70ee
1 changed files with 49 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapsManager.cpp
+49 -37
View File
@@ -577,62 +577,74 @@ void MapsManager::publishMaps(
} }
} }
if(assembledCloud->size()) for(std::list<std::pair<int, Transform> >::reverse_iterator iter=negativePoses.rbegin(); iter!=negativePoses.rend(); ++iter)
{ {
if(cloudFrustumCulling_ && negativePoses.size()) std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
if(jter != clouds_.end() && jter->second->size())
{ {
for(std::list<std::pair<int, Transform> >::reverse_iterator iter=negativePoses.rbegin(); iter!=negativePoses.rend(); ++iter) std::map<int, std::vector<CameraModel> >::iterator kter = cameraModels_.find(iter->first);
if(cloudFrustumCulling_ && kter != cameraModels_.end() && assembledCloud->size())
{ {
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first); for(unsigned int i=0; i<kter->second.size(); ++i)
std::map<int, std::vector<CameraModel> >::iterator kter = cameraModels_.find(iter->first);
if(jter != clouds_.end() && kter != cameraModels_.end())
{ {
for(unsigned int i=0; i<kter->second.size(); ++i) if(kter->second[i].isValidForProjection())
{ {
if(kter->second[i].isValidForProjection()) assembledCloud = util3d::frustumFiltering(
{ assembledCloud,
int size = assembledCloud->size(); iter->second, // FIXME: should include camera local transform
assembledCloud = util3d::frustumFiltering( kter->second[i].horizontalFOV(),
assembledCloud, kter->second[i].verticalFOV(),
iter->second, // FIXME: should include camera local transform 0.0f,
kter->second[i].horizontalFOV(), cloudMaxDepth_>0.0?cloudMaxDepth_:999999.,
kter->second[i].verticalFOV(), true);
0.0f, //ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size());
cloudMaxDepth_>0.0?cloudMaxDepth_:999999.,
true); pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
//ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size()); *assembledCloud+=*transformed;
if(jter->second->size()) ++count;
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed;
}
}
} }
} }
} }
else
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
if(assembledCloud->size())
{
*assembledCloud+=*transformed;
}
else
{
assembledCloud = transformed;
}
++count;
}
} }
}
if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0)) if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0))
{ {
assembledCloud = util3d::passThrough(assembledCloud, "z", assembledCloud = util3d::passThrough(assembledCloud, "z",
cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0, cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0,
cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0); cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0);
} }
if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_) if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
{ {
assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_); assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_);
} }
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());
} }