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);
    //从文件中读取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

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

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

【深度学习笔记】动量梯度下降法

【深度学习笔记】动量梯度下降法

本专栏是网易云课堂人工智能课程《神经网络与深度学习》的学习笔记&#xff0c;视频由网易云课堂与 deeplearning.ai 联合出品&#xff0c;主讲人是吴恩达 Andrew Ng 教授。感兴趣的网友可以观看网易云课堂的视频进行深入学习&#xff0c;视频的链接如下&#xff1a; 神经网络和…
阅读更多...
Python开发之手动实现一维线性插值

Python开发之手动实现一维线性插值

Python开发之手动实现一维线性插值 1.线性插值法介绍2.手动实现线性插值3.案例一&#xff1a;手动实现线性插值4.使用pandas的插值方法实现要求(推荐)5.案例二&#xff1a;对一组数据进行线性插值和SG滤波处理 前言&#xff1a;主要介绍手动实现一维线性插值以及pandas里面的in…
阅读更多...
【Docker】容器的数据卷

【Docker】容器的数据卷

目录 一、数据卷的概念与作用 二、数据卷的配置 三、数据卷容器的配置 一、数据卷的概念与作用 在了解什么是数据卷之前我们先来思考以下这些问题&#xff1a; 1.如果我们一个容器在使用后被删除&#xff0c;那么他里面的数据是否也会丢失呢&#xff1f;比如容器内的MySQL的…
阅读更多...
2023年的深度学习入门指南(21) - 百川大模型

2023年的深度学习入门指南(21) - 百川大模型

2023年的深度学习入门指南(21) - 百川大模型 前面我们用了三节的篇幅介绍了目前最强大的开源模型LLaMA2。这一节我们说一说国产大模型的一个代表&#xff0c;百川大模型。 使用百川大模型 第一步我们先把百川用起来&#xff0c;然后再研究如何训练和其原理如何。 百川的使用…
阅读更多...
Mybatis使用collection映射一对多查询分页问题

Mybatis使用collection映射一对多查询分页问题

场景&#xff1a;页面展示列表&#xff0c;需要查询多的字段&#xff0c;和一的字段。并且还要分页。 这时候直接想到的是手写sql。 /*** 标签*/private List<BasicResidentTags> tags;Data TableName("basic_resident_tags") public class BasicResidentTag…
阅读更多...
vue3 - element-plus 上传各种 word pdf 文件、图片视频并上传到服务器功能效果，示例代码开箱即用。

vue3 - element-plus 上传各种 word pdf 文件、图片视频并上传到服务器功能效果，示例代码开箱即用。

效果图 在 vue3 项目中，使用 element plus 组件库的 el-upload 上传组件，进行文件、图片图像的上传功能示例。 完整代码 可直接复制，再改个接口地址。 在这里上传图片和文件是分成
阅读更多...
新产品：Stimulsoft Forms 2023.3.1 Crack

新产品：Stimulsoft Forms 2023.3.1 Crack

Stimulsoft Forms 是一个用于交互式收集和处理用户数据的组件。表单工具可以轻松集成到您的项目或应用程序中&#xff0c;具有直观且用户友好的界面&#xff0c;并允许您创建丰富的表单模板。Stimulsoft Forms 是应用程序中与用户交互的新水平 什么是 Stimulsoft Forms&#xf…
阅读更多...
华为数通HCIP-EVPN基础

华为数通HCIP-EVPN基础

MP-BGP MP-BGP&#xff08;Multiprotocol Extensions for BGP-4&#xff09;在RFC4760中被定义&#xff0c;用于实现BGP-4的扩展以允许BGP携带多种网络层协议&#xff08;例如IPv6、L3VPN、EVPN等&#xff09;。这种扩展有很好的后向兼容性&#xff0c;即一个支持MP-BGP的路由…
阅读更多...
cad丢失mfc140u.dll怎么办？找不到mfc140u.dll的解决方法

cad丢失mfc140u.dll怎么办？找不到mfc140u.dll的解决方法

第一&#xff1a;mfc140u.dll有什么用途&#xff1f; mfc140u.dll是Windows操作系统中的一个动态链接库文件&#xff0c;它是Microsoft Foundation Class (MFC)库的一部分。MFC是 C中的一个框架&#xff0c;用于构建Windows应用程序的用户界面和功能。mfc140u.dll包含了MFC库中…
阅读更多...
Flowable-中间事件-信号中间抛出事件

Flowable-中间事件-信号中间抛出事件

