使用ndt配准实现简单的激光里程计
1、原理
读入每一帧的点云，和地图做ndt配准，得到当前帧的pose，使用这个pose作为下一帧配准的guess_pose，如果当前帧是关键帧（关键帧如何选取？- 根据与上一关键帧的距离或者每隔几帧设置为关键帧），那么更新地图并且将当前帧更新为关键帧。
2、几点需要注意的
1. 关键帧的选取
当前帧距离上一个关键帧的位移超过阈值（1m），则认为当前帧为关键帧。
// 如果当前帧和上一帧的平移超过了min_add_scan_shift_,那么更新地图,并更新previous帧,发布地图
// .否则,previous帧不变
double shift = sqrt(pow(current_pose_.x - previous_pose_.x, 2.0) + pow(current_pose_.y - previous_pose_.y, 2.0));
if (shift >= min_add_scan_shift_)
{
map_ += *transformed_scan_ptr;
previous_pose_.x = current_pose_.x;previous_pose_.y = current_pose_.y;previous_pose_.z = current_pose_.z;
previous_pose_.roll = current_pose_.roll;previous_pose_.pitch = current_pose_.pitch;previous_pose_.yaw = current_pose_.yaw;
ndt.setInputTarget(map_ptr);
sensor_msgs::PointCloud2::Ptr map_msg_ptr(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*map_ptr, *map_msg_ptr);
ndt_map_pub_.publish(*map_msg_ptr);
}
2. 更新当前帧pose
ndt结束后根据输出的矩阵来更新当前帧的pose
// 配准后的输出点云
pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
ndt.align(*output_cloud, init_guess); // 这个点云是经过下采样的,我们不使用
t_localizer = ndt.getFinalTransformation();
t_base_link = t_localizer * tf_ltob_; // 变换到第一帧的坐标系下
pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, t_localizer);
tf::Matrix3x3 mat_b;
mat_b.setValue(static_cast<double>(t_base_link(0, 0)), static_cast<double>(t_base_link(0, 1)),
static_cast<double>(t_base_link(0, 2)), static_cast<double>(t_base_link(1, 0)),
static_cast<double>(t_base_link(1, 1)), static_cast<double>(t_base_link(1, 2)),
static_cast<double>(t_base_link(2, 0)), static_cast<double>(t_base_link(2, 1)),
static_cast<double>(t_base_link(2, 2)));
// Update current_pose_.
// 更新当前pose
current_pose_.x = t_base_link(0, 3);current_pose_.y = t_base_link(1, 3);current_pose_.z = t_base_link(2, 3);
mat_b.getRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw, 1);//mat2rpy
transform.setOrigin(tf::Vector3(current_pose_.x, current_pose_.y, current_pose_.z));
q.setRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw); //q from rpy
transform.setRotation(q);//trans from q
br_.sendTransform(tf::StampedTransform(transform, input->header.stamp, "map", "base_link"));
3. 设置guess_pose
第二帧和第一帧（map）之间的guess_pose是0变换
// 设置估计的pose 用来配准
Eigen::Matrix4f init_guess =
(init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol_; // 默认是0
后续的帧到地图上的guess_pose是由上一帧配准后输出的current_pose决定的
// 初始值为(0,0,0,0,0,0)
Eigen::Translation3f init_translation(current_pose_.x, current_pose_.y, current_pose_.z);
Eigen::AngleAxisf init_rotation_x(current_pose_.roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf init_rotation_y(current_pose_.pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf init_rotation_z(current_pose_.yaw, Eigen::Vector3f::UnitZ());
// 设置估计的pose 用来配准
Eigen::Matrix4f init_guess =
(init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol_; // 默认是0
3、效果
视频
使用ndt配准实现简单的激光里程计
4、参考
https://github.com/rsasaki0109/ndt_mapping
5、代码
https://github.com/suyunzzz/aiimooc_lesson/tree/master/aiimooc_syz5
ndt_mapping.cpp
/*
* Copyright 2015-2019 Autoware Foundation. All rights reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/*
Localization and mapping program using Normal Distributions Transform
Yuki KITSUKAWA
*/
#include "ndt_mapping.h"
using namespace std;
ndt_mapping::ndt_mapping()
{
// 初始化订阅者,发布者
points_sub_ = nh_.subscribe("/rslidar_points", 100000, &ndt_mapping::points_callback,this);
ndt_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/ndt_map", 1000);
current_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/current_pose", 1000);
// Default values
nh_.param("max_iter", max_iter_, 30); // ndt最大迭代次数
nh_.param("step_size", step_size_, 0.1); // ndt步长
nh_.param("ndt_res", ndt_res_, 5.0); // ndt分辨率
nh_.param("trans_eps", trans_eps_, 0.01); // ndt容许的误差 越小越严苛
nh_.param("voxel_leaf_size", voxel_leaf_size_, 2.0); // voxel的分辨率
nh_.param("scan_rate", scan_rate_, 10.0); // scan rate
nh_.param("min_scan_range", min_scan_range_, 5.0); // min_scan_range
nh_.param("max_scan_range", max_scan_range_, 200.0); // max scan range
nh_.param("use_imu", use_imu_, false); // 不使用imu
initial_scan_loaded = 0; // 未载入地图
min_add_scan_shift_ = 1.0; // 超过1m设置为关键帧
_tf_x=0.0, _tf_y=0.0, _tf_z=0.0, _tf_roll=0.0, _tf_pitch=0.0, _tf_yaw=0.0;
std::cout << "incremental_voxel_update: " << _incremental_voxel_update << std::endl; // voxel 增量
std::cout << "(tf_x,tf_y,tf_z,tf_roll,tf_pitch,tf_yaw): (" << _tf_x << ", " << _tf_y << ", " << _tf_z << ", "
<< _tf_roll << ", " << _tf_pitch << ", " << _tf_yaw << ")" << std::endl;
Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation base--->lidar
Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation
Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
tf_btol_ = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix(); // base to lidar 初始是旋转=0,平移=0
tf_ltob_ = tf_btol_.inverse(); // lidar --->base(地图)
map_.header.frame_id = "/map"; // 设置map_点云的frame.id 为 map 所以要想显示map_点云,必须要在rviz中选择map,并且还要做map和rslidar之间的
// 当前pose和之前的pose
current_pose_.x = current_pose_.y = current_pose_.z = 0.0;current_pose_.roll = current_pose_.pitch = current_pose_.yaw = 0.0;
previous_pose_.x = previous_pose_.y = previous_pose_.z = 0.0;previous_pose_.roll = previous_pose_.pitch = previous_pose_.yaw = 0.0;
voxel_grid_filter_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); // voxel采样
ndt.setTransformationEpsilon(trans_eps_); // 容许误差
ndt.setStepSize(step_size_); // 步长
ndt.setResolution(ndt_res_); // ndt分辨率
ndt.setMaximumIterations(max_iter_); // 最大迭代次数
is_first_map_ = true; // 是初始地图
// 打印初始参数
std::cout << "ndt_res: " << ndt_res_ << std::endl;
std::cout << "step_size: " << step_size_ << std::endl;
std::cout << "trans_epsilon: " << trans_eps_ << std::endl; //为终止条件设置最小转换差异
std::cout << "max_iter: " << max_iter_ << std::endl;
std::cout << "voxel_leaf_size: " << voxel_leaf_size_ << std::endl;
std::cout << "min_scan_range: " << min_scan_range_ << std::endl;
std::cout << "max_scan_range: " << max_scan_range_ << std::endl;
std::cout << "min_add_scan_shift: " << min_add_scan_shift_ << std::endl;
};
ndt_mapping::~ndt_mapping(){};
// poincloud的回调函数,接收到消息就调用
void ndt_mapping::points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
pcl::PointCloud<pcl::PointXYZI> tmp, scan; // tmp scan:裁剪后的点云
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>()); // 滤波之后的点云
pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>()); // 变换之后的点云
tf::Quaternion q; // 表示旋转的四元数 (x y z w)
Eigen::Matrix4f t_localizer(Eigen::Matrix4f::Identity()); // scan相对于map的变换矩阵
Eigen::Matrix4f t_base_link(Eigen::Matrix4f::Identity()); // scan相对于map的变换矩阵
//static tf::TransformBroadcaster br; // 已经加入到类成员中了
tf::Transform transform; // tf 传递的转换本身 ,为了发布pose用
// tmp 裁剪后 得到scan
pcl::fromROSMsg(*input, tmp);
double r; // 每一个点的水平深度 r=(x^2+y^2)^(1/2)
Eigen::Vector3d point_pos; // 不使用imu就用不到
pcl::PointXYZI p; // 点 p
// 裁剪点云
for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = tmp.begin(); item != tmp.end(); item++)
{
if(use_imu_){ // 使用imu预测pose
// deskew(TODO:inplement of predicting pose by imu)
point_pos.x() = (double)item->x;
point_pos.y() = (double)item->y;
point_pos.z() = (double)item->z;
double s = scan_rate_ * (double(item->intensity) - int(item->intensity));
point_pos.x() -= s * current_pose_msg_.pose.position.x;//current_pose_imu_
point_pos.y() -= s * current_pose_msg_.pose.position.y;
point_pos.z() -= s * current_pose_msg_.pose.position.z;
Eigen::Quaterniond start_quat, end_quat, mid_quat;
mid_quat.setIdentity();
end_quat = Eigen::Quaterniond(
current_pose_msg_.pose.orientation.w,
current_pose_msg_.pose.orientation.x,
current_pose_msg_.pose.orientation.y,
current_pose_msg_.pose.orientation.z);
start_quat = mid_quat.slerp(s, end_quat);
point_pos = start_quat.conjugate() * start_quat * point_pos;
point_pos.x() += current_pose_msg_.pose.position.x;
point_pos.y() += current_pose_msg_.pose.position.y;
point_pos.z() += current_pose_msg_.pose.position.z;
p.x = point_pos.x();
p.y = point_pos.y();
p.z = point_pos.z();
} // 不使用
else{ // 不适用imu
p.x = (double)item->x;
p.y = (double)item->y;
p.z = (double)item->z;
}
p.intensity = (double)item->intensity;
// minmax根据距离裁剪,太近或者太远的删掉
r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0));
if (min_scan_range_ < r && r < max_scan_range_)
{
scan.push_back(p);
}
}
// 最终得到scan
pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan)); // scan是在lidar坐标系下的
// Add initial point cloud to velodyne_map
// 如果是第一帧点云,那么将其设置位map_
if (initial_scan_loaded == 0)
{
pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, tf_btol_); // 第一帧转换到base下 变换矩阵应该是base相对于lidar的变换
map_ += *transformed_scan_ptr;
initial_scan_loaded = 1;
}
// 扫描到的点云下采样
voxel_grid_filter_.setInputCloud(scan_ptr);
voxel_grid_filter_.filter(*filtered_scan_ptr);
ndt.setInputSource(filtered_scan_ptr); // source:降采样后的点云
// 如果是最开始的第一帧点云的地图,直接设置配准的目标为这个地图
pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>(map_));
if (is_first_map_ == true){
ndt.setInputTarget(map_ptr); // target:map_
is_first_map_ = false;
}
// 初始值为(0,0,0,0,0,0)
Eigen::Translation3f init_translation(current_pose_.x, current_pose_.y, current_pose_.z);
Eigen::AngleAxisf init_rotation_x(current_pose_.roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf init_rotation_y(current_pose_.pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf init_rotation_z(current_pose_.yaw, Eigen::Vector3f::UnitZ());
// 设置估计的guess_pose 用来配准
Eigen::Matrix4f init_guess =
(init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol_; // 用于ndt配准的guess_pose,第一帧的默认是0
cout<<"tf_btol_:\n"<<tf_btol_<<endl;
cout<<"init_guess:\n"<<init_guess<<endl; // 估计的初始值
// 配准后的输出点云
pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
ndt.align(*output_cloud, init_guess); // 这个点云是经过下采样的,我们不使用
t_localizer = ndt.getFinalTransformation(); // source(scan) ---> target(map_)的变换 source(scan 当前帧)相对于target(关键帧 map_)的位姿
cout<<"t_localizer 当前帧相对于关键帧的位姿:\n"<<t_localizer<<endl;
t_base_link = t_localizer * tf_ltob_; // 当前帧(lidar)在base(map)下的pose
cout<<"tf_ltob_: \n"<<tf_ltob_<<endl;
cout<<"t_base_link 是啥意思: \n"<<t_base_link<<endl;
// source(scan) 经过这个函数转换到target(map_) 坐标系下
pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, t_localizer); // t_localizer:当前帧相对于map的位姿,transformed_scan_ptr:当前帧变换到map中的点云
tf::Matrix3x3 mat_b; // t_base_link 的旋转矩阵
mat_b.setValue(static_cast<double>(t_base_link(0, 0)), static_cast<double>(t_base_link(0, 1)),
static_cast<double>(t_base_link(0, 2)), static_cast<double>(t_base_link(1, 0)),
static_cast<double>(t_base_link(1, 1)), static_cast<double>(t_base_link(1, 2)),
static_cast<double>(t_base_link(2, 0)), static_cast<double>(t_base_link(2, 1)),
static_cast<double>(t_base_link(2, 2)));
// 更新current_pose_.
// 更新当前pose
current_pose_.x = t_base_link(0, 3);current_pose_.y = t_base_link(1, 3);current_pose_.z = t_base_link(2, 3); // 平移
mat_b.getRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw, 1);//mat2rpy 旋转
// current_pose_写入transform
transform.setOrigin(tf::Vector3(current_pose_.x, current_pose_.y, current_pose_.z)); // transform 设置平移
q.setRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw); //q from rpy
transform.setRotation(q);//trans from q
// 发布map和rslidar之间的 transform
br_.sendTransform(tf::StampedTransform(transform, input->header.stamp, "/map", "/rslidar")); // map--->scan的transform
// 如果当前帧和上一帧的平移超过了min_add_scan_shift_,那么更新地图,并更新previous帧,更新target,发布地图
// .否则,previous帧不变
double shift = sqrt(pow(current_pose_.x - previous_pose_.x, 2.0) + pow(current_pose_.y - previous_pose_.y, 2.0));
if (shift >= min_add_scan_shift_)
{
map_ += *transformed_scan_ptr; // 更新地图
// 更新previous帧的位姿
previous_pose_.x = current_pose_.x;previous_pose_.y = current_pose_.y;previous_pose_.z = current_pose_.z;
previous_pose_.roll = current_pose_.roll;previous_pose_.pitch = current_pose_.pitch;previous_pose_.yaw = current_pose_.yaw;
// 使用新的map_更新为target
ndt.setInputTarget(map_ptr);
// 发布地图
sensor_msgs::PointCloud2::Ptr map_msg_ptr(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*map_ptr, *map_msg_ptr);
ndt_map_pub_.publish(*map_msg_ptr);
}
//sensor_msgs::PointCloud2::Ptr map_msg_ptr(new sensor_msgs::PointCloud2);
//pcl::toROSMsg(*map_ptr, *map_msg_ptr);
//ndt_map_pub_.publish(*map_msg_ptr);// it makes rviz very slow.
// 发布当前帧的pose
current_pose_msg_.header.frame_id = "/map"; // 按理说只要发布了/map--->rslidar的头tf_tree,这两个frame_id就能随便选择了 .但是
current_pose_msg_.header.stamp = input->header.stamp;
current_pose_msg_.pose.position.x = current_pose_.x;current_pose_msg_.pose.position.y = current_pose_.y;current_pose_msg_.pose.position.z = current_pose_.z;
current_pose_msg_.pose.orientation.x = q.x();current_pose_msg_.pose.orientation.y = q.y();current_pose_msg_.pose.orientation.z = q.z();current_pose_msg_.pose.orientation.w = q.w();
current_pose_pub_.publish(current_pose_msg_);
std::cout << "-----------------------------------------------------------------" << std::endl;
std::cout << "Sequence number: " << input->header.seq << std::endl;
std::cout << "Number of scan points: " << scan_ptr->size() << " points." << std::endl;
std::cout << "Number of filtered scan points: " << filtered_scan_ptr->size() << " points." << std::endl;
std::cout << "transformed_scan_ptr: " << transformed_scan_ptr->points.size() << " points." << std::endl;
std::cout << "map: " << map_.points.size() << " points." << std::endl; // 一定是隔一段时间增加一下
std::cout << "NDT has converged: " << ndt.hasConverged() << std::endl;
std::cout << "Fitness score: " << ndt.getFitnessScore() << std::endl;
std::cout << "Number of iteration: " << ndt.getFinalNumIteration() << std::endl;
std::cout << "(x,y,z,roll,pitch,yaw):" << std::endl; // 当前帧的pose
std::cout << "(" << current_pose_.x << ", " << current_pose_.y << ", " << current_pose_.z << ", " << current_pose_.roll
<< ", " << current_pose_.pitch << ", " << current_pose_.yaw << ")" << std::endl;
std::cout << "Transformation Matrix:" << std::endl;
std::cout << t_localizer << std::endl; // 当前帧相对于map_的位姿
std::cout << "shift: " << shift << std::endl; // 当前帧与之map_的距离
// 保存pcd文件
// pcl::io::savePCDFile (`/home/s/Dataset/pcd/ndt_map.pcd`, *map_ptr);
std::cout << "-----------------------------------------------------------------" << std::endl;
}