From 6dd7d1ee7cbc7399f98807f548f7c012ef9bb6a9 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 31 Oct 2024 13:59:24 +0900 Subject: [PATCH] fix: attempted to address non freed memory (#206) * fix: attempted to address non freed memory Signed-off-by: Kenzo Lobos-Tsunekawa * chore: deleted an unused method Signed-off-by: Kenzo Lobos-Tsunekawa * chore: typos Signed-off-by: Kenzo Lobos-Tsunekawa --------- Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lidar_calibrator.hpp | 12 +- .../mapping_based_calibrator.hpp | 5 + .../src/lidar_calibrator.cpp | 121 ++++++++++-------- .../src/mapping_based_calibrator.cpp | 79 +++++++++++- .../mapping_based_calibrator/src/utils.cpp | 15 ++- 5 files changed, 164 insertions(+), 68 deletions(-) diff --git a/calibrators/mapping_based_calibrator/include/mapping_based_calibrator/lidar_calibrator.hpp b/calibrators/mapping_based_calibrator/include/mapping_based_calibrator/lidar_calibrator.hpp index 7c2e18aa..50dd9d48 100644 --- a/calibrators/mapping_based_calibrator/include/mapping_based_calibrator/lidar_calibrator.hpp +++ b/calibrators/mapping_based_calibrator/include/mapping_based_calibrator/lidar_calibrator.hpp @@ -51,6 +51,8 @@ class LidarCalibrator : public SensorCalibrator PointPublisher::SharedPtr & calibrated_source_aligned_map_pub, PointPublisher::SharedPtr & target_map_pub); + ~LidarCalibrator(); + /*! * Calibrate the sensor * @returns a tuple containing the calibration success status, the transform, and a score @@ -63,14 +65,6 @@ class LidarCalibrator : public SensorCalibrator void configureCalibrators() override; protected: - /*! - * Prepare calibrators for a specific pair of pointclouds - * @param[in] source_pointcloud_ptr Source pointcloud - * @param[in] target_pointcloud_ptr Target pointcloud - */ - void setUpCalibrators( - PointcloudType::Ptr & source_pointcloud_ptr, PointcloudType::Ptr & target_pointcloud_ptr); - // General methods /*! @@ -107,6 +101,8 @@ class LidarCalibrator : public SensorCalibrator // Calibration std::vector::Ptr> calibration_registrators_; + pcl::search::KdTree::Ptr target_kdtree_; + std::vector::Ptr> calibration_batch_registrators_; // cSpell:ignore pclomp diff --git a/calibrators/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp b/calibrators/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp index 1f0ed12c..ab187c0d 100644 --- a/calibrators/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp +++ b/calibrators/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp @@ -91,6 +91,11 @@ class ExtrinsicMappingBasedCalibrator : public rclcpp::Node rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); + /*! + * Unsubscribe from all the topics to release associated resources + */ + void unsubscribe(); + void loadDatabaseCallback( const std::shared_ptr request, const std::shared_ptr response); diff --git a/calibrators/mapping_based_calibrator/src/lidar_calibrator.cpp b/calibrators/mapping_based_calibrator/src/lidar_calibrator.cpp index 86cee81f..b9ba24c6 100644 --- a/calibrators/mapping_based_calibrator/src/lidar_calibrator.cpp +++ b/calibrators/mapping_based_calibrator/src/lidar_calibrator.cpp @@ -65,6 +65,8 @@ LidarCalibrator::LidarCalibrator( correspondence_estimator_ = pcl::make_shared>(); + target_kdtree_ = pcl::search::KdTree::Ptr(new pcl::search::KdTree); + // cSpell:ignore pclomp calibration_ndt_ = pcl::make_shared>(); calibration_gicp_ = @@ -90,6 +92,14 @@ LidarCalibrator::LidarCalibrator( configureCalibrators(); } +LidarCalibrator::~LidarCalibrator() +{ + PointcloudType::Ptr aux_pc_ptr(new PointcloudType()); + aux_pc_ptr->points.resize(1); + target_kdtree_->setInputCloud(aux_pc_ptr); + target_kdtree_.reset(); +} + void LidarCalibrator::configureCalibrators() { calibration_gicp_->setMaxCorrespondenceDistance(parameters_->max_corr_dist_coarse_); @@ -99,6 +109,7 @@ void LidarCalibrator::configureCalibrators() for (auto & calibrator : calibration_registrators_) { calibrator->setMaximumIterations(parameters_->solver_iterations_); + calibrator->setSearchMethodTarget(target_kdtree_); } calibration_batch_icp_coarse_->setMaxCorrespondenceDistance(parameters_->max_corr_dist_coarse_); @@ -111,15 +122,6 @@ void LidarCalibrator::configureCalibrators() } } -void LidarCalibrator::setUpCalibrators( - PointcloudType::Ptr & source_pointcloud_ptr, PointcloudType::Ptr & target_pointcloud_ptr) -{ - for (auto & calibrator : calibration_registrators_) { - calibrator->setInputSource(source_pointcloud_ptr); - calibrator->setInputTarget(target_pointcloud_ptr); - } -} - std::tuple LidarCalibrator::calibrate() { std::unique_lock lock(data_->mutex_); @@ -171,38 +173,29 @@ std::tuple LidarCalibrator::calibrate() // We no longer used the shared data lock.unlock(); - // Set all the registrators with the pointclouds - for (auto & calibrator : calibration_batch_registrators_) { - calibrator->clearInputSources(); - calibrator->clearInputTargets(); - - for (std::size_t i = 0; i < sources.size(); i++) { - calibrator->addInputSource(sources[i]); - calibrator->addInputTarget(targets[i]); - } - } - // Single-frame calibration for all frames std::vector initial_calibration_single_frame_score(calibration_frames.size()); std::vector single_frame_calibration_multi_frame_score(calibration_frames.size()); std::vector single_frame_calibration_single_frame_score(calibration_frames.size()); std::vector best_single_frame_transforms(calibration_frames.size()); + PointcloudType::Ptr initial_source_aligned_pc_ptr(new PointcloudType()); + RCLCPP_INFO(rclcpp::get_logger(calibrator_name_), "Calibration using single frames..."); for (std::size_t i = 0; i < calibration_frames.size(); i++) { - PointcloudType::Ptr initial_source_aligned_pc_ptr(new PointcloudType()); pcl::transformPointCloud( *sources[i], *initial_source_aligned_pc_ptr, initial_calibration_transform); for (auto & calibrator : calibration_registrators_) { calibrator->setInputSource(sources[i]); calibrator->setInputTarget(targets[i]); + // target_kdtree_->setInputCloud(targets[i]); } - correspondence_estimator_->setInputSource(initial_source_aligned_pc_ptr); - correspondence_estimator_->setInputTarget(targets[i]); - double initial_score = sourceTargetDistance( - *correspondence_estimator_, parameters_->calibration_eval_max_corr_distance_); + double initial_score = sourceTargetDistance( + initial_source_aligned_pc_ptr, targets[i], Eigen::Matrix4f::Identity(), + parameters_->calibration_eval_max_corr_distance_); + *initial_source_aligned_pc_ptr = PointcloudType(); // Find best calibration using an "ensemble" of calibrators std::vector candidate_transforms = {initial_calibration_transform}; @@ -249,6 +242,9 @@ std::tuple LidarCalibrator::calibrate() .target_frame_->keyframe_id_, *best_single_frame_calibration_multi_frame_score_it); + correspondence_estimator_.reset(); + calibration_registrators_.clear(); + // Multi-frame calibration using all frames RCLCPP_INFO(rclcpp::get_logger(calibrator_name_), "Calibration using multiple frames..."); std::vector candidate_transforms = { @@ -257,11 +253,29 @@ std::tuple LidarCalibrator::calibrate() Eigen::Matrix4f best_multi_frame_calibration_transform; float best_multi_frame_calibration_multi_frame_score; + // Set all the registrators with the pointclouds + for (auto & calibrator : calibration_batch_registrators_) { + calibrator->clearInputSources(); + calibrator->clearInputTargets(); + + for (std::size_t i = 0; i < sources.size(); i++) { + calibrator->addInputSource(sources[i]); + calibrator->addInputTarget(targets[i]); + } + } + findBestTransform, PointType>( candidate_transforms, calibration_batch_registrators_, parameters_->calibration_eval_max_corr_distance_, parameters_->calibration_verbose_, best_multi_frame_calibration_transform, best_multi_frame_calibration_multi_frame_score); + // Set all the registrators with the pointclouds + for (auto & calibrator : calibration_batch_registrators_) { + calibrator->clearInputSources(); + calibrator->clearInputTargets(); + calibrator->clearCorrespondenceEstimations(); + } + float initial_score = sourceTargetDistance( sources, targets, initial_calibration_transform, parameters_->calibration_eval_max_corr_distance_); @@ -329,6 +343,13 @@ std::tuple LidarCalibrator::calibrate() calibration_frames, sources, targets_thin, initial_calibration_transform, best_multi_frame_calibration_transform, map_frame); + calibration_batch_registrators_.clear(); + + // Release the resources + sources.clear(); + targets.clear(); + targets_thin.clear(); + return std::make_tuple<>( true, best_multi_frame_calibration_transform.cast(), std::sqrt(best_multi_frame_calibration_multi_frame_score)); @@ -345,6 +366,8 @@ void LidarCalibrator::prepareCalibrationData( rclcpp::get_logger(calibrator_name_), "Preparing dense calibration pointclouds from the map..."); + PointcloudType::Ptr initial_source_aligned_pc_ptr(new PointcloudType()); + // Prepare pointclouds for calibration for (auto & calibration_frame : calibration_frames) { PointcloudType::Ptr source_pc_ptr = cropPointCloud( @@ -362,7 +385,6 @@ void LidarCalibrator::prepareCalibrationData( parameters_->max_calibration_range_ + initial_distance); // Transform the source to target frame to crop it later - PointcloudType::Ptr initial_source_aligned_pc_ptr(new PointcloudType()); pcl::transformPointCloud( *source_pc_ptr, *initial_source_aligned_pc_ptr, initial_calibration_transform); @@ -371,6 +393,7 @@ void LidarCalibrator::prepareCalibrationData( cropTargetPointcloud( initial_source_aligned_pc_ptr, target_thin_pc_ptr, initial_distance); + *initial_source_aligned_pc_ptr = PointcloudType(); sources.push_back(source_pc_ptr); targets.push_back(target_pc_ptr); targets_thin.push_back(target_thin_pc_ptr); @@ -385,41 +408,39 @@ void LidarCalibrator::publishResults( const std::string & map_frame) { // Publish ROS data - PointcloudType::Ptr initial_source_aligned_map_ptr(new PointcloudType()); - PointcloudType::Ptr calibrated_source_aligned_map_ptr(new PointcloudType()); - PointcloudType::Ptr target_thin_map_ptr(new PointcloudType()); + PointcloudType initial_source_aligned_map; + PointcloudType calibrated_source_aligned_map; + PointcloudType target_thin_map; for (std::size_t i = 0; i < calibration_frames.size(); i++) { - PointcloudType::Ptr initial_tmp_ptr(new PointcloudType()); - PointcloudType::Ptr calibrated_tmp_ptr(new PointcloudType()); - PointcloudType::Ptr target_thin_tmp_ptr(new PointcloudType()); + PointcloudType initial_tmp; + PointcloudType calibrated_tmp; + PointcloudType target_thin_tmp; pcl::transformPointCloud( - *sources[i], *initial_tmp_ptr, calibration_frames[i].local_map_pose_ * initial_transform); + *sources[i], initial_tmp, calibration_frames[i].local_map_pose_ * initial_transform); pcl::transformPointCloud( - *sources[i], *calibrated_tmp_ptr, - calibration_frames[i].local_map_pose_ * calibrated_transform); + *sources[i], calibrated_tmp, calibration_frames[i].local_map_pose_ * calibrated_transform); - pcl::transformPointCloud( - *targets[i], *target_thin_tmp_ptr, calibration_frames[i].local_map_pose_); + pcl::transformPointCloud(*targets[i], target_thin_tmp, calibration_frames[i].local_map_pose_); - *initial_source_aligned_map_ptr += *initial_tmp_ptr; - *calibrated_source_aligned_map_ptr += *calibrated_tmp_ptr; - *target_thin_map_ptr += *target_thin_tmp_ptr; + initial_source_aligned_map += initial_tmp; + calibrated_source_aligned_map += calibrated_tmp; + target_thin_map += target_thin_tmp; } sensor_msgs::msg::PointCloud2 initial_source_aligned_map_msg, calibrated_source_aligned_map_msg, target_map_msg; - initial_source_aligned_map_ptr->width = initial_source_aligned_map_ptr->points.size(); - initial_source_aligned_map_ptr->height = 1; - calibrated_source_aligned_map_ptr->width = calibrated_source_aligned_map_ptr->points.size(); - calibrated_source_aligned_map_ptr->height = 1; - target_thin_map_ptr->width = target_thin_map_ptr->points.size(); - target_thin_map_ptr->height = 1; - - pcl::toROSMsg(*initial_source_aligned_map_ptr, initial_source_aligned_map_msg); - pcl::toROSMsg(*calibrated_source_aligned_map_ptr, calibrated_source_aligned_map_msg); - pcl::toROSMsg(*target_thin_map_ptr, target_map_msg); + initial_source_aligned_map.width = initial_source_aligned_map.points.size(); + initial_source_aligned_map.height = 1; + calibrated_source_aligned_map.width = calibrated_source_aligned_map.points.size(); + calibrated_source_aligned_map.height = 1; + target_thin_map.width = target_thin_map.points.size(); + target_thin_map.height = 1; + + pcl::toROSMsg(initial_source_aligned_map, initial_source_aligned_map_msg); + pcl::toROSMsg(calibrated_source_aligned_map, calibrated_source_aligned_map_msg); + pcl::toROSMsg(target_thin_map, target_map_msg); initial_source_aligned_map_msg.header = calibration_frames[0].source_header_; initial_source_aligned_map_msg.header.frame_id = map_frame; diff --git a/calibrators/mapping_based_calibrator/src/mapping_based_calibrator.cpp b/calibrators/mapping_based_calibrator/src/mapping_based_calibrator.cpp index 4d127949..078582ed 100644 --- a/calibrators/mapping_based_calibrator/src/mapping_based_calibrator.cpp +++ b/calibrators/mapping_based_calibrator/src/mapping_based_calibrator.cpp @@ -322,9 +322,11 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( auto base_lidar_augmented_pub = this->create_publisher("ground_pointcloud", 10); - base_lidar_calibrator_ = std::make_shared( - calibration_parameters_, mapping_data_, tf_buffer_, tf_broadcaster_, - base_lidar_augmented_pointcloud_pub, base_lidar_augmented_pub); + if (calibration_parameters_->calibrate_base_frame_) { + base_lidar_calibrator_ = std::make_shared( + calibration_parameters_, mapping_data_, tf_buffer_, tf_broadcaster_, + base_lidar_augmented_pointcloud_pub, base_lidar_augmented_pub); + } srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -372,7 +374,9 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( this->create_subscription( calibration_pointcloud_topic, rclcpp::SensorDataQoS().keep_all(), [&](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - mapper_->calibrationPointCloudCallback(msg, calibration_frame_name); + if (mapper_) { + mapper_->calibrationPointCloudCallback(msg, calibration_frame_name); + } }); } @@ -396,6 +400,7 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( [&]( [[maybe_unused]] const std::shared_ptr request, [[maybe_unused]] const std::shared_ptr response) { + unsubscribe(); mapper_->stop(); RCLCPP_INFO_STREAM(this->get_logger(), "Mapper stopped through service"); }, @@ -430,9 +435,17 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicMappingBasedCalibrator::paramC result.reason = "success"; std::unique_lock service_lock(service_mutex_); + + if (!mapping_data_) { + RCLCPP_WARN(this->get_logger(), "Can not modify the parameters after finishing calibration"); + result.successful = false; + result.reason = "Can not modify the parameters after finishing calibration"; + return result; + } + std::unique_lock mapping_lock(mapping_data_->mutex_); - if (mapper_->getState() == CalibrationMapper::MAPPING || calibration_pending_) { + if ((mapper_ && mapper_->getState()) == CalibrationMapper::MAPPING || calibration_pending_) { RCLCPP_WARN(this->get_logger(), "Can not modify the parameters while mapping or calibrating"); result.successful = false; result.reason = "Attempted to modify the parameters while mapping / calibrating"; @@ -510,6 +523,27 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicMappingBasedCalibrator::paramC return result; } +void ExtrinsicMappingBasedCalibrator::unsubscribe() +{ + for (std::size_t i = 0; i < mapping_data_->calibration_camera_optical_link_frame_names.size(); + i++) { + const std::string & calibration_frame_name = + mapping_data_->calibration_camera_optical_link_frame_names[i]; + + calibration_camera_info_subs_[calibration_frame_name].reset(); + calibration_image_subs_[calibration_frame_name].reset(); + } + + for (std::size_t i = 0; i < mapping_data_->calibration_lidar_frame_names_.size(); i++) { + const std::string & calibration_frame_name = mapping_data_->calibration_lidar_frame_names_[i]; + calibration_pointcloud_subs_[calibration_frame_name].reset(); + } + + mapping_pointcloud_sub_.reset(); + detected_objects_sub_.reset(); + predicted_objects_sub_.reset(); +} + void ExtrinsicMappingBasedCalibrator::requestReceivedCallback( [[maybe_unused]] const std::shared_ptr request, @@ -563,6 +597,8 @@ void ExtrinsicMappingBasedCalibrator::requestReceivedCallback( result.success = status; result.message.data = "Score corresponds to the source->target distance error"; response->results.emplace_back(result); + + lidar_calibrators_[calibration_frame_name].reset(); }); } @@ -587,6 +623,8 @@ void ExtrinsicMappingBasedCalibrator::requestReceivedCallback( result.success = status; result.message.data = "Not implemented"; response->results.emplace_back(result); + + camera_calibrators_[calibration_frame_name].reset(); }); } @@ -616,6 +654,7 @@ void ExtrinsicMappingBasedCalibrator::requestReceivedCallback( result.success = status; result.message.data = "Base calibration provides no score"; response->results.emplace_back(result); + base_lidar_calibrator_.reset(); }); } @@ -628,11 +667,21 @@ void ExtrinsicMappingBasedCalibrator::requestReceivedCallback( std::unique_lock lock(service_mutex_); calibration_pending_ = false; + + publisher_timer_.reset(); + data_matching_timer_.reset(); + + mapper_.reset(); + mapping_data_.reset(); } void ExtrinsicMappingBasedCalibrator::detectedObjectsCallback( const autoware_perception_msgs::msg::DetectedObjects::SharedPtr objects) { + if (!mapper_ || mapper_->getState() != CalibrationMapper::MAPPING || calibration_pending_) { + RCLCPP_WARN_ONCE(this->get_logger(), "Received objects while not mapping. Ignoring..."); + } + // Convert objects into ObjectBB ObjectsBB new_objects; new_objects.header_ = objects->header; @@ -658,6 +707,10 @@ void ExtrinsicMappingBasedCalibrator::detectedObjectsCallback( void ExtrinsicMappingBasedCalibrator::predictedObjectsCallback( const autoware_perception_msgs::msg::PredictedObjects::SharedPtr objects) { + if (!mapper_ || mapper_->getState() != CalibrationMapper::MAPPING || calibration_pending_) { + RCLCPP_WARN_ONCE(this->get_logger(), "Received objects while not mapping. Ignoring..."); + } + // Convert objects into ObjectBB ObjectsBB new_objects; new_objects.header_ = objects->header; @@ -684,6 +737,13 @@ void ExtrinsicMappingBasedCalibrator::loadDatabaseCallback( const std::shared_ptr request, const std::shared_ptr response) { + if (!mapper_) { + RCLCPP_ERROR( + this->get_logger(), "Attempted to load a database when the mapper had already been released"); + response->success = false; + return; + } + // cSpell:ignore iarchive std::ifstream ifs(request->path); boost::archive::binary_iarchive ia(ifs); @@ -722,6 +782,7 @@ void ExtrinsicMappingBasedCalibrator::loadDatabaseCallback( ia >> mapping_data_->detected_objects_; RCLCPP_INFO(this->get_logger(), "Loaded %ld objects...", mapping_data_->detected_objects_.size()); + unsubscribe(); mapper_->stop(); RCLCPP_INFO(this->get_logger(), "Finished"); @@ -733,6 +794,13 @@ void ExtrinsicMappingBasedCalibrator::saveDatabaseCallback( const std::shared_ptr request, const std::shared_ptr response) { + if (!mapper_) { + RCLCPP_ERROR( + this->get_logger(), "Attempted to save a database when the mapper had already been released"); + response->success = false; + return; + } + // cSpell:ignore oarchive std::ofstream ofs(request->path); boost::archive::binary_oarchive oa(ofs); @@ -789,6 +857,7 @@ void ExtrinsicMappingBasedCalibrator::saveDatabaseCallback( voxel_grid.filter(*map_subsampled_cloud_ptr); pcl::io::savePCDFileASCII("dense_map.pcd", *map_subsampled_cloud_ptr); + unsubscribe(); mapper_->stop(); RCLCPP_INFO(this->get_logger(), "Finished"); diff --git a/calibrators/mapping_based_calibrator/src/utils.cpp b/calibrators/mapping_based_calibrator/src/utils.cpp index f2d1af9a..90343bfd 100644 --- a/calibrators/mapping_based_calibrator/src/utils.cpp +++ b/calibrators/mapping_based_calibrator/src/utils.cpp @@ -50,6 +50,7 @@ void transformPointcloud( pcl::transformPointCloud(*pc_ptr, *transformed_pc_ptr, transform); pc_ptr.swap(transformed_pc_ptr); + *transformed_pc_ptr = PointcloudType(); } catch (tf2::TransformException & ex) { RCLCPP_WARN(rclcpp::get_logger("tf_buffer"), "could not get initial tf. %s", ex.what()); } @@ -154,7 +155,10 @@ float sourceTargetDistance( estimator.setInputSource(source_aligned); estimator.setInputTarget(target); - return sourceTargetDistance(estimator, max_corr_distance); + auto source_target_distance = sourceTargetDistance(estimator, max_corr_distance); + *source_aligned = pcl::PointCloud(); + + return source_target_distance; } template @@ -168,12 +172,12 @@ float sourceTargetDistance( assert(sources.size() == targets.size()); + pcl::registration::CorrespondenceEstimation estimator; + for (std::size_t i = 0; i < sources.size(); i++) { typename pcl::PointCloud::Ptr source_aligned(new typename pcl::PointCloud()); transformPointCloud(*sources[i], *source_aligned, transform); - - pcl::registration::CorrespondenceEstimation estimator; estimator.setInputSource(source_aligned); estimator.setInputTarget(targets[i]); @@ -200,6 +204,7 @@ void findBestTransform( best_transform = Eigen::Matrix4f::Identity(); best_score = std::numeric_limits::max(); std::string best_name; + typename pcl::PointCloud aligned_cloud; for (auto & registrator : registrators) { Eigen::Matrix4f best_registrator_transform = Eigen::Matrix4f::Identity(); @@ -210,8 +215,8 @@ void findBestTransform( auto & transform = transforms[i]; auto & transform_name = transforms_names[i]; - typename pcl::PointCloud::Ptr aligned_cloud_ptr(new pcl::PointCloud()); - registrator->align(*aligned_cloud_ptr, transform); + aligned_cloud.clear(); + registrator->align(aligned_cloud, transform); Eigen::Matrix4f candidate_transform = registrator->getFinalTransformation(); float candidate_score =