/**
* Register cloud to the octomap
*/
bool srs_env_model::CPcToOcRegistration::registerCloud( tPointCloudPtr & cloud, const SMapWithParameters & map )
{
if( !m_registration.isRegistering() )
{
ROS_ERROR( "No registration method selected." );
return false;
}
// std::cerr << "Reg start" << std::endl;
// Get patch
m_patchMaker.setCloudFrameId( map.frameId );
m_patchMaker.computeCloud( map, cloud->header.stamp );
if( m_patchMaker.getCloud().size() == 0 )
return false;
// Resample input cloud
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2 ());
pcl::toROSMsg( *cloud, *cloudMsg);
// std::cerr << "Patch size: " << m_patchMaker.getCloud().size() << ", " << cloudMsg->data.size() << std::endl;
m_gridFilter.setInputCloud( cloudMsg );
m_gridFilter.setLeafSize( map.resolution, map.resolution, map.resolution );
m_gridFilter.filter( *m_resampledCloud );
// std::cerr << "Voxel grid size: " << m_resampledCloud->data.size() << std::endl;
// Try to register cloud
tPointCloudPtr cloudSource( new tPointCloud() );
tPointCloudPtr cloudBuffer( new tPointCloud() );
tPointCloudPtr cloudTarget( new tPointCloud() );
pcl::fromROSMsg( *m_resampledCloud, *cloudSource );
pcl::copyPointCloud( m_patchMaker.getCloud(), *cloudTarget );
// std::cerr << "Calling registration: " << cloudSource->size() << ", " << cloudTarget->size() << std::endl;
bool rv( m_registration.process( cloudSource, cloudTarget, cloudBuffer ) );
if( ! rv )
std::cerr << "Registration failed" << std::endl;
return rv;
}
void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
{
// Add new clouds...
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{
int id = map.nodes[i].id;
if(cloud_infos_.find(id) == cloud_infos_.end())
{
// 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())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = rtabmap::util3d::cloudRGBFromSensorData(
s.sensorData(),
cloud_decimation_->getInt(),
cloud_max_depth_->getFloat(),
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::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));
}
}
}
}
}
}
// Update graph
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
{
poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
}
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
{
poses = rtabmap::graph::radiusPosesFiltering(poses,
node_filtering_radius_->getFloat(),
node_filtering_angle_->getFloat()*CV_PI/180.0);
}
{
boost::mutex::scoped_lock lock(current_map_mutex_);
current_map_ = poses;
}
}