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

MapCloud: auto refresh clouds when rtabmap is reset

Browse Source
This commit is contained in:
matlabbe
2016-11-09 16:21:06 -05:00
parent 2c326714ec
commit 8eb164f047
1 changed files with 42 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/MapCloudDisplay.cpp
+42 -43
View File
@@ -272,58 +272,56 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{
int id = map.nodes[i].id;
if(poses.find(id) != poses.end() &&
cloud_infos_.find(id) == cloud_infos_.end())
// Always refresh the cloud if there are data
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection()))
{
// Cloud not added to RVIZ, add it!
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection()))
cv::Mat image, depth;
s.sensorData().uncompressData(&image, &depth, 0);
if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
{
cv::Mat image, depth;
s.sensorData().uncompressData(&image, &depth, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr validIndices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(
s.sensorData(),
cloud_decimation_->getInt(),
cloud_max_depth_->getFloat(),
cloud_min_depth_->getFloat(),
validIndices.get());
if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
if(cloud_voxel_size_->getFloat())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr validIndices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(
s.sensorData(),
cloud_decimation_->getInt(),
cloud_max_depth_->getFloat(),
cloud_min_depth_->getFloat(),
validIndices.get());
cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
}
if(cloud_voxel_size_->getFloat())
if(cloud->size())
{
if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
cloud = rtabmap::util3d::passThrough(cloud, "z",
cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
}
if(cloud->size())
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*cloud, *cloudMsg);
cloudMsg->header = map.header;
CloudInfoPtr info(new CloudInfo);
info->message_ = cloudMsg;
info->pose_ = rtabmap::Transform::getIdentity();
info->id_ = id;
if (transformCloud(info, true))
{
if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::passThrough(cloud, "z",
cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*cloud, *cloudMsg);
cloudMsg->header = map.header;
CloudInfoPtr info(new CloudInfo);
info->message_ = cloudMsg;
info->pose_ = rtabmap::Transform::getIdentity();
info->id_ = id;
if (transformCloud(info, true))
{
boost::mutex::scoped_lock lock(new_clouds_mutex_);
new_cloud_infos_.insert(std::make_pair(id, info));
}
boost::mutex::scoped_lock lock(new_clouds_mutex_);
new_cloud_infos_.erase(id);
new_cloud_infos_.insert(std::make_pair(id, info));
}
}
}
@@ -633,6 +631,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
cloud_info->scene_node_->setVisible(false);
cloud_infos_.erase(it->first);
cloud_infos_.insert(*it);
}