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<pcl::Registration<PointType, PointType>::Ptr> calibration_registrators_;
+  pcl::search::KdTree<PointType>::Ptr target_kdtree_;
+
   std::vector<pcl::JointIterativeClosestPointExtended<PointType, PointType>::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<rclcpp::Parameter> & parameters);
 
+  /*!
+   * Unsubscribe from all the topics to release associated resources
+   */
+  void unsubscribe();
+
   void loadDatabaseCallback(
     const std::shared_ptr<tier4_calibration_msgs::srv::CalibrationDatabase::Request> request,
     const std::shared_ptr<tier4_calibration_msgs::srv::CalibrationDatabase::Response> 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<pcl::registration::CorrespondenceEstimation<PointType, PointType>>();
 
+  target_kdtree_ = pcl::search::KdTree<PointType>::Ptr(new pcl::search::KdTree<PointType>);
+
   // cSpell:ignore pclomp
   calibration_ndt_ = pcl::make_shared<pclomp::NormalDistributionsTransform<PointType, PointType>>();
   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<bool, Eigen::Matrix4d, float> LidarCalibrator::calibrate()
 {
   std::unique_lock<std::recursive_mutex> lock(data_->mutex_);
@@ -171,38 +173,29 @@ std::tuple<bool, Eigen::Matrix4d, float> 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<float> initial_calibration_single_frame_score(calibration_frames.size());
   std::vector<float> single_frame_calibration_multi_frame_score(calibration_frames.size());
   std::vector<float> single_frame_calibration_single_frame_score(calibration_frames.size());
   std::vector<Eigen::Matrix4f> 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<PointType>(
+      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<Eigen::Matrix4f> candidate_transforms = {initial_calibration_transform};
@@ -249,6 +242,9 @@ std::tuple<bool, Eigen::Matrix4d, float> 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<Eigen::Matrix4f> candidate_transforms = {
@@ -257,11 +253,29 @@ std::tuple<bool, Eigen::Matrix4d, float> 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<pcl::JointIterativeClosestPointExtended<PointType, PointType>, 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<PointType>(
     sources, targets, initial_calibration_transform,
     parameters_->calibration_eval_max_corr_distance_);
@@ -329,6 +343,13 @@ std::tuple<bool, Eigen::Matrix4d, float> 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<double>(),
     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<PointcloudType>(
@@ -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<PointType>(
       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<sensor_msgs::msg::PointCloud2>("ground_pointcloud", 10);
 
-  base_lidar_calibrator_ = std::make_shared<BaseLidarCalibrator>(
-    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<BaseLidarCalibrator>(
+      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<sensor_msgs::msg::PointCloud2>(
         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<std_srvs::srv::Empty::Request> request,
       [[maybe_unused]] const std::shared_ptr<std_srvs::srv::Empty::Response> 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<std::mutex> 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<std::recursive_mutex> 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<tier4_calibration_msgs::srv::ExtrinsicCalibrator::Request>
     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<std::mutex> 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<tier4_calibration_msgs::srv::CalibrationDatabase::Request> request,
   const std::shared_ptr<tier4_calibration_msgs::srv::CalibrationDatabase::Response> 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<tier4_calibration_msgs::srv::CalibrationDatabase::Request> request,
   const std::shared_ptr<tier4_calibration_msgs::srv::CalibrationDatabase::Response> 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<PointType>();
+
+  return source_target_distance;
 }
 
 template <class PointType>
@@ -168,12 +172,12 @@ float sourceTargetDistance(
 
   assert(sources.size() == targets.size());
 
+  pcl::registration::CorrespondenceEstimation<PointType, PointType> estimator;
+
   for (std::size_t i = 0; i < sources.size(); i++) {
     typename pcl::PointCloud<PointType>::Ptr source_aligned(new
                                                             typename pcl::PointCloud<PointType>());
     transformPointCloud(*sources[i], *source_aligned, transform);
-
-    pcl::registration::CorrespondenceEstimation<PointType, PointType> 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<float>::max();
   std::string best_name;
+  typename pcl::PointCloud<PointType> 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<PointType>::Ptr aligned_cloud_ptr(new pcl::PointCloud<PointType>());
-      registrator->align(*aligned_cloud_ptr, transform);
+      aligned_cloud.clear();
+      registrator->align(aligned_cloud, transform);
 
       Eigen::Matrix4f candidate_transform = registrator->getFinalTransformation();
       float candidate_score =