Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix: attempted to address non freed memory #206

Merged
merged 3 commits into from
Oct 31, 2024
Merged

fix: attempted to address non freed memory #206

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a0bdb4a
fix: attempted to address non freed memory
knzo25 Oct 28, 2024
483ccfe
chore: deleted an unused method
knzo25 Oct 28, 2024
5486e50
chore: typos
knzo25 Oct 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 4 additions & 8 deletions calibrators/mapping_based_calibrator/include/mapping_based_calibrator/lidar_calibrator.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,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 @@ -85,6 +87,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 @@ -94,6 +104,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 @@ -106,15 +117,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 @@ -166,38 +168,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 @@ -244,6 +237,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 @@ -252,11 +248,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 @@ -324,6 +338,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 @@ -340,6 +361,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 @@ -357,7 +380,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 @@ -366,6 +388,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 @@ -380,41 +403,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