hdl_localization代码解析
简介
hdl_localization是基于UKF滤波框架，融合了ndt点云配准结果，在已经构建的点云地图上实习激光重定位的一种方法。在使用16线激光雷达进行机器人定位时，不用IMU也可以取得不错的效果。
安装依赖
依赖库
1.Ros-Melodic
2.Pcl-1.8
3.Open-MP
4.Eigen3.3(及以上)
依赖包
运行
rosparam set use_sim_time true
roslaunch hdl_localization hdl_localization.launch
roscd hdl_localization/rviz
rviz -d hdl_localization.rviz
注意
1.如果进行纯定位时的初始位姿在地图坐标系附近，在launch文件中可以将 “specify_init_pose” 设为 “true”，这样，其默认的三维位置（0，0，0）和默认的表示旋转的四元数（0，0，0，1）就可以很好的给予点云一个初始状态，有利于其后续匹配和重定位。
2.如果想在地图中任意位置进行重定位，需要在开启rviz -d hdl_localization.rviz后，选择rviz上方的2D pose estimator，并在地图中左键点击和鼠标拖动，选择一个与真实位置相近的位置与航向。
效果
; UKF知识补充（无迹卡尔曼滤波）
网页链接
原始高斯分布经过非线性变换之后其实是一个不规则的非线性分布，在EKF中我们在高斯均值点附近用泰勒级数近似非线性分布，从而获得近似高斯分布。但是问题在于，这个近似高斯分布有时不是足够精确，单单一个均值点往往无法预测上图中这种很不规则的分布的。这个时候我们就需要无迹卡尔曼滤波UKF了，通过无迹转换将非线性函数变换的结果近似成高斯分布。
以下是无迹变换执行的步骤：
1.计算Sigma点集
2.为每个Sigma点分配权重
3.把所有单个Sigma点代入非线性函数f
4.对经过上述加权和转换的点近似新的高斯分布
5.计算新高斯分布的均值和方差。
代码阅读
总览
该项目是使用nodelet统一管理的，apps为定义的两个类，也就是程序实现。include内为状态估计器和无迹卡尔曼的实现。launch是启动文件。rviz内为rviz的配置文件。data为实例的定位用点云地图。
launch
定义了几个参数，使用nodelet运行了velodyne_nodelet_manager、globalmap_server_nodelet、hdl_localization_nodelet三个节点。如果只用于仿真，可以在 arguments 前面加上。
<param name="use_sim_time" value="true"/>
apps(程序实现)
本文件夹是只有两个cpp文件，直接继承了nodelet的类。
globalmap_server_nodelet
类GlobalmapServerNodelet 继承了 nodelet::Nodelet。
关于ros，声明了三个句柄，1个发布，1个计时器，1个globalmap的变量。
ros::NodeHandle nh;
ros::NodeHandle mt_nh;
ros::NodeHandle private_nh;
ros::Publisher globalmap_pub;
ros::WallTimer globalmap_pub_timer;
pcl::PointCloud<PointT>::Ptr globalmap;
globalmap_server_nodelet::onInit()
这里是在重写了初始化函数。同时利用计时器出发回调函数。
void onInit() override {
nh = getNodeHandle();
mt_nh = getMTNodeHandle();
private_nh = getPrivateNodeHandle();
initialize_params();
globalmap_pub = nh.advertise<sensor_msgs::PointCloud2>("/globalmap", 5, true);
globalmap_pub_timer = nh.createWallTimer(ros::WallDuration(0.05), &GlobalmapServerNodelet::pub_once_cb, this, true, true);
}
globalmap_server_nodelet::initialize_params()
在程序initialize_params()中，完成了读取地图pcd文件的功能，并对该地图进行下采样，最终的globalmap是下采样的地图。
void initialize_params() {
std::string globalmap_pcd = private_nh.param<std::string>("globalmap_pcd", "");
globalmap.reset(new pcl::PointCloud<PointT>());
pcl::io::loadPCDFile(globalmap_pcd, *globalmap);
globalmap->header.frame_id = "map";
std::ifstream utm_file(globalmap_pcd + ".utm");
if (utm_file.is_open() && private_nh.param<bool>("convert_utm_to_local", true)) {
std::cout << "now utf_file is open" << std::endl;
double utm_easting;
double utm_northing;
double altitude;
utm_file >> utm_easting >> utm_northing >> altitude;
for(auto& pt : globalmap->points) {
pt.getVector3fMap() -= Eigen::Vector3f(utm_easting, utm_northing, altitude);
}
ROS_INFO_STREAM("Global map offset by UTM reference coordinates (x = "
<< utm_easting << ", y = " << utm_northing << ") and altitude (z = " << altitude << ")");
}
double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
voxelgrid->setInputCloud(globalmap);
pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
voxelgrid->filter(*filtered);
globalmap = filtered;
}
同时，每隔0.05s发布一次。（onInit定义的）
void pub_once_cb(const ros::WallTimerEvent& event) {
globalmap_pub.publish(globalmap);
}
hdl_localization_nodelet
类 HdlLocalizationNodelet 继承了 nodelet::Nodelet，先看初始化。
hdl_localization_nodelet::onInit()
void onInit() override {
nh = getNodeHandle();
mt_nh = getMTNodeHandle();
private_nh = getPrivateNodeHandle();
processing_time.resize(16);
initialize_params();
odom_child_frame_id = private_nh.param<std::string>("odom_child_frame_id", "base_link");
use_imu = private_nh.param<bool>("use_imu", true);
invert_imu = private_nh.param<bool>("invert_imu", false);
if(use_imu) {
NODELET_INFO("enable imu-based prediction");
imu_sub = mt_nh.subscribe("/gpsimu_driver/imu_data", 256, &HdlLocalizationNodelet::imu_callback, this);
}
points_sub = mt_nh.subscribe("/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this);
globalmap_sub = nh.subscribe("/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this);
initialpose_sub = nh.subscribe("/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this);
pose_pub = nh.advertise<nav_msgs::Odometry>("/odom", 5, false);
aligned_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 5, false);
}
hdl_localization_nodelet::initialize_params()
初始化参数
void initialize_params() {
double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
std::string ndt_neighbor_search_method = private_nh.param<std::string>("ndt_neighbor_search_method", "DIRECT7");
double ndt_resolution = private_nh.param<double>("ndt_resolution", 1.0);
boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
downsample_filter = voxelgrid;
pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>());
ndt->setTransformationEpsilon(0.01);
ndt->setResolution(ndt_resolution);
if(ndt_neighbor_search_method == "DIRECT1") {
NODELET_INFO("search_method DIRECT1 is selected");
ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
registration = ndt;
} else if(ndt_neighbor_search_method == "DIRECT7") {
NODELET_INFO("search_method DIRECT7 is selected");
ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
registration = ndt;
} else if(ndt_neighbor_search_method == "GICP_OMP"){
NODELET_INFO("search_method GICP_OMP is selected");
registration = gicp;
}
else {
if(ndt_neighbor_search_method == "KDTREE") {
NODELET_INFO("search_method KDTREE is selected");
} else {
NODELET_WARN("invalid search method was given");
NODELET_WARN("default method is selected (KDTREE)");
}
ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
registration = ndt;
}
if(private_nh.param<bool>("specify_init_pose", true)) {
NODELET_INFO("initialize pose estimator with specified parameters!!");
pose_estimator.reset(new hdl_localization::PoseEstimator(registration,
ros::Time::now(),
Eigen::Vector3f(private_nh.param<double>("init_pos_x", 0.0), private_nh.param<double>("init_pos_y", 0.0), private_nh.param<double>("init_pos_z", 0.0)),
Eigen::Quaternionf(private_nh.param<double>("init_ori_w", 1.0), private_nh.param<double>("init_ori_x", 0.0), private_nh.param<double>("init_ori_y", 0.0), private_nh.param<double>("init_ori_z", 0.0)),
private_nh.param<double>("cool_time_duration", 0.5)
));
}
}
downsample(const pcl::PointCloud::ConstPtr& cloud)
当前帧点云数据下采样
pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
if(!downsample_filter) {
return cloud;
}
pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
downsample_filter->setInputCloud(cloud);
downsample_filter->filter(*filtered);
filtered->header = cloud->header;
return filtered;
}
publish_odometry
发布里程计的tf和msg。输入为当前帧点云时间戳与pose_estimator的结果矩阵。这里还用到了matrix2transform这个函数，用于做eigen矩阵到tf的转化（取值）。
void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) {
geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", odom_child_frame_id);
pose_broadcaster.sendTransform(odom_trans);
nav_msgs::Odometry odom;
odom.header.stamp = stamp;
odom.header.frame_id = "map";
odom.pose.pose.position.x = pose(0, 3);
odom.pose.pose.position.y = pose(1, 3);
odom.pose.pose.position.z = pose(2, 3);
odom.pose.pose.orientation = odom_trans.transform.rotation;
odom.child_frame_id = odom_child_frame_id;
odom.twist.twist.linear.x = 0.0;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.angular.z = 0.0;
pose_pub.publish(odom);
}
matrix2transform
matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) 从matrix 到 geometry_msgs::TransformStamped。
geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
quat.normalize();
geometry_msgs::Quaternion odom_quat;
odom_quat.w = quat.w();
odom_quat.x = quat.x();
odom_quat.y = quat.y();
odom_quat.z = quat.z();
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = stamp;
odom_trans.header.frame_id = frame_id;
odom_trans.child_frame_id = child_frame_id;
odom_trans.transform.translation.x = pose(0, 3);
odom_trans.transform.translation.y = pose(1, 3);
odom_trans.transform.translation.z = pose(2, 3);
odom_trans.transform.rotation = odom_quat;
return odom_trans;
}
include(状态估计器及ukf)
apps里的两个cpp大致内容均为以上。可以看到在points_callback里使用了pose_estimator作为位姿的估计。而该类又使用了ukf作为位姿的解算。二者的实现都在include文件夹内。
hdl_localization/pose_estimator.hpp
该文件定义了类PoseEstimator。
PoseEstimator（构造函数）
首先看构造函数。可以看到在初始化之后，最重要的是进入了ukf的处理。
PoseEstimator(pcl::Registration<PointT, PointT>::Ptr& registration, const ros::Time& stamp, const Eigen::Vector3f& pos, const Eigen::Quaternionf& quat, double cool_time_duration = 1.0)
: init_stamp(stamp),
registration(registration),
cool_time_duration(cool_time_duration)
{
process_noise = Eigen::MatrixXf::Identity(16, 16);
process_noise.middleRows(0, 3) *= 1.0;
process_noise.middleRows(3, 3) *= 1.0;
process_noise.middleRows(6, 4) *= 0.5;
process_noise.middleRows(10, 3) *= 1e-6;
process_noise.middleRows(13, 3) *= 1e-6;
Eigen::MatrixXf measurement_noise = Eigen::MatrixXf::Identity(7, 7);
measurement_noise.middleRows(0, 3) *= 0.01;
measurement_noise.middleRows(3, 4) *= 0.001;
Eigen::VectorXf mean(16);
mean.middleRows(0, 3) = pos;
mean.middleRows(3, 3).setZero();
mean.middleRows(6, 4) = Eigen::Vector4f(quat.w(), quat.x(), quat.y(), quat.z());
mean.middleRows(10, 3).setZero();
mean.middleRows(13, 3).setZero();
Eigen::MatrixXf cov = Eigen::MatrixXf::Identity(16, 16) * 0.01;
PoseSystem system;
ukf.reset(new kkl::alg::UnscentedKalmanFilterX<float, PoseSystem>(system, 16, 6, 7, process_noise, measurement_noise, mean, cov));
}
pose_estimator->predict（预测）
另外在hdl_localization.cpp中用到的pose_estimator->predict等也在本文件进行了解释。
void predict(const ros::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro) {
if((stamp - init_stamp).toSec() < cool_time_duration || prev_stamp.is_zero() || prev_stamp == stamp) {
prev_stamp = stamp;
return;
}
double dt = (stamp - prev_stamp).toSec();
prev_stamp = stamp;
ukf->setProcessNoiseCov(process_noise * dt);
ukf->system.dt = dt;
Eigen::VectorXf control(6);
control.head<3>() = acc;
control.tail<3>() = gyro;
ukf->predict(control);
}
pose_estimator->correct(观测)
pcl::PointCloud<PointT>::Ptr correct(const pcl::PointCloud<PointT>::ConstPtr& cloud) {
Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity();
init_guess.block<3, 3>(0, 0) = quat().toRotationMatrix();
init_guess.block<3, 1>(0, 3) = pos();
pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
registration->setInputSource(cloud);
registration->align(*aligned, init_guess);
Eigen::Matrix4f trans = registration->getFinalTransformation();
Eigen::Vector3f p = trans.block<3, 1>(0, 3);
Eigen::Quaternionf q(trans.block<3, 3>(0, 0));
if(quat().coeffs().dot(q.coeffs()) < 0.0f) {
q.coeffs() *= -1.0f;
}
Eigen::VectorXf observation(7);
observation.middleRows(0, 3) = p;
observation.middleRows(3, 4) = Eigen::Vector4f(q.w(), q.x(), q.y(), q.z());
ukf->correct(observation);
return aligned;
}
Original: https://blog.csdn.net/qq_46480130/article/details/125010146
Author: 入门打工人
Title: hdl_localization代码解析
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/558969/
转载文章受原作者版权保护。转载请注明原作者出处！