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

news2025/7/3 11:17:58

一.代码实现流程
二.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);
    //从文件中读取pcd
    auto 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: translation
    Eigen::AngleAxisf rot_x_m2w(tf_roll_, Eigen::Vector3f::UnitX());  // rot: rotation
    Eigen::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.0

ros::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 filter
    pcl::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的键 并将值设置为Initializing
  key_value_stdmap_["state"] = "Initializing";
  // 初始参数读取
  init_params();

  // Publishers
  sensor_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()
{
  // 100hz
  ros::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;
      // 键state
      key_value_msg.key = key_value.first;
     //值 Initializing
      key_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 map
  if (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_frame
    geometry_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_frame
    geometry_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
  // detach
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  // 执行ndt计算
  ndt_new.align(*output_cloud, Eigen::Matrix4f::Identity());

  // swap
  ndt_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 Map
  std::lock_guard<std::mutex> lock(ndt_map_mtx_);
// lidar frame
  const 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 sensor
  geometry_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;
  }
  // align
  Eigen::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_trans
    pre_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";
  // 执行ndt
  ndt_.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;
  
  // publish
  geometry_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 cloud
  pcl::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(){
// 底盘的id
  private_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相同 则坐标变换为0
  if (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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/805493.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

Cisco 路由器配置管理

Cisco 路由器配置管理

大多数网络中断的最常见原因是错误的配置更改。对网络设备配置的每一次更改都伴随着造成网络中断、安全问题甚至性能下降的风险。计划外更改使网络容易受到意外中断的影响。 Network Configuration Manager 网络更改和配置管理 &#xff08;NCCM&#xff09;解决方案&#xff…
阅读更多...
操作符（超详解）

操作符（超详解）

操作符详解 1. 操作符分类2. 算术操作符3. 移位操作符3.1 左移操作符3.2 右移操作符 4. 位操作符5. 赋值操作符6. 单目操作符6.1 单目操作符介绍6.2 sizeof和数组 7. 关系操作符8. 逻辑操作符9. 条件操作符10.逗号表达式11.下标引用&#xff0c;函数调用和结构成员12.表达式求值…
阅读更多...
【TypeScript】接口类型 Interfaces 的使用理解

【TypeScript】接口类型 Interfaces 的使用理解

导语&#xff1a; 什么是 类型接口&#xff1f; 在面向对象语言中&#xff0c;接口&#xff08;Interfaces&#xff09;是一个很重要的概念&#xff0c;它是对行为的抽象&#xff0c;而具体如何行动需要由类&#xff08;classes&#xff09;去实现&#xff08;implement&#x…
阅读更多...
基于IP地址的证书实现https

基于IP地址的证书实现https

基于IP地址实现传递数据的&#xff0c;默认的HTTP很容易被不法分子劫持数据&#xff0c;网络防洪是当下的互联网为确保安全&#xff0c;要用HTTPS协议更为妥当。 使用IP地址申请证书的主要条件&#xff0c;必须在申请认证过程&#xff0c;开放IP地址外网可以访问&#xff0c;包…
阅读更多...
web漏洞-java安全（41）

web漏洞-java安全（41）

这个重点是讲关于java的代码审计&#xff0c;看这些漏洞是怎么在java代码里面产生的。 #Javaweb 代码分析-目录遍历安全问题 这个漏洞原因前面文章有&#xff0c;这次我们看看这个漏洞如何在代码中产生的&#xff0c;打开靶场 解题思路就是通过文件上传&#xff0c;上传文件…
阅读更多...
文件按关键字分组-切割-染色-写入excel

文件按关键字分组-切割-染色-写入excel

1. 背景 针对下面的文件data.csv&#xff0c;首先根据fid进行排序&#xff0c;然后分组&#xff0c;使相同fid的记录放到同一个excel文件中&#xff0c;并对每列重复的数据元素染上红色。 fid,user_id -1000078398032092029,230410010036537520 -1000078398032092029,23042301…
阅读更多...
聊聊嵌入式编程中的主函数和循环函数

聊聊嵌入式编程中的主函数和循环函数

a在嵌入式编程中&#xff0c;主函数&#xff08;main函数&#xff09;是程序的入口点&#xff0c;也是程序的起点。在主函数中&#xff0c;我们可以进行一些初始化操作和设置&#xff0c;然后进入一个主循环&#xff0c;执行特定的任务或处理。 主函数在程序开始时被调用&…
阅读更多...
链路模型的分析

链路模型的分析

链路模型 目录概述需求&#xff1a; 设计思路实现思路分析1.解说 拓展实现性能参数测试&#xff1a; 参考资料和推荐阅读 Survive by day and develop by night. talk for import biz , show your perfect code,full busy&#xff0c;skip hardness,make a better result,wait …
阅读更多...
计算机组成原理（2）- 浮点数的存储

计算机组成原理（2）- 浮点数的存储

1、浮点数的表示方法 假设有以下小数&#xff0c;它表示的十进制数是多少呢&#xff1f; 00000000 00000000 00000000 1010.10101*2^3 1*2^1 1*2^-1 1*2^-3 10.625 1010.1010可以用科学计数法来表示为1.0101010 * 2^3。关于科学计数法再举个例子0.10101用科学计数法表示…
阅读更多...
如何以管理员权限安装某个msi

如何以管理员权限安装某个msi

介绍 如何以管理员权限安装某个msi 方法 要以管理员权限在控制台中安装一个 MSI 文件&#xff0c;你可以按照以下步骤操作&#xff1a; 打开命令提示符&#xff08;或 PowerShell&#xff09;&#xff1a;按下 Win R 键&#xff0c;在运行窗口中输入 “cmd”&#xff08;或 …
阅读更多...
Redis 笔记，基本数据类型、持久化、主从、集群等等问题

Redis 笔记，基本数据类型、持久化、主从、集群等等问题

标题 &#x1f600;&#x1f600;&#x1f600;创作不易&#xff0c;各位看官点赞收藏. 文章目录 标题Redis 基础笔记1、安装及环境搭建2、Redis 数据类型2.1、String2.2、List2.3、Hash2.4、Set2.5、Zset2.6、BitMap2.7、HyperLogLog2.8、Geospatial2.9、Stream 3、Redis 持久…
阅读更多...
数据结构--算法的时间复杂度和空间复杂度

数据结构--算法的时间复杂度和空间复杂度

文章目录 算法效率时间复杂度时间复杂度的概念大O的渐进表示法计算实例 时间复杂度实例 常见复杂度对比例题 算法效率 算法效率是指算法在计算机上运行时所消耗的时间和资源。这是衡量算法执行速度和资源利用情况的重要指标。 例子&#xff1a; long long Fib(int N) {if(N …
阅读更多...
如何建立Docker私有仓库?

如何建立Docker私有仓库?

文章目录 docker私有仓库harborHarbor仓库部署Harbor仓库使用 docker私有仓库 Docker 私有仓库是一个用于存储和管理 Docker 镜像的私有存储库。它允许你在内部网络中创建和管理 Docker 镜像&#xff0c;并提供了更好的安全性和控制&#xff0c;因为你可以完全控制谁能够访问和…
阅读更多...
el-table 设置行背景颜色 鼠标移入高亮问题处理

el-table 设置行背景颜色 鼠标移入高亮问题处理

一、 设置行背景颜色 1. 需求描述 后端返回表格数据&#xff0c;有特定行数需要用颜色标识。类似于以下需求&#xff1a; 2. 解决方式 方式区别:row-class-name“tableRowClassName”已返回类名的形式设置样式&#xff0c;代码整洁&#xff0c;但是会鼠标高亮&#xff0c…
阅读更多...
【IDEA】idea不自动生成target

【IDEA】idea不自动生成target

文章目录 1. 不生成target2. 仅部分文件不生成target2.1. 一般原因就是资源没有设置2.2. 配置编译src/main/java文件夹下的资源文件2.3. 清理缓存&#xff08;王炸&#xff09; 3. 参考资料 本文描述idea不生成target的几种情况以及处理方法 1. 不生成target 像下图这样根本就…
阅读更多...
JavaSwing+MySQL的在线考试系统

JavaSwing+MySQL的在线考试系统

点击以下链接获取源码&#xff1a; https://download.csdn.net/download/qq_64505944/88114390?spm1001.2014.3001.5503 JDK1.8 MySQL5.7 功能&#xff1a;开始做题&#xff0c;上一题&#xff0c;下一题&#xff0c;提交&#xff0c;每题都有时间限制
阅读更多...
【SpringⅢ】Spring 的生命周期

【SpringⅢ】Spring 的生命周期

目录 &#x1f96a;1 Bean 的作用域 &#x1f969;1.1 singleton&#xff1a;单例模式 &#x1f359;1.2 prototype&#xff1a;原型模式 &#x1f371;1.3 Bean 的其他作用域 &#x1f35c;2 Spring 生命周期(执行流程) &#x1f958;2.1 启动容器 &#x1f372; 2.2 读…
阅读更多...
【代理模式】了解篇：静态代理 动态代理~

【代理模式】了解篇：静态代理 动态代理~

目录 1、什么是代理模式&#xff1f; 2、静态代理 3、动态代理 3.1 JDK动态代理类 3.2 CGLIB动态代理类 4、JDK动态代理和CGLIB动态代理的区别&#xff1f; 1、什么是代理模式&#xff1f; 定义&#xff1a; 代理模式就是为其他对象提供一种代理以控制这个对象的访问。在某…
阅读更多...
图像 检测 - FCOS: Fully Convolutional One-Stage Object Detection (ICCV 2019)

图像 检测 - FCOS: Fully Convolutional One-Stage Object Detection (ICCV 2019)

FCOS: Fully Convolutional One-Stage Object Detection - 全卷积一阶段目标检测&#xff08;ICCV 2019&#xff09; 摘要1. 引言2. 相关工作3. 我们的方法3.1 全卷积一阶目标检测器3.2 FCOS的FPN多级预测3.3 FCOS中心度 4. 实验4.1 消融研究4.1.1 FPN多级预测4.1.2 有无中心度…
阅读更多...
python学习时与chatgpt4对话的一些感悟

python学习时与chatgpt4对话的一些感悟

今天学SCENIC教程&#xff0c;看到里面有一句不是很懂 If you run this from a python script instead of a Jupyter notebook, please enclose the code in a if __name__ __main__: construct. 现在把和chatgpt4问答的内容发上来&#xff0c;确实是很厉害 没有太看懂&…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023