精读陈佬的重定位算法
一、代码框架
1.main函数
采用了ros2的可组合节点，main函数被cmakelist的一句注册节点代替，只需要看该类的构造函数即可。
首先是参数定义：
this->declare_parameter("num_threads", 4);this->declare_parameter("num_neighbors", 20);this->declare_parameter("global_leaf_size", 0.25);this->declare_parameter("registered_leaf_size", 0.25);this->declare_parameter("max_dist_sq", 1.0);this->declare_parameter("map_frame", "map");this->declare_parameter("odom_frame", "odom");this->declare_parameter("base_frame", "");this->declare_parameter("lidar_frame", "");this->declare_parameter("prior_pcd_file", "");this->get_parameter("num_threads", num_threads_);this->get_parameter("num_neighbors", num_neighbors_);this->get_parameter("global_leaf_size", global_leaf_size_);this->get_parameter("registered_leaf_size", registered_leaf_size_);this->get_parameter("max_dist_sq", max_dist_sq_);this->get_parameter("map_frame", map_frame_);this->get_parameter("odom_frame", odom_frame_);this->get_parameter("base_frame", base_frame_);this->get_parameter("lidar_frame", lidar_frame_);this->get_parameter("prior_pcd_file", prior_pcd_file_);
接着预载点云信息，并将点云参考系从雷达转到底盘：
void SmallGicpRelocalizationNode::loadGlobalMap(const std::string & file_name)
{if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_name, *global_map_) == -1) {RCLCPP_ERROR(this->get_logger(), "Couldn't read PCD file: %s", file_name.c_str());return;}RCLCPP_INFO(this->get_logger(), "Loaded global map with %zu points", global_map_->points.size());// NOTE: Transform global pcd_map (based on `lidar_odom` frame) to the `odom` frameEigen::Affine3d odom_to_lidar_odom;while (true) {try {auto tf_stamped = tf_buffer_->lookupTransform(base_frame_, lidar_frame_, this->now(), rclcpp::Duration::from_seconds(1.0));odom_to_lidar_odom = tf2::transformToEigen(tf_stamped.transform);RCLCPP_INFO_STREAM(this->get_logger(), "odom_to_lidar_odom: translation = "<< odom_to_lidar_odom.translation().transpose() << ", rpy = "<< odom_to_lidar_odom.rotation().eulerAngles(0, 1, 2).transpose());break;} catch (tf2::TransformException & ex) {RCLCPP_WARN(this->get_logger(), "TF lookup failed: %s Retrying...", ex.what());rclcpp::sleep_for(std::chrono::seconds(1));}}pcl::transformPointCloud(*global_map_, *global_map_, odom_to_lidar_odom);
}
接着对上步得到的点云进行降采样处理
target_ = small_gicp::voxelgrid_sampling_omp<pcl::PointCloud<pcl::PointXYZ>, pcl::PointCloud<pcl::PointCovariance>>(*global_map_, global_leaf_size_);
然后就是计算点云的协方差矩阵并得出Kd搜索树
// Estimate covariances of points 估计点的协方差small_gicp::estimate_covariances_omp(*target_, num_neighbors_, num_threads_);// Create KdTree for targettarget_tree_ = std::make_shared<small_gicp::KdTree<pcl::PointCloud<pcl::PointCovariance>>>(target_, small_gicp::KdTreeBuilderOMP(num_threads_));
最后就是一堆的订阅和接收话题，接下来对每个订阅和接收进行分析
pcd_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>("registered_scan", 10,std::bind(&SmallGicpRelocalizationNode::registeredPcdCallback, this, std::placeholders::_1));initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10,std::bind(&SmallGicpRelocalizationNode::initialPoseCallback, this, std::placeholders::_1));register_timer_ = this->create_wall_timer(std::chrono::milliseconds(500), // 2 Hzstd::bind(&SmallGicpRelocalizationNode::performRegistration, this));transform_timer_ = this->create_wall_timer(std::chrono::milliseconds(50), // 20 Hzstd::bind(&SmallGicpRelocalizationNode::publishTransform, this));
2.pcd_sub
该函数的作用是将传感器获得的点云数据进行降采样和生成kd搜索树
void SmallGicpRelocalizationNode::registeredPcdCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{last_scan_time_ = msg->header.stamp;pcl::fromROSMsg(*msg, *registered_scan_);// Downsample Registered points and convert them into pcl::PointCloud<pcl::PointCovariance>.source_ = small_gicp::voxelgrid_sampling_omp<pcl::PointCloud<pcl::PointXYZ>, pcl::PointCloud<pcl::PointCovariance>>(*registered_scan_, registered_leaf_size_);// Estimate point covariancessmall_gicp::estimate_covariances_omp(*source_, num_neighbors_, num_threads_);// Create KdTree for source.source_tree_ = std::make_shared<small_gicp::KdTree<pcl::PointCloud<pcl::PointCovariance>>>(source_, small_gicp::KdTreeBuilderOMP(num_threads_));
}
3.initial_pose_sub
该函数的作用是获得初始位姿并赋值（可以没有，没有就是默认【0，0，0】）
void SmallGicpRelocalizationNode::initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{RCLCPP_INFO(this->get_logger(), "Received initial pose: [x: %f, y: %f, z: %f]", msg->pose.pose.position.x,msg->pose.pose.position.y, msg->pose.pose.position.z);Eigen::Isometry3d initial_pose;initial_pose.translation() << msg->pose.pose.position.x, msg->pose.pose.position.y,msg->pose.pose.position.z;initial_pose.linear() = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x,msg->pose.pose.orientation.y, msg->pose.pose.orientation.z).toRotationMatrix();previous_result_t_ = initial_pose;result_t_ = initial_pose;
}
4.register_timer
该函数是核心部分，以上一步获得的初始位姿（如果有）为初始变换来对齐点云，使用的是gip函数库的align函数
void SmallGicpRelocalizationNode::performRegistration()
{if (!source_ || !source_tree_) {return;}register_->reduction.num_threads = num_threads_;register_->rejector.max_dist_sq = max_dist_sq_;// Align point clouds using the previous result as the initial transformation 使用上一个结果作为初始变换来对齐点云auto result = register_->align(*target_, *source_, *target_tree_, previous_result_t_);if (!result.converged) {RCLCPP_WARN(this->get_logger(), "GICP did not converge.");return;}result_t_ = result.T_target_source;previous_result_t_ = result.T_target_source;
}
5.transform_timer
将第4步获得的点云差进行tf转换作用到实际中去
void SmallGicpRelocalizationNode::publishTransform()
{if (result_t_.matrix().isZero()) {return;}geometry_msgs::msg::TransformStamped transform_stamped;// `+ 0.1` means transform into future. according to https://robotics.stackexchange.com/a/96615transform_stamped.header.stamp = last_scan_time_ + rclcpp::Duration::from_seconds(0.1);transform_stamped.header.frame_id = map_frame_;transform_stamped.child_frame_id = odom_frame_;const Eigen::Vector3d translation = result_t_.translation();const Eigen::Quaterniond rotation(result_t_.rotation());transform_stamped.transform.translation.x = translation.x();transform_stamped.transform.translation.y = translation.y();transform_stamped.transform.translation.z = translation.z();transform_stamped.transform.rotation.x = rotation.x();transform_stamped.transform.rotation.y = rotation.y();transform_stamped.transform.rotation.z = rotation.z();transform_stamped.transform.rotation.w = rotation.w();tf_broadcaster_->sendTransform(transform_stamped);
}
至此就是此代码的全部逻辑。
