目录
1. NDT 的算法处理流程
2. NDT 公式推导
3. NDT 实例
3.1 常规NDT的位姿估计
3.2 front_end_node
1、ROS常规初始化
2、初始化操作：读取传感器数据、获取lidar-to-imu变换矩阵、保证三个传感器时间对齐
3、GNSS初始化位姿、更新里程计位姿
4、前端里程计
3.3 front_end
update()
UpdateNewFrame（）
下面部分引自无人驾驶汽车系统入门（十三）——正态分布变换（NDT）配准与无人车定位_AdamShan的博客-CSDN博客_ndt无人驾驶
使用正态分布来表示原本离散的点云有诸多好处，这种分块（通过 一个个cell）的光滑的表示是连续可导的，每一个概率密度函数可以被认为是一个局部表面的近似，它不但描述了这个表面在空间中的位置，同时还包含了这个表面的方向和光滑性等信息。
上图中立方体的边长为1米，其中越明亮的位置表示概率越高。此外，局部表面的方向和光滑性则可以通过协方差矩阵的特征值和特征向量反映出来。我们以三维的概率密度函数为例，如果三个特征值很接近，那么这个正态分布描述的表面是一个球面，如果一个特征值远大于另外两个特征值，则这个正态分布描述的是一条线，如果一个特征值远小于另外两个特征值，则这个正态分布描述的是一个平面。下图描述了协方差矩阵特征值与表面形状之间的关系：
变换参数和最大似然
NDT并没有比较两个点云点与点之间的差距，而是先将参考点云（即高精度地图）转换为多维变量的正态分布（使用正态分布来表示原本离散的点云），如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。
PCL 中ndt相关参数
1. setInputCloud (cloud_source) 设置原始点云
2. setInputTarget (cloud_target) 设置目标点云
3. setMaxCorrespondenceDistance（） 设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。默认值为：1.7976931348623157e+308，基本上对所有点都计算了匹配点。
4. 三个迭代终止条件设置：
1) setMaximumIterations（） 设置最大的迭代次数。迭代停止条件之一
2) setTransformationEpsilon（） 设置前后两次迭代的转换矩阵的最大容差（epsilion），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。迭代停止条件之二，默认值为：0
3) setEuclideanFitnessEpsilon（） 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max ()
迭代满足上述任一条件，终止迭代。
5.getFinalTransformation () 获取最终的配准的转化矩阵，即原始点云到目标点云的刚体变换，返回Matrix4数据类型，该数据类型采用了另一个专门用于矩阵计算的开源c++库eigen。
6. align (PointCloudSource &output) 进行ICP配准，输出变换后点云
7. hasConverged（） 获取收敛状态，注意，只要迭代过程符合上述三个终止条件之一，该函数返回true。
8. min_number_correspondences_ 最小匹配点对数量，默认值为3，即由空间中的非共线的三点就能确定刚体变换，建议将该值设置大点，否则容易出现错误匹配。
9. 最终迭代终止的原因可从convergence_criteria_变量中获取
注意：getFitnessScore（）用于获取迭代结束后目标点云和配准后的点云的最近点之间距离的均值。
参考下面作者的代码示例 NDT在slam中前端的用法
从零开始做自动驾驶定位(四): 前端里程计之初试 - 知乎
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
// 读取点云pcd文件
pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud_point(std::string const &file_path){
// Loading first scan.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (file_path, *cloud) == -1)
{
PCL_ERROR ("Couldn't read the pcd file\n");
return nullptr;
}
return cloud;
}
void visualizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud){
// 1、初始化点云可视化
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer_final->setBackgroundColor (0, 0, 0);
///2、对目标点云和匹配后的点云上色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color (target_cloud, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color (output_cloud, 0, 255, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
// 3、开始可视化，坐标系和相机参数
viewer_final->addCoordinateSystem (1.0, "global");
viewer_final->initCameraParameters ();
///4、Wait until visualizer window is closed.
while (!viewer_final->wasStopped ())
{
viewer_final->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
int main(int argc, char **argv) {
// 1、读取pcd文件（source和target）
auto target_cloud = read_cloud_point(argv[1]);
std::cout << "Loaded " << target_cloud->size () << " data points from cloud1.pcd" << std::endl;
auto input_cloud = read_cloud_point(argv[2]);
std::cout << "Loaded " << input_cloud->size () << " data points from cloud2.pcd" << std::endl;
// 2、过滤输入点云（减少数据量到10%）
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);
approximate_voxel_filter.setInputCloud(input_cloud);
approximate_voxel_filter.filter(*filtered_cloud);
std::cout<<"Filtered cloud contains "<< filtered_cloud->size() << "data points from cloud2.pcd" << std::endl;
// 3、初始化NDT并设置参数
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setTransformationEpsilon(0.01);// 优化过程是否收敛到的阈值
ndt.setStepSize(0.1);// 设置牛顿法优化的最大步长
ndt.setResolution(1.0);// 设置网格化时立方体的边长
ndt.setMaximumIterations(35); // 优化的迭代次数
ndt.setInputSource(filtered_cloud); // 待匹配点云
ndt.setInputTarget(target_cloud); // 目标点云
// 4、初始化变换参数
Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
// 5、开始配准优化并保存配准后的点云图，保存下来
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*output_cloud, init_guess);
std::cout << "Normal Distribution Transform has converged:" << ndt.hasConverged()
<< "score: " << ndt.getFitnessScore() << std::endl;
pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
pcl::io::savePCDFileASCII("cloud3.pcd", *output_cloud);
// 6、可视化
visualizer(target_cloud, output_cloud);
return 0;
}
前端激光雷达里程计主要分为：点云下采样滤波、点云匹配、位姿估计、关键帧选取等步骤。具体流程为：
第一帧点云数据设置为地图；
提取关键帧点云，拼接成地图，保证稀疏点云数据；
除了全局地图，还需要在当前帧附近形成滑动窗局部地图，减小计算量；
匹配之前需要滤波，对点云稀疏化；可以使用PCL中的voxel_filter基本原理是把三维空间划分未等尺寸的立方体格子，在一个立方体格子内最多只留一个点，这样就起到稀疏作用。
点云匹配对位姿预测值比较敏感，所以在载体运动时，不能以它上一帧的位姿作为这一帧的预测值，可以使用IMU预测，也可以使用运动模型预测。
激光雷达点云前端里程计主要包含以下步骤
订阅发布节点，包含订阅sub 3个传感器消息：点云、imu、GNSS。
发布pub 5个节点：点云、局部地图、全局地图、点云里程计、GNSS，
坐标转换的Lidar-to-imu变换矩阵T
参数定义上订阅和发布的指针分别命名为XXX_sub_ptr或者XXX_pub_ptr，传感器数据队列命名为XXX_data_ptr，针对地图指针为XXX_map_ptr，最重要的前端里程计指针为front_end_ptr。
重要变量：
// 三个传感器sub消息
cloud_sub_ptr,imu_sub_ptr,gnss_sub_ptr
// 两个传感器发布pub节点
cloud_pub_ptr,gnss_pub_ptr
// 发布的局部、全局地图
local_map_pub_ptr,global_map_pub_ptr
// 发布的点云里程计
laser_odom_pub_ptr
// 前端
front_end_ptr
// 三个传感器队列buff
cloud_data_buff，imu_data_buff，gnss_data_buff
// lidar-to-imu变换矩阵
lidar_to_imu
// 三个指针：扫描点云、局部，全局地图
current_scan_ptr,local_map_ptr，global_map_ptr
ROS初始化
ros::init(argc, argv, "front_end_node");
ros::NodeHandle nh;
// 三个订阅sub消息：点云、imu、GNSS
std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);
std::shared_ptr<IMUSubscriber> imu_sub_ptr = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);
std::shared_ptr<GNSSSubscriber> gnss_sub_ptr = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);
// lidar-to-imu
std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");
// 发布pub节点：点云、局部地图、全局地图
std::shared_ptr<CloudPublisher> cloud_pub_ptr = std::make_shared<CloudPublisher>(nh, "current_scan", 100, "/map");
std::shared_ptr<CloudPublisher> local_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "local_map", 100, "/map");
std::shared_ptr<CloudPublisher> global_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "global_map", 100, "/map");
// 点云里程计、GNSS
std::shared_ptr<OdometryPublisher> laser_odom_pub_ptr = std::make_shared<OdometryPublisher>(nh, "laser_odom", "map", "lidar", 100);
std::shared_ptr<OdometryPublisher> gnss_pub_ptr = std::make_shared<OdometryPublisher>(nh, "gnss", "map", "lidar", 100);
// 前端
std::shared_ptr<FrontEnd> front_end_ptr = std::make_shared<FrontEnd>();
// 三个数据队列buff
std::deque<CloudData> cloud_data_buff;
std::deque<IMUData> imu_data_buff;
std::deque<GNSSData> gnss_data_buff;
// lidar到imu的转换矩阵
Eigen::Matrix4f lidar_to_imu = Eigen::Matrix4f::Identity();
bool transform_received = false;
bool gnss_origin_position_inited = false;
bool front_end_pose_inited = false;
// 三个指针：点云、局部地图、全局地图
CloudData::CLOUD_PTR local_map_ptr(new CloudData::CLOUD());
CloudData::CLOUD_PTR global_map_ptr(new CloudData::CLOUD());
CloudData::CLOUD_PTR current_scan_ptr(new CloudData::CLOUD());
double run_time = 0.0;
double init_time = 0.0;
bool time_inited = false;
bool has_global_map_published = false;
主要是为了取出三个传感器数据，在取出之前先把最新扫描的传感器数据添加到队列buff中，然后取出过程中保证三个传感器时间对齐，最后要把读取的传感器数据从队列buff中剔除。
重要变量：
// lidar-to-imu变换矩阵
lidar_to_imu
// 三个传感器数据
cloud_data，imu_data ，gnss_data
ros::Rate rate(100);
while (ros::ok()) {
ros::spinOnce();
// 1.读取新扫描的数据添加到三个队列buff中，即更新传感器buff
cloud_sub_ptr->ParseData(cloud_data_buff);
imu_sub_ptr->ParseData(imu_data_buff);
gnss_sub_ptr->ParseData(gnss_data_buff);
if (!transform_received) {
if (lidar_to_imu_ptr->LookupData(lidar_to_imu)) {
transform_received = true;
}
} else {
while (cloud_data_buff.size() > 0 && imu_data_buff.size() > 0 && gnss_data_buff.size() > 0) {
// 1.读取三个传感器数据
CloudData cloud_data = cloud_data_buff.front();
IMUData imu_data = imu_data_buff.front();
GNSSData gnss_data = gnss_data_buff.front();
if (!time_inited) {
time_inited = true;
init_time = cloud_data.time;
} else {
run_time = cloud_data.time - init_time;// 点云运行时间
}
// 2.保证三个传感器时间对齐
double d_time = cloud_data.time - imu_data.time;
if (d_time < -0.05) {
cloud_data_buff.pop_front();
} else if (d_time > 0.05) {
imu_data_buff.pop_front();
gnss_data_buff.pop_front();
} else
{ // 剔除读取出的传感器数据
cloud_data_buff.pop_front();
imu_data_buff.pop_front();
gnss_data_buff.pop_front();
三种传感器：雷达点云、imu、GNSS的变量内容为：
激光雷达点云cloud_data.h
构成分为两部分：时间戳+点云指针
//时间戳+点云指针
class CloudData {
public:
using POINT = pcl::PointXYZ;// 三维点
using CLOUD = pcl::PointCloud<POINT>;// 点云
using CLOUD_PTR = CLOUD::Ptr;// 点云指针
public:
CloudData()
:cloud_ptr(new CLOUD()) {
}
public:
double time = 0.0;
CLOUD_PTR cloud_ptr;
};
imu_data.h
imu包含时间戳、线速度、角速度、方向角类（四元数+归一化）
类成员函数：四元数转旋转矩阵GetOrientationMatrix()
class IMUData {
public:
struct LinearAcceleration {
double x = 0.0;
double y = 0.0;
double z = 0.0;
};
struct AngularVelocity {
double x = 0.0;
double y = 0.0;
double z = 0.0;
};
// 方向角类（四元数+归一化）
class Orientation {
public:
double x = 0.0;
double y = 0.0;
double z = 0.0;
double w = 0.0;
public:
void Normlize() {
double norm = sqrt(pow(x, 2.0) + pow(y, 2.0) + pow(z, 2.0) + pow(w, 2.0));
x /= norm;
y /= norm;
z /= norm;
w /= norm;
}
};
double time = 0.0;
LinearAcceleration linear_acceleration;
AngularVelocity angular_velocity;
Orientation orientation;
public:
// 把四元数转换成旋转矩阵送出去
Eigen::Matrix3f GetOrientationMatrix();
gnss_data.h
GNSS数据包括：时间戳、地理坐标系、东北天坐标系。
class GNSSData {
public:
double time = 0.0;// 时间戳
// 地理坐标系：经纬度、高度
double longitude = 0.0;
double latitude = 0.0;
double altitude = 0.0;
// 东北天坐标系
double local_E = 0.0;
double local_N = 0.0;
double local_U = 0.0;
int status = 0;
int service = 0;
private:
static GeographicLib::LocalCartesian geo_converter;
static bool origin_position_inited;
类成员函数：初始化位置、更新位姿。
// 初始化位置reset
void GNSSData::InitOriginPosition() {
geo_converter.Reset(latitude, longitude, altitude);
origin_position_inited = true;
}
// 更新位姿
void GNSSData::UpdateXYZ() {
if (!origin_position_inited) {
LOG(WARNING) << "GeoConverter has not set origin position";
}
geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);
}
获取Lidar-to-IMU的转换矩阵
test_frame_node.cpp中：初始化为
// 一个坐标系转换指针ptr:基坐标系雷达、子坐标系imu
std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");
主要利用tf_lisener.cpp中的两个函数:
// 给lidar-to-imu赋值
bool TFListener::LookupData(Eigen::Matrix4f& transform_matrix) {
try {
tf::StampedTransform transform;
listener_.lookupTransform(base_frame_id_, child_frame_id_, ros::Time(0), transform);
TransformToMatrix(transform, transform_matrix);
return true;
} catch (tf::TransformException &ex) {
return false;
}
}
bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) {
Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
double roll, pitch, yaw;
tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);
Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ());
// 此矩阵为 child_frame_id 到 base_frame_id 的转换矩阵 zyx
transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
return true;
}
利用GNSS数据作为初始化位姿，然后从LIdar坐标系转化到imu坐标系，最后把估计位姿发布出去。
重要变量：
// 里程计估计位姿
odometry_matrix
Eigen::Matrix4f odometry_matrix = Eigen::Matrix4f::Identity();
// 1.GNSS数据初始化位姿
if (!gnss_origin_position_inited) {
gnss_data.InitOriginPosition();
gnss_origin_position_inited = true;
}
// 2.GNSS数据更新里程计位姿
gnss_data.UpdateXYZ();
odometry_matrix(0,3) = gnss_data.local_E;
odometry_matrix(1,3) = gnss_data.local_N;
odometry_matrix(2,3) = gnss_data.local_U;
odometry_matrix.block<3,3>(0,0) = imu_data.GetOrientationMatrix();
// 从lidar坐标系转换到imu系
odometry_matrix *= lidar_to_imu;
// 3、发布消息
gnss_pub_ptr->Publish(odometry_matrix);
使用GNSS数据（平移）和IMU（旋转）数据初始化位姿、更新位姿的两个函数为
gnss_data.InitOriginPosition();
gnss_data.UpdateXYZ();
函数内部实现为：
geo_converter.Reset(latitude, longitude, altitude);
geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);
这是最重要的里程计部分，其实就是对front_end文件进行详细解读。
// 1.前端里程计初始化
if (!front_end_pose_inited) {
front_end_pose_inited = true;
front_end_ptr->SetInitPose(odometry_matrix);
}
// 2.更新预测位姿为当前估计位姿
front_end_ptr->SetPredictPose(odometry_matrix);
// 3.更新位姿
Eigen::Matrix4f laser_matrix = front_end_ptr->Update(cloud_data);
// 4.发布位姿
laser_odom_pub_ptr->Publish(laser_matrix);
// 5.获得当前帧点云和局部地图并发布（都是下采样）
// 获得当前扫描并发布
front_end_ptr->GetCurrentScan(current_scan_ptr);
cloud_pub_ptr->Publish(current_scan_ptr);
// 获得新局部地图并发布
if (front_end_ptr->GetNewLocalMap(local_map_ptr))
local_map_pub_ptr->Publish(local_map_ptr);
}
// 6.如果运行时间超了且全局地图还没发布，获得下采样后的全局地图并发布
if (run_time > 460.0 && !has_global_map_published) {
if (front_end_ptr->GetNewGlobalMap(global_map_ptr)) {
global_map_pub_ptr->Publish(global_map_ptr);
has_global_map_published = true;
}
}
接下来专门对front_end.cpp进行讲解。
首先建立一个Frame类数据结构：
Frame类两个：位姿+点云数据
class Frame {
public:
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
CloudData cloud_data;
};
重要变量：
// 三个滤波器
cloud_filter_,local_map_filter_,display_filter_
// ndt匹配方法的对象
pcl::NormalDistributionsTransform<>::PTR ndt_ptr_
// 局部、全局地图队列
local_map_frames_,global_map_frames_
// 指针：局部、全局地图、点云结果
local_map_ptr_，global_map_ptr_，result_cloud_ptr_
// Frame对象
current_frame_
// 初始位姿，预测位姿
init_pose,predict_pose
初始化
// 构造函数初始化
FrontEnd::FrontEnd()
:ndt_ptr_(new pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>()),
local_map_ptr_(new CloudData::CLOUD()),
global_map_ptr_(new CloudData::CLOUD()),
result_cloud_ptr_(new CloudData::CLOUD()) {
// 给个默认参数，以免类的使用者在匹配之前忘了设置参数
cloud_filter_.setLeafSize(1.3, 1.3, 1.3);
local_map_filter_.setLeafSize(0.6, 0.6, 0.6);
display_filter_.setLeafSize(0.5, 0.5, 0.5);
ndt_ptr_->setResolution(1.0);// NDT网格结构的分辨率
ndt_ptr_->setStepSize(0.1);// 为More-Thuente线搜索设置最大步长
ndt_ptr_->setTransformationEpsilon(0.01);// 为终止条件设置最大转换差异
ndt_ptr_->setMaximumIterations(30);// 匹配迭代最大次数
}
主要的两个函数为更新位姿Update（）和更新关键帧UpdateNewFrame（）
输入当前帧扫描的点云图，对位姿进行更新
主要分为以下流程：
1、参数初始化
获得时间戳，剔除无效点云，对点云下采样滤波，初始化四个位姿。
// 1.参数初始化
current_frame_.cloud_data.time = cloud_data.time;// 时间戳
// 剔除无效点云：输入点云，输出点云，索引
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud_data.cloud_ptr, *current_frame_.cloud_data.cloud_ptr, indices);
// 下采样滤波
CloudData::CLOUD_PTR filtered_cloud_ptr(new CloudData::CLOUD());// 过滤的点云指针
cloud_filter_.setInputCloud(current_frame_.cloud_data.cloud_ptr);
cloud_filter_.filter(*filtered_cloud_ptr);
// 四个位姿
static Eigen::Matrix4f step_pose = Eigen::Matrix4f::Identity();// 初始化下一个位姿
static Eigen::Matrix4f last_pose = init_pose_;// 更新上一帧位姿
static Eigen::Matrix4f predict_pose = init_pose_;// 更新预测位姿
static Eigen::Matrix4f last_key_frame_pose = init_pose_;// 更新上一帧位姿
2、第一帧为地图
此时把当前帧数据作为第一个关键帧，并更新局部地图容器和全局地图容器
if (local_map_frames_.size() == 0) {
current_frame_.pose = init_pose_;
UpdateNewFrame(current_frame_);// 关键函数
return current_frame_.pose;
}
3、正常匹配
ndt_ptr_->setInputSource(filtered_cloud_ptr);// 要匹配的点云
ndt_ptr_->align(*result_cloud_ptr_, predict_pose);// 点云配准，变换后的点云在result中
current_frame_.pose = ndt_ptr_->getFinalTransformation();// 获得位姿
4、关键帧选取
// 4.隔2米设置一个关键帧，匹配之后根据距离判断是否需要生成新的关键帧，如果需要，则做相应更新
if (fabs(last_key_frame_pose(0,3) - current_frame_.pose(0,3)) +
fabs(last_key_frame_pose(1,3) - current_frame_.pose(1,3)) +
fabs(last_key_frame_pose(2,3) - current_frame_.pose(2,3)) > 2.0) {
//更新帧
UpdateNewFrame(current_frame_);
last_key_frame_pose = current_frame_.pose;
}
return current_frame_.pose;
主要分为四步骤
创建Frame对象，保存关键帧点云
更新局部地图
更新ndt匹配的目标点云
更新全局地图
1、创建Frame对象，保存关键帧点云
由于用的是共享指针，所以直接复制只是复制了一个指针而已，此时无论你放多少个关键帧在容器里，这些关键帧点云指针都是指向的同一个点云
Frame key_frame = new_key_frame;
key_frame.cloud_data.cloud_ptr.reset(new CloudData::CLOUD(*new_key_frame.cloud_data.cloud_ptr));
CloudData::CLOUD_PTR transformed_cloud_ptr(new CloudData::CLOUD());
2.更新局部地图
// 先加到滑窗队列中
local_map_frames_.push_back(key_frame);
while (local_map_frames_.size() > 20) {// 滑窗大小固定
local_map_frames_.pop_front();
}
// 再把局部地图指针进行更新
local_map_ptr_.reset(new CloudData::CLOUD());
for (size_t i = 0; i < local_map_frames_.size(); ++i) {// 遍历滑动窗
// 恢复下采样的点云数据，参数：输入，输出，估计的姿态
pcl::transformPointCloud(*local_map_frames_.at(i).cloud_data.cloud_ptr,
*transformed_cloud_ptr,
local_map_frames_.at(i).pose);
// 局部地图指针
*local_map_ptr_ += *transformed_cloud_ptr;
}
// 最后表示局部地图更新完成
has_new_local_map_ = true;
3.更新ndt匹配的目标点云
// 如果滑动窗数量小于10个，直接设置目标点云
if (local_map_frames_.size() < 10) {
ndt_ptr_->setInputTarget(local_map_ptr_);
}
else // 否则，先对局部地图进行下采样，再加进去。
{
CloudData::CLOUD_PTR filtered_local_map_ptr(new CloudData::CLOUD());
local_map_filter_.setInputCloud(local_map_ptr_);
local_map_filter_.filter(*filtered_local_map_ptr);
ndt_ptr_->setInputTarget(filtered_local_map_ptr);
}
4.更新全局地图
global_map_frames_.push_back(key_frame);
if (global_map_frames_.size() % 100 != 0) {
return;
} else {
global_map_ptr_.reset(new CloudData::CLOUD());
for (size_t i = 0; i < global_map_frames_.size(); ++i) {
pcl::transformPointCloud(*global_map_frames_.at(i).cloud_data.cloud_ptr,
*transformed_cloud_ptr,
global_map_frames_.at(i).pose);
*global_map_ptr_ += *transformed_cloud_ptr;
}
has_new_global_map_ = true;
}