3d激光slam建图与定位(1)_基于ndt算法定位

一.代码实现流程
二.ndt算法原理

一.该算法定位有三个进程文件
1.map_loader.cpp用于点云地图的读取，从文件中读取点云后对这个点云地图进行旋转平移后发布点云地图到ros

	#include "map_loader.h"MapLoader::MapLoader(ros::NodeHandle &nh){std::string pcd_file_path, map_topic;// 点云地图读取路径nh.param<std::string>("pcd_path", pcd_file_path, "");// 点云地图话题名nh.param<std::string>("map_topic", map_topic, "point_map");//初始位置的相对位置坐标变换 参数读取init_tf_params(nh);//要发布的map话题pc_map_pub_ = nh.advertise<sensor_msgs::PointCloud2>(map_topic, 10, true);// 点云地图路径存储file_list_.push_back(pcd_file_path);//从文件中读取pcdauto pc_msg = CreatePcd();// 根据初始坐标变换进行点云地图的坐标变换auto out_msg = TransformMap(pc_msg);
//发布点云地图话题if (out_msg.width != 0) {out_msg.header.frame_id = "map";pc_map_pub_.publish(out_msg);}}void MapLoader::init_tf_params(ros::NodeHandle &nh){// 从参数中加载坐标系变换值nh.param<float>("x", tf_x_, 0.0);nh.param<float>("y", tf_y_, 0.0);nh.param<float>("z", tf_z_, 0.0);nh.param<float>("roll", tf_roll_, 0.0);nh.param<float>("pitch", tf_pitch_, 0.0);nh.param<float>("yaw", tf_yaw_, 0.0);ROS_INFO_STREAM("x" << tf_x_ <<"y: "<<tf_y_<<"z: "<<tf_z_<<"roll: "<<tf_roll_<<" pitch: "<< tf_pitch_<<"yaw: "<<tf_yaw_);
}sensor_msgs::PointCloud2 MapLoader::TransformMap(sensor_msgs::PointCloud2 & in){pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc(new pcl::PointCloud<pcl::PointXYZ>);// 传感器点云格式转换为pcl点云格式pcl::fromROSMsg(in, *in_pc);pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_pc_ptr(new pcl::PointCloud<pcl::PointXYZ>);// 坐标变换Eigen::Translation3f tl_m2w(tf_x_, tf_y_, tf_z_);                 // tl: translationEigen::AngleAxisf rot_x_m2w(tf_roll_, Eigen::Vector3f::UnitX());  // rot: rotationEigen::AngleAxisf rot_y_m2w(tf_pitch_, Eigen::Vector3f::UnitY());Eigen::AngleAxisf rot_z_m2w(tf_yaw_, Eigen::Vector3f::UnitZ());Eigen::Matrix4f tf_m2w = (tl_m2w * rot_z_m2w * rot_y_m2w * rot_x_m2w).matrix();pcl::transformPointCloud(*in_pc, *transformed_pc_ptr, tf_m2w);// 保存变换后的点云地图// SaveMap(transformed_pc_ptr);sensor_msgs::PointCloud2 output_msg;pcl::toROSMsg(*transformed_pc_ptr, output_msg);return output_msg;
}void MapLoader::SaveMap(const pcl::PointCloud<pcl::PointXYZ>::Ptr map_pc_ptr){pcl::io::savePCDFile("/tmp/transformed_map.pcd", *map_pc_ptr);
}sensor_msgs::PointCloud2 MapLoader::CreatePcd()
{sensor_msgs::PointCloud2 pcd, part;for (const std::string& path : file_list_) {if (pcd.width == 0) {if (pcl::io::loadPCDFile(path.c_str(), pcd) == -1) {std::cerr << "load failed " << path << std::endl;}} else{if (pcl::io::loadPCDFile(path.c_str(), part) == -1) {std::cerr << "load failed " << path << std::endl;}pcd.width += part.width;pcd.row_step += part.row_step;pcd.data.insert(pcd.data.end(), part.data.begin(), part.data.end());}std::cerr << "load " << path << std::endl;if (!ros::ok()) break;}return pcd;
}int main(int argc, char** argv)
{ros::init(argc, argv, "map_loader");ROS_INFO("点云地图读取并发布map话题");ros::NodeHandle nh("~");MapLoader map_loader(nh);ros::spin();return 0;
}

