diff --git a/object_manager/src/dynamic_object_compute_mask_server.cpp b/object_manager/src/dynamic_object_compute_mask_server.cpp index 8e270816..cb5b1029 100644 --- a/object_manager/src/dynamic_object_compute_mask_server.cpp +++ b/object_manager/src/dynamic_object_compute_mask_server.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -12,7 +13,11 @@ #include #include #include -#include \ +#include +#include + +#include +#include typedef pcl::PointXYZRGB PointType; typedef semantic_map_load_utilties::DynamicObjectData ObjectData; @@ -22,11 +27,19 @@ typedef pcl::search::KdTree Tree; using namespace std; -//pcl::visualization::PCLVisualizer *p; +ros::ServiceClient surfel_client; +ros::ServiceClient registration_client; +pcl::visualization::PCLVisualizer *p; +std::string g_segmentation_method; + +std::vector compute_convex_segmentation(pcl::PointCloud::Ptr cloud_normals); + +CloudPtr compute_mask_per_view_from_object(CloudPtr view, CloudPtr object, + cv::Mat& mask_image, vector& mask_indices, double neighbor_distance); -CloudPtr compute_mask_per_view_conv_seg(CloudPtr view, CloudPtr object, CloudPtr prevMask, - cv::Mat& mask_image, vector& mask_indices); -CloudPtr rebuildCloud(std::vector intermediate_clouds, std::vector intermediate_transforms); + +CloudPtr find_object_using_metaroom(std::string sweep_xml, std::string object_xml, CloudPtr registered_views_cloud, CloudPtr object_cloud); +CloudPtr find_object_using_conv_seg(CloudPtr reg_views_at_origin, pcl::PointCloud::Ptr reg_views_at_origin_normals, CloudPtr object_cloud_at_origin); bool dynamic_object_compute_mask_service( object_manager::DynamicObjectComputeMaskService::Request &req, @@ -34,7 +47,7 @@ bool dynamic_object_compute_mask_service( { ROS_INFO("Received a dynamic object compute mask request"); ROS_INFO_STREAM("Object " << req.object_xml); - res.result = false; + ROS_INFO_STREAM("Observation "<(req.object_xml); @@ -68,7 +81,7 @@ bool dynamic_object_compute_mask_service( for (size_t i=0; iclearAdditionalViewMasks(); + // load observation data and transform object into the observation frame + // (originally it's in the frame of the second observation (or the metaroom's frame) that this observation is registered to) + auto observation_data = SimpleXMLParser::loadRoomFromXML(req.observation_xml,{"RoomCompleteCloud", "RoomIntermediateCloud"}, false, false); + pcl::transformPointCloud(*object.objectCloud, *object.objectCloud, Eigen::Matrix4f(observation_data.roomTransform.inverse())); + + // process the additional view clouds (surfelize!) + observation_registration_services::ProcessRegisteredViews srv; + for (size_t i=0; im_vAdditionalViews.size(); i++){ + // + sensor_msgs::PointCloud2 cloud_msg; + pcl::toROSMsg(*(original_object->m_vAdditionalViews[i]), cloud_msg); + srv.request.registered_views.push_back(cloud_msg); + + geometry_msgs::Transform transform_msg; + tf::Transform first_transform_inv = original_object->m_vAdditionalViewsTransformsRegistered[0].inverse(); + tf::Transform combined; combined.mult(first_transform_inv, original_object->m_vAdditionalViewsTransformsRegistered[i]); + + tf::transformTFToMsg(combined,transform_msg); + srv.request.registered_views_transforms.push_back(transform_msg); + } + // pass camera intrinsics to the surfelize server + image_geometry::PinholeCameraModel camParams; + if (observation_data.vIntermediateRoomCloudCamParamsCorrected.size()){ + camParams = observation_data.vIntermediateRoomCloudCamParamsCorrected[0]; + } else { + camParams = observation_data.vIntermediateRoomCloudCamParams[0]; + } + std_msgs::Float32 intrinsic; + intrinsic.data = (float)camParams.fx(); srv.request.intrinsics.push_back(intrinsic); + intrinsic.data = (float)camParams.fy(); srv.request.intrinsics.push_back(intrinsic); + intrinsic.data = (float)camParams.cx(); srv.request.intrinsics.push_back(intrinsic); + intrinsic.data = (float)camParams.cy(); srv.request.intrinsics.push_back(intrinsic); + + ROS_INFO_STREAM("Using surfelize_server service"); + CloudPtr object_views(new Cloud); + CloudPtr object_views_at_origin(new Cloud); + pcl::PointCloud::Ptr object_views_normals(new pcl::PointCloud); + bool surfelized = false; + if (surfel_client.call(srv)){ + CloudPtr surfelized_object_views(new Cloud); + pcl::PointCloud::Ptr processed_cloud(new pcl::PointCloud); + pcl::fromROSMsg(srv.response.processed_cloud, *processed_cloud); + // transform back into the observation frame + for (const pcl::PointXYZRGBNormal& pn : processed_cloud->points) { + PointType p; + p.getVector3fMap() = pn.getVector3fMap(); + p.rgba = pn.rgba; + pcl::Normal n; + n.getNormalVector3fMap() = pn.getNormalVector3fMap(); + object_views_normals->push_back(n); + surfelized_object_views->push_back(p); + } + *object_views_at_origin = *surfelized_object_views; + pcl_ros::transformPointCloud(*surfelized_object_views, *surfelized_object_views,object.vAdditionalViewsTransformsRegistered[0]); + pcl_ros::transformPointCloud(*surfelized_object_views, *surfelized_object_views,object.additionalViewsTransformToObservation); + *object_views = *surfelized_object_views; + surfelized = true; + } else { + ROS_ERROR_STREAM("Could not call surfelize_server to process the object views. Will proceed with the unfiltered views."); + for (CloudPtr view: registered_object_views){ + *object_views += *view; + } +// pcl_ros::transformPointCloud(*object_views, *object_views,object.additionalViewsTransformToObservation); + // downsample views + object_views = MetaRoom::downsampleCloud(object_views->makeShared()); + } + + CloudPtr segmented_object_cloud(new Cloud); + if ((g_segmentation_method == "convex_segmentation") && (surfelized)){ + // transform object cloud to the origin (needed for convex segmentation) + ROS_INFO_STREAM("Segmenting object using the convex segmentation"); + CloudPtr object_cloud_at_origin(new Cloud); + pcl_ros::transformPointCloud(*object.objectCloud, *object_cloud_at_origin,object.additionalViewsTransformToObservation.inverse()); + pcl_ros::transformPointCloud(*object_cloud_at_origin, *object_cloud_at_origin,object.vAdditionalViewsTransformsRegistered[0].inverse()); + + + segmented_object_cloud = find_object_using_conv_seg(object_views_at_origin, object_views_normals, object_cloud_at_origin); + // transform object from origin to observation frame of ref + pcl_ros::transformPointCloud(*segmented_object_cloud, *segmented_object_cloud,object.vAdditionalViewsTransformsRegistered[0]); + pcl_ros::transformPointCloud(*segmented_object_cloud, *segmented_object_cloud,object.additionalViewsTransformToObservation); + } else { + // default -> metaroom + ROS_INFO_STREAM("Segmenting object using the metaroom segmentation"); + segmented_object_cloud = find_object_using_metaroom(req.observation_xml, req.object_xml, object_views, object.objectCloud); + } // compute the mask per view CloudPtr prev_mask(new Cloud); ROS_INFO_STREAM("Computing masks for the additional views"); for (size_t i=0; i mask_indices; - CloudPtr mask = compute_mask_per_view_conv_seg(registered_view_to_origin, object_to_origin, prev_mask_to_origin, - mask_image, mask_indices); - - *prev_mask = *mask; - pcl_ros::transformPointCloud(*prev_mask, *prev_mask, object.vAdditionalViewsTransformsRegistered[i]); // transform to map frame - -// // visualize mask -// p->addPointCloud(registered_view_to_origin, "view"); -// pcl::visualization::PointCloudColorHandlerCustom mask_handler(mask, 0, 255,0); -// p->addPointCloud (mask,mask_handler,"mask"); -// p->spin(); -// p->removeAllPointClouds(); - -// // visualize mask image + double neighbor_distance = 0.001; + if (g_segmentation_method != "meta_room"){ + neighbor_distance = 0.00025; + } + CloudPtr mask = compute_mask_per_view_from_object(registered_view, segmented_object_cloud, mask_image, mask_indices, neighbor_distance); + // // visualize mask image // ROS_INFO_STREAM("Mask no indices "<("/surfelize_server"); + registration_client = n.serviceClient("/observation_registration_server"); +// g_segmentation_method = "convex_segmentation"; + g_segmentation_method = "meta_room"; // p = new pcl::visualization::PCLVisualizer (argc, argv, "Labelled data"); // p->addCoordinateSystem(); ros::ServiceServer service = n.advertiseService("dynamic_object_compute_mask_server", dynamic_object_compute_mask_service); + ROS_INFO("dynamic_object_compute_mask_server started."); + ROS_INFO_STREAM("Object segmentation method is "<& mask_indices){ - // // compute normals - // double radius = 0.05; - // boost::shared_ptr> normals(new pcl::PointCloud); - // pcl::NormalEstimationOMP normalEstimation; - // normalEstimation.setNumberOfThreads(2); - // normalEstimation.setInputCloud(filtered_view); - // normalEstimation.setRadiusSearch(radius); - // Tree::Ptr kdtree(new Tree); - // normalEstimation.setSearchMethod(kdtree); - // normalEstimation.compute(*normals); +CloudPtr compute_mask_per_view_from_object(CloudPtr view, CloudPtr object, + cv::Mat& mask_image, vector& mask_indices, double neighbor_distance){ + using namespace std; // mask image -> empty by default mask_image = cv::Mat::zeros(480, 640, CV_8UC3); + CloudPtr mask(new Cloud); + if (!object->points.size()){ + ROS_ERROR_STREAM("Could not find mask. The segmented object has 0 points."); + return mask; + } + + // compute mask + // find indices in original point cloud + std::vector nn_indices (1); + std::vector nn_distances (1); + typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (object); + + // Iterate through the source data set + for (int i = 0; i < static_cast (view->points.size ()); ++i) + { + if (!isFinite (view->points[i])) + continue; + // Search for the closest point in the target data set (number of neighbors to find = 1) + if (!tree->nearestKSearch (view->points[i], 1, nn_indices, nn_distances)) + { + PCL_WARN ("No neighbor found for point %zu (%f %f %f)!\n", i, view->points[i].x, view->points[i].y, view->points[i].z); + continue; + } - // find convex segments + if (nn_distances[0] < neighbor_distance) + { + mask_indices.push_back (i); + mask->push_back(object->points[nn_indices[0]]); + } + } + + // create mask image + for (int index : mask_indices) + { + pcl::PointXYZRGB point = view->points[index]; + int y = index / mask_image.cols; + int x = index % mask_image.cols; + mask_image.at(y, x)[0] = point.b; + mask_image.at(y, x)[1] = point.g; + mask_image.at(y, x)[2] = point.r; + } + + // visualize +// { +// pcl::visualization::PointCloudColorHandlerCustom view_handler(view, 255, 0,0); +// p->addPointCloud (view,view_handler,"view"); +// pcl::visualization::PointCloudColorHandlerCustom object_handler(object, 0, 255, 0); +// p->addPointCloud (object,object_handler,"object"); +// pcl::visualization::PointCloudColorHandlerCustom mask_handler(mask, 0, 255, 255); +// p->addPointCloud (mask,mask_handler,"mask"); +// p->spin(); +// p->removeAllPointClouds(); +// } + + +} + +std::vector compute_convex_segmentation(CloudPtr cloud, pcl::PointCloud::Ptr normals){ + + using Graph = supervoxel_segmentation::Graph; supervoxel_segmentation ss(0.02f, 0.2f, 0.4f, false); // do not filter - supervoxel_segmentation::Graph* g; - supervoxel_segmentation::Graph* convex_g; + Graph* g; + Graph* convex_g; vector supervoxels; vector convex_segments; map indices; - std::tie(g, convex_g, supervoxels, convex_segments, indices) = ss.compute_convex_oversegmentation(view, false);// normals, false); + std::tie(g, convex_g, supervoxels, convex_segments, indices) = ss.compute_convex_oversegmentation(cloud, normals, false); - // visualize segments -// for (auto seg : convex_segments){ -// // // visualize mask -// p->addPointCloud(view, "view"); -// pcl::visualization::PointCloudColorHandlerCustom seg_handler(seg, 0, 255,0); -// p->addPointCloud (seg,seg_handler,"seg"); + std::vector segments; + for (CloudPtr c : convex_segments) { + segments.push_back(c); + } -// pcl::visualization::PointCloudColorHandlerCustom mask_handler(object, 255, 0,0); -// p->addPointCloud (object,mask_handler,"mask"); + return segments; -// p->spin(); -// p->removeAllPointClouds(); -// } +} +CloudPtr find_object_using_conv_seg(CloudPtr reg_views_at_origin, pcl::PointCloud::Ptr reg_views_at_origin_normals, CloudPtr object_cloud_at_origin){ + using namespace std; + CloudPtr built_object_cloud(new Cloud); - CloudPtr mask(new Cloud); + // compute convex segmentation + vector convex_segments = compute_convex_segmentation(reg_views_at_origin, reg_views_at_origin_normals); + // transform initial set of inidices to the origin for (int i = 0; i < convex_segments.size(); ++i) { pcl::SegmentDifferences segment; - segment.setInputCloud(object); + segment.setInputCloud(object_cloud_at_origin); segment.setTargetCloud(convex_segments[i]); - segment.setDistanceThreshold(0.001); + segment.setDistanceThreshold(0.0001); typename Tree::Ptr tree (new pcl::search::KdTree); tree->setInputCloud (convex_segments[i]); segment.setSearchMethod(tree); CloudPtr remainder(new Cloud); segment.segment(*remainder); - if (remainder->points.size() < object->points.size()*0.9){ - *mask += *convex_segments[i]; + if (remainder->points.size() < object_cloud_at_origin->points.size()*0.9){ + *built_object_cloud += *convex_segments[i]; } } - if (!mask->points.size()){ - ROS_ERROR_STREAM("Could not find mask using the initial object indices. Will try with the previous mask"); - if (prev_mask->points.size()){ - for (int i = 0; i < convex_segments.size(); ++i) { - pcl::SegmentDifferences segment; - segment.setInputCloud(prev_mask); - segment.setTargetCloud(convex_segments[i]); - segment.setDistanceThreshold(0.001); - typename Tree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud (convex_segments[i]); - segment.setSearchMethod(tree); - CloudPtr remainder(new Cloud); - segment.segment(*remainder); - if (remainder->points.size() < object->points.size() * 0.9){ - *mask += *convex_segments[i]; - } - } - } + // visualize +// { +// pcl::visualization::PointCloudColorHandlerCustom views_handler(reg_views_at_origin, 0, 255,0); +// p->addPointCloud (reg_views_at_origin,views_handler,"views"); +// pcl::visualization::PointCloudColorHandlerCustom object_handler(object_cloud_at_origin, 0, 255, 255); +// p->addPointCloud (object_cloud_at_origin,object_handler,"object"); +// pcl::visualization::PointCloudColorHandlerCustom built_object_handler(built_object_cloud, 255, 0, 255); +// p->addPointCloud (built_object_cloud,built_object_handler,"built_object"); +// p->spin(); +// p->removeAllPointClouds(); +// } - } + return built_object_cloud; +} - if (!mask->points.size()){ - ROS_ERROR_STREAM("Could not find mask. "); - } else { - // find indices in original point cloud - std::vector nn_indices (1); - std::vector nn_distances (1); - typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud (mask); - - // Iterate through the source data set - for (int i = 0; i < static_cast (view->points.size ()); ++i) - { - if (!isFinite (view->points[i])) - continue; - // Search for the closest point in the target data set (number of neighbors to find = 1) - if (!tree->nearestKSearch (view->points[i], 1, nn_indices, nn_distances)) - { - PCL_WARN ("No neighbor found for point %zu (%f %f %f)!\n", i, view->points[i].x, view->points[i].y, view->points[i].z); - continue; - } +CloudPtr find_object_using_metaroom(std::string observation_xml, std::string object_xml, CloudPtr registered_views_cloud, CloudPtr object_cloud){ + using namespace std; + CloudPtr built_object_cloud(new Cloud); + // load observation + auto observation_data = SimpleXMLParser::loadRoomFromXML(observation_xml,{"RoomCompleteCloud"}, false, true); + auto observation_data_int_transforms = SimpleXMLParser::loadRoomFromXML(observation_xml,{"RoomIntermediateCloud"}, false, false); + + // find previous observation + string room_waypoint_id = observation_data.roomWaypointId; + string root_folder = SimpleXMLParser::getRootFolderFromSweepXml(observation_xml); + + if (root_folder == ""){ + passwd* pw = getpwuid(getuid()); + root_folder = std::string(pw->pw_dir); + root_folder+="/.semanticMap"; + } + ROS_INFO_STREAM("Looking for additional observations in "< matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint(root_folder, room_waypoint_id); + if (matchingObservations.size() < 2){ + ROS_ERROR_STREAM("Not enough observations to build metaroom."); + return built_object_cloud; + } + string previous_observation_xml = ""; + for (size_t i=0; ipoints[index]; - int y = index / mask_image.cols; - int x = index % mask_image.cols; - mask_image.at(y, x)[0] = point.b; - mask_image.at(y, x)[1] = point.g; - mask_image.at(y, x)[2] = point.r; + if (registration_client.call(srv)) + { + ROS_INFO_STREAM("Registration done using the observation_registration_server service. Number of constraints "< get registered transform + tf::Transform tf_registered_transform; + tf::transformMsgToTF(srv.response.transform, tf_registered_transform); + pcl_ros::transformAsMatrix(tf_registered_transform, registered_transform); } - + } else { + ROS_ERROR_STREAM("Could not call observation_registration_server service."); } - return mask; + if (registered_transform.isIdentity(0.001)){ + ROS_ERROR_STREAM("Registration failed while constructing meta-room"); + return built_object_cloud; + } + auto previous_observation_data = SimpleXMLParser::loadRoomFromXML(previous_observation_xml,{"RoomCompleteCloud"}, false, true); + CloudPtr observation_interior_cloud = MetaRoom::downsampleCloud(observation_data.completeRoomCloud->makeShared()); + CloudPtr previous_interior_cloud = MetaRoom::downsampleCloud(previous_observation_data.completeRoomCloud->makeShared()); + pcl::transformPointCloud(*previous_interior_cloud, *previous_interior_cloud,registered_transform); + + // compute differences and update metaroom + CloudPtr differenceRoomToPrevRoom(new Cloud); + CloudPtr differencePrevRoomToRoom(new Cloud); + // compute the differences + pcl::SegmentDifferences segment; + segment.setInputCloud(observation_interior_cloud); + segment.setTargetCloud(previous_interior_cloud); + segment.setDistanceThreshold(0.001); + typename Tree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (previous_interior_cloud); + segment.setSearchMethod(tree); + segment.segment(*differenceRoomToPrevRoom); + + segment.setInputCloud(previous_interior_cloud); + segment.setTargetCloud(observation_interior_cloud); + tree->setInputCloud(observation_interior_cloud); + segment.segment(*differencePrevRoomToRoom); + + CloudPtr toBeAdded(new Cloud()); + CloudPtr toBeRemoved(new Cloud()); + + OcclusionChecker occlusionChecker; + occlusionChecker.setSensorOrigin(observation_data_int_transforms.vIntermediateRoomCloudTransforms[0].getOrigin()); // since it's already transformed + auto occlusions = occlusionChecker.checkOcclusions(differenceRoomToPrevRoom, differencePrevRoomToRoom,720 ); + + CloudPtr meta_room(new Cloud); + segment.setInputCloud(observation_interior_cloud); + segment.setTargetCloud(differenceRoomToPrevRoom); + tree->setInputCloud(differenceRoomToPrevRoom); + segment.segment(*meta_room); + if (occlusions.toBeAdded->points.size()){ + *meta_room += *occlusions.toBeAdded; + } -} + CloudPtr dynamic_clusters(new Cloud); + segment.setInputCloud(registered_views_cloud); + segment.setTargetCloud(meta_room); + tree->setInputCloud(meta_room); + segment.setDistanceThreshold(0.001); + segment.segment(*dynamic_clusters); + + // find object cluster + std::vector vClusters = MetaRoom::clusterPointCloud(dynamic_clusters,0.02,50,1000000); + for (int i = 0; i < vClusters.size(); ++i) { + segment.setInputCloud(object_cloud); + segment.setTargetCloud(vClusters[i]); + tree->setInputCloud (vClusters[i]); + CloudPtr remainder(new Cloud); + segment.segment(*remainder); + if (remainder->points.size() < object_cloud->points.size()*0.9){ + *built_object_cloud += *vClusters[i]; + } + } -CloudPtr rebuildCloud(std::vector intermediate_clouds, std::vector intermediate_transforms){ - CloudPtr mergedCloud(new Cloud); + // // visualize +// { +// pcl::visualization::PointCloudColorHandlerCustom meta_room_handler(meta_room, 255, 0,0); +// p->addPointCloud (meta_room,meta_room_handler,"meta_room"); +// pcl::visualization::PointCloudColorHandlerCustom views_handler(registered_views_cloud, 0, 255,0); +// p->addPointCloud (registered_views_cloud,views_handler,"views"); +// pcl::visualization::PointCloudColorHandlerCustom clusters_handler(dynamic_clusters, 0, 0, 255); +// p->addPointCloud (dynamic_clusters,clusters_handler,"clusters"); +// pcl::visualization::PointCloudColorHandlerCustom object_handler(object_cloud, 0, 255, 255); +// p->addPointCloud (object_cloud,object_handler,"object"); +// pcl::visualization::PointCloudColorHandlerCustom built_object_handler(built_object_cloud, 255, 0, 255); +// p->addPointCloud (built_object_cloud,built_object_handler,"built_object"); +// p->spin(); +// p->removeAllPointClouds(); +// } - for (size_t i=0; ipoints.size()<