定义 当流程执行到达信号抛出事件时&#xff0c;流程引擎会直接抛出信号&#xff0c;其他引用了与其相同的信号捕获 事件会被触发&#xff0c;信号发出后事件结束&#xff0c;流程沿后继路线继续执行。其抛出的信号可以被信号开始事 件&#xff08;Signal Start Event&#xf…
阅读更多...
从官网认识 JDK，JRE，JVM 三者的关系

从官网认识 JDK，JRE，JVM 三者的关系

点击下方关注我&#xff0c;然后右上角点击...“设为星标”&#xff0c;就能第一时间收到更新推送啦~~~ JVM 是一些大厂面试必问点&#xff0c;要想解决 OOM、性能调优方面的问题&#xff0c;掌握 JVM 知识必不可少&#xff0c;从今天开始&#xff0c;将为大家介绍 JVM 的常用知…
阅读更多...
css实现有缺口的border

css实现有缺口的border

css实现有缺口的border 1.问题回溯2.css实现有缺口的border 1.问题回溯 通常会有那种两个div都有border重叠在一起就会有种加粗的效果。 div1,div2,div3都有个1px的border&#xff0c;箭头标记的地方是没有处理解决的&#xff0c;很明显看着是有加粗效果的。其实这种感觉把di…
阅读更多...
【Vscode】远程内存占用大

【Vscode】远程内存占用大

查看远程服务器上的扩展 依次删除&#xff0c;重新连接后观察内存占用 此扩展占用较高&#xff0c;约2G&#xff08;前后端项目&#xff0c;依赖较多导致&#xff09;
阅读更多...
【node.js】04-模块化

【node.js】04-模块化

目录 一、什么是模块化 二、node.js中的模块化 1. node.js中模块的分类 2. 加载模块 3. node.js 中的模块作用域 4. 向外共享模块作用域中的成员 4.1 module对象 4.2 module.exports 对象 4.3 exports对象 5. node.js 中的模块化规范 一、什么是模块化 模块化是指解…
阅读更多...
为Android构建现代应用——主体结构

为Android构建现代应用——主体结构

创建Screents和ViewModels 在前面的章节中&#xff0c;我们已经分析了OrderNow项目的理论概念和我们将赋予的组织。 在本章中&#xff0c;我们将开始实现初始结构和模板&#xff0c;这将联接每一个应用程序的部分。 首先将添加以下带有各自视图模型的主屏幕&#xff1a; •…
阅读更多...
springboot 之以enable开头的注解

springboot 之以enable开头的注解

Spring​ 有很多 Enable 开头的注解&#xff0c;平时在使用的时候也没有注意过为什么会有这些注解 Enable 注解 首先我们先看一下有哪些常用的 Enable 开头的注解&#xff0c;以及都是干什么用的。 EnableRetry​&#xff1a;开启Spring 的重试功能&#xff1b; EnableSch…
阅读更多...
【环境配置】Windows下WSL将ubuntu挪位置-系统盘清理

【环境配置】Windows下WSL将ubuntu挪位置-系统盘清理

问题–垃圾太多&#xff0c;系统盘空间占用太大 最近 C 盘空间暴涨&#xff0c;用工具 WinDirStat-强烈推荐的工具 查看发现 WSL 子系统占用了6个多 G 的空间&#xff0c;遂想办法挪个位置&#xff1b; 【关键字】将 Windows 里的子系统挪到非系统盘 D 盘&#xff1b; 解决 打…
阅读更多...
Python numpy库的应用、matplotlib绘图、opencv的应用

Python numpy库的应用、matplotlib绘图、opencv的应用

numpy import numpy as npl1 [1, 2, 3, 4, 5]# array():将列表同构成一个numpy的数组 l2 np.array(l1) print(type(l2)) print(l2) # ndim : 返回数组的轴数&#xff08;维度数&#xff09; # shape&#xff1a;返回数组的形状&#xff0c;用元组表示&#xff1b;元组的元素…
阅读更多...
wangeditor编辑器配置

wangeditor编辑器配置

vue项目中使用编辑器&#xff0c;轻量&#xff0c;操作栏选取自己需要的 官网地址&#xff1a;用于 Vue React | wangEditor 使用在vue项目中引入 npm install wangeditor/editor --savenpm install wangeditor/editor-for-vue --save 封装成组件使用 <template>&…
阅读更多...
处理多维特征的输入

处理多维特征的输入

数据的每一列称为&#xff1a;特征/字段 x的数据变为8列&#xff0c;维数8 step one:构建数据集 x_data;y_data&#xff1a;创建两个Tensor step two:定义模型 step three:构造损失和优化器 step four:训练 else 激活函数&#xff1a; 代码更改部分&#xff1a; 转->大佬笔…
阅读更多...
最新文章

Copyright @ 2022~2023