2.points_downsampler.cpp对雷达数据的预处理，当雷达数据回调后进行点云范围的过滤，过滤掉过远的点，通过体素滤波进行下采样，提升匹配效率

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>// #include "points_downsampler.h"#define MAX_MEASUREMENT_RANGE 120.0ros::Publisher filtered_points_pub;// Leaf size of VoxelGrid filter.
static double voxel_leaf_size = 2.0;static bool _output_log = false;
static std::ofstream ofs;
static std::string filename;static std::string POINTS_TOPIC;
// 移除超出搜索半径的点
static pcl::PointCloud<pcl::PointXYZ> removePointsByRange(pcl::PointCloud<pcl::PointXYZ> scan, double min_range, double max_range)
{pcl::PointCloud<pcl::PointXYZ> narrowed_scan;narrowed_scan.header = scan.header;if( min_range>=max_range ) {ROS_ERROR_ONCE("min_range>=max_range @(%lf, %lf)", min_range, max_range );return scan;}double square_min_range = min_range * min_range;double square_max_range = max_range * max_range;for(pcl::PointCloud<pcl::PointXYZ>::const_iterator iter = scan.begin(); iter != scan.end(); ++iter){const pcl::PointXYZ &p = *iter;// 点云点到原点的位置的欧式距离double square_distance = p.x * p.x + p.y * p.y;if(square_min_range <= square_distance && square_distance <= square_max_range){narrowed_scan.points.push_back(p);}}return narrowed_scan;
}static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{pcl::PointCloud<pcl::PointXYZ> scan;// 传感器点云转换为pcl点云pcl::fromROSMsg(*input, scan);// 移除距离原点中心过远的点scan = removePointsByRange(scan, 0, MAX_MEASUREMENT_RANGE);pcl::PointCloud<pcl::PointXYZ>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(scan));pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());sensor_msgs::PointCloud2 filtered_msg;//体素滤波器过滤点云if (voxel_leaf_size >= 0.1){// Downsampling the velodyne scan using VoxelGrid filterpcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);voxel_grid_filter.setInputCloud(scan_ptr);voxel_grid_filter.filter(*filtered_scan_ptr);pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);}else{pcl::toROSMsg(*scan_ptr, filtered_msg);}filtered_msg.header = input->header;// 发布体素滤波之后的点云filtered_points_pub.publish(filtered_msg);}int main(int argc, char** argv)
{ros::init(argc, argv, "voxel_grid_filter");ros::NodeHandle nh;ros::NodeHandle private_nh("~");
// 点云话题名private_nh.getParam("points_topic", POINTS_TOPIC);// private_nh.getParam("output_log", _output_log);
//体素滤波器格子大小private_nh.param<double>("leaf_size", voxel_leaf_size, 2.0);ROS_INFO_STREAM("Voxel leaf size is: "<<voxel_leaf_size);// 写入log到本地if(_output_log == true){char buffer[80];std::time_t now = std::time(NULL);std::tm *pnow = std::localtime(&now);std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow);filename = "voxel_grid_filter_" + std::string(buffer) + ".csv";ofs.open(filename.c_str(), std::ios::app);}//发布滤波后的点云话题filtered_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);// 订阅从雷达拿下来的点云话题ros::Subscriber scan_sub = nh.subscribe(POINTS_TOPIC, 10, scan_callback);ros::spin();return 0;
}

3.ndt.cpp对点云地图，雷达点云，发布的雷达点云相对于点云地图的初始位置进行回调处理，通过ndt算法，将雷达点云匹配到点云地图，并计算出它们的位置姿态关系，这个就是定位

#include "ndt.h"NdtLocalizer::NdtLocalizer(ros::NodeHandle &nh, ros::NodeHandle &private_nh):nh_(nh), private_nh_(private_nh), tf2_listener_(tf2_buffer_){
//添加一个名为state的键 并将值设置为Initializingkey_value_stdmap_["state"] = "Initializing";// 初始参数读取init_params();// Publisherssensor_aligned_pose_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("points_aligned", 10);ndt_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("ndt_pose", 10);exe_time_pub_ = nh_.advertise<std_msgs::Float32>("exe_time_ms", 10);transform_probability_pub_ = nh_.advertise<std_msgs::Float32>("transform_probability", 10);iteration_num_pub_ = nh_.advertise<std_msgs::Float32>("iteration_num", 10);diagnostics_pub_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 10);// Subscribers// 重定位设置initial_pose_sub_ = nh_.subscribe("initialpose", 100, &NdtLocalizer::callback_init_pose, this);// 点云地图map_points_sub_ = nh_.subscribe("points_map", 1, &NdtLocalizer::callback_pointsmap, this);// 经过下采样之后的点云sensor_points_sub_ = nh_.subscribe("filtered_points", 1, &NdtLocalizer::callback_pointcloud, this);
// // 创建个独立线程去执行timer_diagnostic
//   diagnostic_thread_ = std::thread(&NdtLocalizer::timer_diagnostic, this);
//   diagnostic_thread_.detach();
}NdtLocalizer::~NdtLocalizer() {}void NdtLocalizer::timer_diagnostic()
{// 100hzros::Rate rate(100);while (ros::ok()) {diagnostic_msgs::DiagnosticStatus diag_status_msg;diag_status_msg.name = "ndt_scan_matcher";diag_status_msg.hardware_id = "";for (const auto & key_value : key_value_stdmap_) {diagnostic_msgs::KeyValue key_value_msg;// 键statekey_value_msg.key = key_value.first;//值 Initializingkey_value_msg.value = key_value.second;diag_status_msg.values.push_back(key_value_msg);}diag_status_msg.level = diagnostic_msgs::DiagnosticStatus::OK;diag_status_msg.message = "";if (key_value_stdmap_.count("state") && key_value_stdmap_["state"] == "Initializing") {diag_status_msg.level = diagnostic_msgs::DiagnosticStatus::WARN;diag_status_msg.message += "Initializing State. ";}if (key_value_stdmap_.count("skipping_publish_num") &&std::stoi(key_value_stdmap_["skipping_publish_num"]) > 1) {diag_status_msg.level = diagnostic_msgs::DiagnosticStatus::WARN;diag_status_msg.message += "skipping_publish_num > 1. ";}if (key_value_stdmap_.count("skipping_publish_num") &&std::stoi(key_value_stdmap_["skipping_publish_num"]) >= 5) {diag_status_msg.level = diagnostic_msgs::DiagnosticStatus::ERROR;diag_status_msg.message += "skipping_publish_num exceed limit. ";}diagnostic_msgs::DiagnosticArray diag_msg;diag_msg.header.stamp = ros::Time::now();diag_msg.status.push_back(diag_status_msg);diagnostics_pub_.publish(diag_msg);rate.sleep();}
}void NdtLocalizer::callback_init_pose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & initial_pose_msg_ptr)
{// base_link to mapif (initial_pose_msg_ptr->header.frame_id == map_frame_) {initial_pose_cov_msg_ = *initial_pose_msg_ptr;}else{// get TF from pose_frame to map_framegeometry_msgs::TransformStamped::Ptr TF_pose_to_map_ptr(new geometry_msgs::TransformStamped);get_transform(map_frame_, initial_pose_msg_ptr->header.frame_id, TF_pose_to_map_ptr);// transform pose_frame to map_framegeometry_msgs::PoseWithCovarianceStamped::Ptr mapTF_initial_pose_msg_ptr(new geometry_msgs::PoseWithCovarianceStamped);tf2::doTransform(*initial_pose_msg_ptr, *mapTF_initial_pose_msg_ptr, *TF_pose_to_map_ptr);initial_pose_cov_msg_ = *mapTF_initial_pose_msg_ptr;}// if click the initpose again, re init！init_pose = false;
}
// ndt的目标点云部分 通过ndt_new 赋值给ndt 也就是 点云地图
void NdtLocalizer::callback_pointsmap(const sensor_msgs::PointCloud2::ConstPtr & map_points_msg_ptr)
{const auto trans_epsilon = ndt_.getTransformationEpsilon();const auto step_size = ndt_.getStepSize();const auto resolution = ndt_.getResolution();const auto max_iterations = ndt_.getMaximumIterations();pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_new;ndt_new.setTransformationEpsilon(trans_epsilon);ndt_new.setStepSize(step_size);ndt_new.setResolution(resolution);ndt_new.setMaximumIterations(max_iterations);pcl::PointCloud<pcl::PointXYZ>::Ptr map_points_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);// 设置目标点云ndt_new.setInputTarget(map_points_ptr);// create Thread// detachpcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);// 执行ndt计算ndt_new.align(*output_cloud, Eigen::Matrix4f::Identity());// swapndt_map_mtx_.lock();ndt_ = ndt_new;ndt_map_mtx_.unlock();
}void NdtLocalizer::callback_pointcloud(const sensor_msgs::PointCloud2::ConstPtr & sensor_points_sensorTF_msg_ptr)
{// 时间戳const auto exe_start_time = std::chrono::system_clock::now();// mutex Mapstd::lock_guard<std::mutex> lock(ndt_map_mtx_);
// lidar frameconst std::string sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id;//点云进来的时间戳const auto sensor_ros_time = sensor_points_sensorTF_msg_ptr->header.stamp;boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_sensorTF_ptr(new pcl::PointCloud<pcl::PointXYZ>);// 传感器点云转换到pcl点云pcl::fromROSMsg(*sensor_points_sensorTF_msg_ptr, *sensor_points_sensorTF_ptr);// get TF base to sensorgeometry_msgs::TransformStamped::Ptr TF_base_to_sensor_ptr(new geometry_msgs::TransformStamped);// 获取map to base_link的坐标变换get_transform(base_frame_, sensor_frame, TF_base_to_sensor_ptr);const Eigen::Affine3d base_to_sensor_affine = tf2::transformToEigen(*TF_base_to_sensor_ptr);const Eigen::Matrix4f base_to_sensor_matrix = base_to_sensor_affine.matrix().cast<float>();boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_baselinkTF_ptr(new pcl::PointCloud<pcl::PointXYZ>);// 将点云坐标变换到map坐标系下pcl::transformPointCloud(*sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix);//设置ndt的输入点云ndt_.setInputSource(sensor_points_baselinkTF_ptr);if (ndt_.getInputTarget() == nullptr) {ROS_WARN_STREAM_THROTTLE(1, "No MAP!");return;}// alignEigen::Matrix4f initial_pose_matrix;// 如果进行重定位设置了if (!init_pose){Eigen::Affine3d initial_pose_affine;// 将ros格式的初始位姿发布 转换成 eigen格式tf2::fromMsg(initial_pose_cov_msg_.pose.pose, initial_pose_affine);initial_pose_matrix = initial_pose_affine.matrix().cast<float>();//将重定位的发布赋值给pre_transpre_trans = initial_pose_matrix;// 只执行一次的目的init_pose = true;}else{//ndt初始计算位置初值  当前变换矩阵*（当前与上一次之间的变换矩阵）initial_pose_matrix = pre_trans * delta_trans;}pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);const auto align_start_time = std::chrono::system_clock::now();// 键的值改变来确定ndt执行状态key_value_stdmap_["state"] = "Aligning";// 执行ndtndt_.align(*output_cloud, initial_pose_matrix);// 键的值改变来确定ndt执行状态key_value_stdmap_["state"] = "Sleeping";// 时间戳const auto align_end_time = std::chrono::system_clock::now();// 单纯的ndt执行时间差值const double align_time = std::chrono::duration_cast<std::chrono::microseconds>(align_end_time - align_start_time).count() /1000.0;
//ndt变换得到的目标点云和输入点云的关系const Eigen::Matrix4f result_pose_matrix = ndt_.getFinalTransformation();Eigen::Affine3d result_pose_affine;result_pose_affine.matrix() = result_pose_matrix.cast<double>();//定位结果const geometry_msgs::Pose result_pose_msg = tf2::toMsg(result_pose_affine);const auto exe_end_time = std::chrono::system_clock::now();// 执行一次ndt的时间差值，包括获取目标点云与实际点云的坐标关系流程const double exe_time = std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count() / 1000.0;
// ndt变换概率const float transform_probability = ndt_.getTransformationProbability();//ndt的最终迭代次数const int iteration_num = ndt_.getFinalNumIteration();bool is_converged = true;static size_t skipping_publish_num = 0;if (iteration_num >= ndt_.getMaximumIterations() + 2 ||transform_probability < converged_param_transform_probability_) {is_converged = false;++skipping_publish_num;std::cout << "Not Converged" << std::endl;} else {skipping_publish_num = 0;}// 目标点云与当前点云的姿态关系delta_trans = pre_trans.inverse() * result_pose_matrix;
// 位置向量Eigen::Vector3f delta_translation = delta_trans.block<3, 1>(0, 3);std::cout<<"delta x: "<<delta_translation(0) << " y: "<<delta_translation(1)<<" z: "<<delta_translation(2)<<std::endl;
// 旋转向量Eigen::Matrix3f delta_rotation_matrix = delta_trans.block<3, 3>(0, 0);Eigen::Vector3f delta_euler = delta_rotation_matrix.eulerAngles(2,1,0);std::cout<<"delta yaw: "<<delta_euler(0) << " pitch: "<<delta_euler(1)<<" roll: "<<delta_euler(2)<<std::endl;pre_trans = result_pose_matrix;// publishgeometry_msgs::PoseStamped result_pose_stamped_msg;result_pose_stamped_msg.header.stamp = sensor_ros_time;result_pose_stamped_msg.header.frame_id = map_frame_;result_pose_stamped_msg.pose = result_pose_msg;if (is_converged) {// 发布定位结果ndt_pose_pub_.publish(result_pose_stamped_msg);}//发布map to base_link的坐标关系publish_tf(map_frame_, base_frame_, result_pose_stamped_msg);
std::cout<<"发布了坐标变换"<<map_frame_<<"dsa"<<base_frame_<<std::endl;// publish aligned point cloudpcl::PointCloud<pcl::PointXYZ>::Ptr sensor_points_mapTF_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, result_pose_matrix);sensor_msgs::PointCloud2 sensor_points_mapTF_msg;pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg);sensor_points_mapTF_msg.header.stamp = sensor_ros_time;sensor_points_mapTF_msg.header.frame_id = map_frame_;sensor_aligned_pose_pub_.publish(sensor_points_mapTF_msg);std_msgs::Float32 exe_time_msg;exe_time_msg.data = exe_time;exe_time_pub_.publish(exe_time_msg);std_msgs::Float32 transform_probability_msg;transform_probability_msg.data = transform_probability;transform_probability_pub_.publish(transform_probability_msg);std_msgs::Float32 iteration_num_msg;iteration_num_msg.data = iteration_num;iteration_num_pub_.publish(iteration_num_msg);key_value_stdmap_["seq"] = std::to_string(sensor_points_sensorTF_msg_ptr->header.seq);key_value_stdmap_["transform_probability"] = std::to_string(transform_probability);key_value_stdmap_["iteration_num"] = std::to_string(iteration_num);key_value_stdmap_["skipping_publish_num"] = std::to_string(skipping_publish_num);std::cout << "------------------------------------------------" << std::endl;std::cout << "align_time: " << align_time << "ms" << std::endl;std::cout << "exe_time: " << exe_time << "ms" << std::endl;std::cout << "trans_prob: " << transform_probability << std::endl;std::cout << "iter_num: " << iteration_num << std::endl;std::cout << "skipping_publish_num: " << skipping_publish_num << std::endl;
}void NdtLocalizer::init_params(){
// 底盘的idprivate_nh_.getParam("base_frame", base_frame_);ROS_INFO("base_frame_id: %s", base_frame_.c_str());//收敛判定阈值 两次迭代之间的矩阵变化量小于该值认为已经收敛 停止迭代double trans_epsilon = ndt_.getTransformationEpsilon();// 移动步长 格子大小double step_size = ndt_.getStepSize();// 点云分辨率double resolution = ndt_.getResolution();// 迭代次数int max_iterations = ndt_.getMaximumIterations();
// 从配置中读取这几个参数private_nh_.getParam("trans_epsilon", trans_epsilon);private_nh_.getParam("step_size", step_size);private_nh_.getParam("resolution", resolution);private_nh_.getParam("max_iterations", max_iterations);map_frame_ = "map";
// 进行ndt参数设置ndt_.setTransformationEpsilon(trans_epsilon);ndt_.setStepSize(step_size);ndt_.setResolution(resolution);ndt_.setMaximumIterations(max_iterations);private_nh_.getParam("converged_param_transform_probability", converged_param_transform_probability_);
}bool NdtLocalizer::get_transform(const std::string & target_frame, const std::string & source_frame,const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr, const ros::Time & time_stamp)
{if (target_frame == source_frame) {transform_stamped_ptr->header.stamp = time_stamp;transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return true;}try {*transform_stamped_ptr =tf2_buffer_.lookupTransform(target_frame, source_frame, time_stamp);} catch (tf2::TransformException & ex) {ROS_WARN("%s", ex.what());ROS_ERROR("Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str());transform_stamped_ptr->header.stamp = time_stamp;transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return false;}return true;
}
// 返回baseid to map 的坐标变换
bool NdtLocalizer::get_transform(const std::string & target_frame, const std::string & source_frame,const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr)
{// 如果id相同 则坐标变换为0if (target_frame == source_frame) {transform_stamped_ptr->header.stamp = ros::Time::now();transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return true;}try {*transform_stamped_ptr =tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0), ros::Duration(1.0));} catch (tf2::TransformException & ex) {ROS_WARN("%s", ex.what());ROS_ERROR("Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str());transform_stamped_ptr->header.stamp = ros::Time::now();transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return false;}return true;
}void NdtLocalizer::publish_tf(const std::string & frame_id, const std::string & child_frame_id,const geometry_msgs::PoseStamped & pose_msg)
{geometry_msgs::TransformStamped transform_stamped;transform_stamped.header.frame_id = frame_id;transform_stamped.child_frame_id = child_frame_id;transform_stamped.header.stamp = pose_msg.header.stamp;transform_stamped.transform.translation.x = pose_msg.pose.position.x;transform_stamped.transform.translation.y = pose_msg.pose.position.y;transform_stamped.transform.translation.z = pose_msg.pose.position.z;tf2::Quaternion tf_quaternion;tf2::fromMsg(pose_msg.pose.orientation, tf_quaternion);transform_stamped.transform.rotation.x = tf_quaternion.x();transform_stamped.transform.rotation.y = tf_quaternion.y();transform_stamped.transform.rotation.z = tf_quaternion.z();transform_stamped.transform.rotation.w = tf_quaternion.w();tf2_broadcaster_.sendTransform(transform_stamped);
}int main(int argc, char **argv)
{ros::init(argc, argv, "ndt_localizer");ros::NodeHandle nh;ros::NodeHandle private_nh("~");NdtLocalizer ndt_localizer(nh, private_nh);ros::spin();return 0;
}

二.ndt算法原理
1.通过将点云分成多个体素格子
2.计算格子内的协方差与均值，得到概率模型

3.变换输入点云，与目标点云配准得到坐标变换关系

4.根据正态分布参数计算每个转换点落在对应cell中的概率

5.NDT配准得分（score）：计算对应点落在对应网格cell中的概率之和

6.根据牛顿优化算法对目标函数score进行优化，即寻找变换参数p使得 score的值最大。优化的关键步骤是要计算目标函数的梯度和Hessian矩阵：

7.跳转到第3步重复执行，直到满足收敛条件结束

运行效果

参考：https://blog.csdn.net/weixin_41469272/article/details/105622447
代码：git clone https://github.com/AbangLZU/ndt_localizer.git