Skip to content

Commit

Permalink
fix: attempted to address non freed memory (#206)
Browse files Browse the repository at this point in the history 
* fix: attempted to address non freed memory

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

* chore: deleted an unused method

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

* chore: typos

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

---------

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>
  • Loading branch information
@knzo25
knzo25 authored Oct 31, 2024
1 parent 1bb940c commit 6dd7d1e
Show file tree
Hide file tree
Showing 5 changed files with 164 additions and 68 deletions.
12 changes: 4 additions & 8 deletions calibrators/mapping_based_calibrator/include/mapping_based_calibrator/lidar_calibrator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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

/*!
Expand Down Expand Up @@ -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
Expand Down
5 changes: 5 additions & 0 deletions ...rs/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
121 changes: 71 additions & 50 deletions calibrators/mapping_based_calibrator/src/lidar_calibrator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_ =
Expand All @@ -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_);
Expand All @@ -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_);
Expand All @@ -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_);
Expand Down Expand Up @@ -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};
Expand Down Expand Up @@ -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 = {
Expand All @@ -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_);
Expand Down Expand Up @@ -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));
Expand All @@ -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>(
Expand All @@ -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);

Expand All @@ -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);
Expand All @@ -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;
Expand Down
Loading

0 comments on commit 6dd7d1e

Please sign in to comment.