From f9dc100c0890d3090c9c4e2e5ee9e601c9228b77 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 26 Dec 2024 13:29:52 +0000 Subject: [PATCH] [ros-o] boost::make_shared -> std::make_shared, use .makeShared() --- .../container_occupancy_detector.h | 7 +++++ .../jsk_pcl_ros/particle_filter_tracking.h | 2 +- .../jsk_pcl_ros/target_adaptive_tracking.h | 1 + .../src/edgebased_cube_finder_nodelet.cpp | 4 +++ .../euclidean_cluster_extraction_nodelet.cpp | 15 ++++++++-- jsk_pcl_ros/src/icp_registration_nodelet.cpp | 14 ++++++++++ .../moving_least_square_smoothing_nodelet.cpp | 3 ++ ...nized_multi_plane_segmentation_nodelet.cpp | 28 +++++++++++++++++++ .../src/particle_filter_tracking_nodelet.cpp | 9 ++---- .../pointcloud_database_server_nodelet.cpp | 1 + .../src/pointcloud_screenpoint_nodelet.cpp | 7 ++--- ...ng_multiple_plane_segmentation_nodelet.cpp | 4 +++ .../region_growing_segmentation_nodelet.cpp | 4 +++ .../src/resize_points_publisher_nodelet.cpp | 4 +++ .../src/supervoxel_segmentation_nodelet.cpp | 8 ++++-- .../src/target_adaptive_tracking_nodelet.cpp | 16 ++++++++--- 16 files changed, 108 insertions(+), 19 deletions(-) diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/container_occupancy_detector.h b/jsk_pcl_ros/include/jsk_pcl_ros/container_occupancy_detector.h index d05afe36cb..6d1e2b15a3 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/container_occupancy_detector.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/container_occupancy_detector.h @@ -107,10 +107,17 @@ namespace jsk_pcl_ros{ tf2_ros::TransformListener* tf_listener_; sensor_msgs::PointCloud2::Ptr transformed_points_msg_ = boost::shared_ptr(new sensor_msgs::PointCloud2); +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::PCLPointCloud2Ptr pcl_pc2_ptr_ = + std::shared_ptr(new pcl::PCLPointCloud2); + pcl::PointCloud::Ptr pcl_xyz_ptr_ = + std::shared_ptr>(new pcl::PointCloud); +#else pcl::PCLPointCloud2Ptr pcl_pc2_ptr_ = boost::shared_ptr(new pcl::PCLPointCloud2); pcl::PointCloud::Ptr pcl_xyz_ptr_ = boost::shared_ptr>(new pcl::PointCloud); +#endif //////////////////////////////////////////////////////// // Diagnostics Variables diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h b/jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h index f4c85d0476..b92be9bcfd 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h @@ -186,7 +186,7 @@ namespace pcl // } if (!change_detector_) - change_detector_ = boost::shared_ptr >(new pcl::octree::OctreePointCloudChangeDetector (change_detector_resolution_)); + pcl::octree::OctreePointCloudChangeDetector *change_detector_ = new pcl::octree::OctreePointCloudChangeDetector (change_detector_resolution_); if (!particles_ || particles_->points.empty ()) initParticles (true); diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/target_adaptive_tracking.h b/jsk_pcl_ros/include/jsk_pcl_ros/target_adaptive_tracking.h index 73bde4f7e5..c85c0469a9 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/target_adaptive_tracking.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/target_adaptive_tracking.h @@ -42,6 +42,7 @@ #include #include #include +#include // for computePairFeatures #include #include #include diff --git a/jsk_pcl_ros/src/edgebased_cube_finder_nodelet.cpp b/jsk_pcl_ros/src/edgebased_cube_finder_nodelet.cpp index fba7929ed7..c66f3789d0 100644 --- a/jsk_pcl_ros/src/edgebased_cube_finder_nodelet.cpp +++ b/jsk_pcl_ros/src/edgebased_cube_finder_nodelet.cpp @@ -756,7 +756,11 @@ namespace jsk_pcl_ros *points_on_edges = *points_on_edges + *points_on_edge; cubes.push_back(cube); } +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pub_debug_filtered_cloud_.publish(*points_on_edges); +#else pub_debug_filtered_cloud_.publish(points_on_edges); +#endif } } diff --git a/jsk_pcl_ros/src/euclidean_cluster_extraction_nodelet.cpp b/jsk_pcl_ros/src/euclidean_cluster_extraction_nodelet.cpp index e1cac93e4d..a655d0a236 100644 --- a/jsk_pcl_ros/src/euclidean_cluster_extraction_nodelet.cpp +++ b/jsk_pcl_ros/src/euclidean_cluster_extraction_nodelet.cpp @@ -140,7 +140,9 @@ namespace jsk_pcl_ros EuclideanClusterExtraction impl; if (filtered_cloud->points.size() > 0) { jsk_topic_tools::ScopedTimer timer = kdtree_acc_.scopedTimer(); -#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 ) +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree()); +#elif ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 ) pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); tree = boost::make_shared< pcl::search::KdTree > (); #else @@ -332,7 +334,9 @@ namespace jsk_pcl_ros pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::fromROSMsg(req.input, *cloud); -#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 ) +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); +#elif ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 ) pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); tree = boost::make_shared< pcl::search::KdTree > (); #else @@ -361,12 +365,19 @@ namespace jsk_pcl_ros pcl_conversions::toPCL(req.input, *pcl_cloud); pcl::ExtractIndices ex; ex.setInputCloud(pcl_cloud); +#elif ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::ExtractIndices ex; + ex.setInputCloud ( req.input.makeShared() ); #else pcl::ExtractIndices ex; ex.setInputCloud ( boost::make_shared< sensor_msgs::PointCloud2 > (req.input) ); #endif for ( size_t i = 0; i < cluster_indices.size(); i++ ) { +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + ex.setIndices ( std::make_shared< pcl::PointIndices > (cluster_indices[i]) ); +#else ex.setIndices ( boost::make_shared< pcl::PointIndices > (cluster_indices[i]) ); +#endif ex.setNegative ( false ); #if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 ) pcl::PCLPointCloud2 output_cloud; diff --git a/jsk_pcl_ros/src/icp_registration_nodelet.cpp b/jsk_pcl_ros/src/icp_registration_nodelet.cpp index 016a862326..3fb2195122 100644 --- a/jsk_pcl_ros/src/icp_registration_nodelet.cpp +++ b/jsk_pcl_ros/src/icp_registration_nodelet.cpp @@ -592,6 +592,12 @@ namespace jsk_pcl_ros pcl::PointCloud::Ptr& output_cloud, Eigen::Affine3d& output_transform) { +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::GeneralizedIterativeClosestPoint icp; + icp.setRotationEpsilon(rotation_epsilon_); + icp.setCorrespondenceRandomness(correspondence_randomness_); + icp.setMaximumOptimizerIterations(maximum_optimizer_iterations_); +#else pcl::IterativeClosestPoint icp; if (use_normal_) { pcl::IterativeClosestPointWithNormals icp_with_normal; @@ -606,6 +612,7 @@ namespace jsk_pcl_ros gicp.setMaximumOptimizerIterations(maximum_optimizer_iterations_); icp = gicp; } +#endif if (correspondence_algorithm_ == 1) { // Projective if (!camera_info_msg_) { NODELET_ERROR("no camera info is available yet"); @@ -636,10 +643,17 @@ namespace jsk_pcl_ros icp.setInputTarget(cloud); if (transform_3dof_) { +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::registration::WarpPointRigid3D::Ptr warp_func + (new pcl::registration::WarpPointRigid3D); + pcl::registration::TransformationEstimationLM::Ptr te + (new pcl::registration::TransformationEstimationLM); +#else boost::shared_ptr > warp_func (new pcl::registration::WarpPointRigid3D); boost::shared_ptr > te (new pcl::registration::TransformationEstimationLM); +#endif te->setWarpFunction(warp_func); icp.setTransformationEstimation(te); } diff --git a/jsk_pcl_ros/src/moving_least_square_smoothing_nodelet.cpp b/jsk_pcl_ros/src/moving_least_square_smoothing_nodelet.cpp index e01ea5ae4d..93d41a7299 100644 --- a/jsk_pcl_ros/src/moving_least_square_smoothing_nodelet.cpp +++ b/jsk_pcl_ros/src/moving_least_square_smoothing_nodelet.cpp @@ -39,6 +39,7 @@ #include #include "jsk_recognition_utils/pcl_conversion_util.h" +#include // for KdTree namespace jsk_pcl_ros { @@ -56,7 +57,9 @@ namespace jsk_pcl_ros pcl::MovingLeastSquares smoother; smoother.setSearchRadius (search_radius_); if (gauss_param_set_) smoother.setSqrGaussParam (gauss_param_set_); +#if PCL_VERSION_COMPARE (<, 1, 9, 0) // https://github.com/PointCloudLibrary/pcl/blob/cc08f815a9a5f2a1d3f897fb2bc9d541635792c9/CHANGES.md?plain=1#L1633 smoother.setPolynomialFit (use_polynomial_fit_); +#endif smoother.setPolynomialOrder (polynomial_order_); smoother.setComputeNormals (calc_normal_); diff --git a/jsk_pcl_ros/src/organized_multi_plane_segmentation_nodelet.cpp b/jsk_pcl_ros/src/organized_multi_plane_segmentation_nodelet.cpp index d04bd9b521..3a7841b922 100644 --- a/jsk_pcl_ros/src/organized_multi_plane_segmentation_nodelet.cpp +++ b/jsk_pcl_ros/src/organized_multi_plane_segmentation_nodelet.cpp @@ -246,10 +246,17 @@ namespace jsk_pcl_ros // compute the distance between two boundaries. // if they are near enough, we can regard these two map should connect +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::PointIndices::Ptr a_indices + = std::make_shared(boundary_indices[i]); + pcl::PointIndices::Ptr b_indices + = std::make_shared(boundary_indices[j]); +#else pcl::PointIndices::Ptr a_indices = boost::make_shared(boundary_indices[i]); pcl::PointIndices::Ptr b_indices = boost::make_shared(boundary_indices[j]); +#endif pcl::PointCloud a_cloud, b_cloud; extract.setIndices(a_indices); extract.filter(a_cloud); @@ -364,10 +371,17 @@ namespace jsk_pcl_ros pcl::ModelCoefficients pcl_new_coefficients; pcl_new_coefficients.values = new_coefficients; // estimate concave hull +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::PointIndices::Ptr indices_ptr + = std::make_shared(one_boundaries); + pcl::ModelCoefficients::Ptr coefficients_ptr + = std::make_shared(pcl_new_coefficients); +#else pcl::PointIndices::Ptr indices_ptr = boost::make_shared(one_boundaries); pcl::ModelCoefficients::Ptr coefficients_ptr = boost::make_shared(pcl_new_coefficients); +#endif jsk_recognition_utils::ConvexPolygon::Ptr convex = jsk_recognition_utils::convexFromCoefficientsAndInliers( input, indices_ptr, coefficients_ptr); @@ -478,7 +492,11 @@ namespace jsk_pcl_ros for (size_t i = 0; i < boundary_indices.size(); i++) { pcl::PointCloud boundary_cloud; pcl::PointIndices boundary_one_indices = boundary_indices[i]; +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::PointIndices::Ptr indices_ptr = std::make_shared(boundary_indices[i]); +#else pcl::PointIndices::Ptr indices_ptr = boost::make_shared(boundary_indices[i]); +#endif extract.setIndices(indices_ptr); extract.filter(boundary_cloud); boundaries.push_back(boundary_cloud); @@ -510,8 +528,13 @@ namespace jsk_pcl_ros //////////////////////////////////////////////////////// jsk_recognition_utils::Vertices centroids; for (size_t i = 0; i < inliers.size(); i++) { +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::PointIndices::Ptr target_inliers + = std::make_shared(inliers[i]); +#else pcl::PointIndices::Ptr target_inliers = boost::make_shared(inliers[i]); +#endif pcl::PointCloud::Ptr target_cloud (new pcl::PointCloud); Eigen::Vector4f centroid; pcl::ExtractIndices ex; @@ -640,8 +663,13 @@ namespace jsk_pcl_ros jsk_topic_tools::ScopedTimer timer = ransac_refinement_time_acc_.scopedTimer(); for (size_t i = 0; i < input_indices.size(); i++) { +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::PointIndices::Ptr input_indices_ptr + = std::make_shared(input_indices[i]); +#else pcl::PointIndices::Ptr input_indices_ptr = boost::make_shared(input_indices[i]); +#endif Eigen::Vector3f normal(input_coefficients[i].values[0], input_coefficients[i].values[1], input_coefficients[i].values[2]); diff --git a/jsk_pcl_ros/src/particle_filter_tracking_nodelet.cpp b/jsk_pcl_ros/src/particle_filter_tracking_nodelet.cpp index 9c55c45728..43b27ade27 100644 --- a/jsk_pcl_ros/src/particle_filter_tracking_nodelet.cpp +++ b/jsk_pcl_ros/src/particle_filter_tracking_nodelet.cpp @@ -145,19 +145,16 @@ namespace jsk_pcl_ros } - boost::shared_ptr > - distance_coherence(new DistanceCoherence); + DistanceCoherence::Ptr distance_coherence(new DistanceCoherence); coherence->addPointCoherence(distance_coherence); //add HSV coherence if (use_hsv) { - boost::shared_ptr > hsv_color_coherence - = boost::shared_ptr >(new HSVColorCoherence()); + HSVColorCoherence::Ptr hsv_color_coherence(new HSVColorCoherence()); coherence->addPointCoherence(hsv_color_coherence); } - boost::shared_ptr > search - (new pcl::search::Octree(octree_resolution)); + pcl::search::Octree::Ptr search(new pcl::search::Octree(octree_resolution)); //boost::shared_ptr > search(new pcl::search::KdTree()); coherence->setSearchMethod(search); double max_distance; diff --git a/jsk_pcl_ros/src/pointcloud_database_server_nodelet.cpp b/jsk_pcl_ros/src/pointcloud_database_server_nodelet.cpp index fa7ae6acf0..4eb3dcdc72 100644 --- a/jsk_pcl_ros/src/pointcloud_database_server_nodelet.cpp +++ b/jsk_pcl_ros/src/pointcloud_database_server_nodelet.cpp @@ -39,6 +39,7 @@ #include #include #include "jsk_recognition_utils/pcl_conversion_util.h" +#include namespace jsk_pcl_ros { diff --git a/jsk_pcl_ros/src/pointcloud_screenpoint_nodelet.cpp b/jsk_pcl_ros/src/pointcloud_screenpoint_nodelet.cpp index 4f1a334924..c27bb84c83 100644 --- a/jsk_pcl_ros/src/pointcloud_screenpoint_nodelet.cpp +++ b/jsk_pcl_ros/src/pointcloud_screenpoint_nodelet.cpp @@ -49,7 +49,7 @@ void PointcloudScreenpoint::onInit() ConnectionBasedNodelet::onInit(); - normals_tree_ = boost::make_shared< pcl::search::KdTree< pcl::PointXYZ > > (); + pcl::search::KdTree::Ptr normals_tree_(new pcl::search::KdTree); n3d_.setSearchMethod (normals_tree_); dyn_srv_ = boost::make_shared< dynamic_reconfigure::Server< Config > >(*pnh_); @@ -257,8 +257,7 @@ bool PointcloudScreenpoint::screenpoint_cb ( } // search normal - n3d_.setSearchSurface( - boost::make_shared > (latest_cloud_)); + n3d_.setSearchSurface(latest_cloud_.makeShared()); pcl::PointCloud cloud_; pcl::PointXYZ pt; @@ -268,7 +267,7 @@ bool PointcloudScreenpoint::screenpoint_cb ( cloud_.points.resize(0); cloud_.points.push_back(pt); - n3d_.setInputCloud (boost::make_shared > (cloud_)); + n3d_.setInputCloud (cloud_.makeShared()); pcl::PointCloud cloud_normals; n3d_.compute (cloud_normals); diff --git a/jsk_pcl_ros/src/region_growing_multiple_plane_segmentation_nodelet.cpp b/jsk_pcl_ros/src/region_growing_multiple_plane_segmentation_nodelet.cpp index 05381374b5..be966a4ed5 100644 --- a/jsk_pcl_ros/src/region_growing_multiple_plane_segmentation_nodelet.cpp +++ b/jsk_pcl_ros/src/region_growing_multiple_plane_segmentation_nodelet.cpp @@ -204,7 +204,11 @@ namespace jsk_pcl_ros for (size_t i = 0; i < clusters->size(); i++) { pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients); +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::PointIndices::Ptr cluster = std::make_shared((*clusters)[i]); +#else pcl::PointIndices::Ptr cluster = boost::make_shared((*clusters)[i]); +#endif ransacEstimation(cloud, cluster, *plane_inliers, *plane_coefficients); if (plane_inliers->indices.size() > 0) { diff --git a/jsk_pcl_ros/src/region_growing_segmentation_nodelet.cpp b/jsk_pcl_ros/src/region_growing_segmentation_nodelet.cpp index c373f2d7c4..de6850bce1 100644 --- a/jsk_pcl_ros/src/region_growing_segmentation_nodelet.cpp +++ b/jsk_pcl_ros/src/region_growing_segmentation_nodelet.cpp @@ -89,7 +89,11 @@ namespace jsk_pcl_ros void RegionGrowingSegmentation::segment(const sensor_msgs::PointCloud2::ConstPtr& msg) { boost::mutex::scoped_lock lock(mutex_); +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + pcl::search::Search::Ptr tree(new pcl::search::KdTree()); +#else pcl::search::Search::Ptr tree = boost::shared_ptr > (new pcl::search::KdTree); +#endif pcl::PointCloud cloud; pcl::fromROSMsg(*msg, cloud); diff --git a/jsk_pcl_ros/src/resize_points_publisher_nodelet.cpp b/jsk_pcl_ros/src/resize_points_publisher_nodelet.cpp index 9eeb105a89..ebb35d45ec 100644 --- a/jsk_pcl_ros/src/resize_points_publisher_nodelet.cpp +++ b/jsk_pcl_ros/src/resize_points_publisher_nodelet.cpp @@ -184,7 +184,11 @@ namespace jsk_pcl_ros } pcl::ExtractIndices extract; extract.setInputCloud (pcl_input_cloud.makeShared()); +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + extract.setIndices (std::make_shared > (ex_indices)); +#else extract.setIndices (boost::make_shared > (ex_indices)); +#endif extract.setNegative (false); extract.filter (output); diff --git a/jsk_pcl_ros/src/supervoxel_segmentation_nodelet.cpp b/jsk_pcl_ros/src/supervoxel_segmentation_nodelet.cpp index 3eba32ee8c..2c0eb57b19 100644 --- a/jsk_pcl_ros/src/supervoxel_segmentation_nodelet.cpp +++ b/jsk_pcl_ros/src/supervoxel_segmentation_nodelet.cpp @@ -72,8 +72,12 @@ namespace jsk_pcl_ros pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::fromROSMsg(*cloud_msg, *cloud); pcl::SupervoxelClustering super(voxel_resolution_, - seed_resolution_, - use_transform_); + seed_resolution_ +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + ); +#else + , use_transform_); +#endif super.setInputCloud(cloud); super.setColorImportance(color_importance_); super.setSpatialImportance(spatial_importance_); diff --git a/jsk_pcl_ros/src/target_adaptive_tracking_nodelet.cpp b/jsk_pcl_ros/src/target_adaptive_tracking_nodelet.cpp index 3f0126b080..8ddca9f8bb 100644 --- a/jsk_pcl_ros/src/target_adaptive_tracking_nodelet.cpp +++ b/jsk_pcl_ros/src/target_adaptive_tracking_nodelet.cpp @@ -1782,8 +1782,12 @@ namespace jsk_pcl_ros boost::mutex::scoped_lock lock(mutex_); pcl::SupervoxelClustering super( voxel_resolution_, - static_cast(seed_resolution), - use_transform_); + static_cast(seed_resolution) +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + ); +#else + , use_transform_); +#endif super.setInputCloud(cloud); super.setColorImportance(color_importance_); super.setSpatialImportance(spatial_importance_); @@ -1804,8 +1808,12 @@ namespace jsk_pcl_ros } boost::mutex::scoped_lock lock(mutex_); pcl::SupervoxelClustering super(voxel_resolution_, - seed_resolution_, - use_transform_); + seed_resolution_ +#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 ) + ); +#else + , use_transform_); +#endif super.setInputCloud(cloud); super.setColorImportance(color_importance_); super.setSpatialImportance(spatial_importance_);