南城网站建设公司,贵阳市有翻译资质的公司,网络营销类型有哪些,app程序开发用什么编程一.代码实现流程 二.ndt算法原理
一.该算法定位有三个进程文件 1.map_loader.cpp用于点云地图的读取#xff0c;从文件中读取点云后对这个点云地图进行旋转平移后发布点云地图到ros #include map_loader.hMapLoader::MapLoader(ros::NodeHandle nh){std::st…一.代码实现流程 二.ndt算法原理
一.该算法定位有三个进程文件 1.map_loader.cpp用于点云地图的读取从文件中读取点云后对这个点云地图进行旋转平移后发布点云地图到ros #include map_loader.hMapLoader::MapLoader(ros::NodeHandle nh){std::string pcd_file_path, map_topic;// 点云地图读取路径nh.paramstd::string(pcd_path, pcd_file_path, );// 点云地图话题名nh.paramstd::string(map_topic, map_topic, point_map);//初始位置的相对位置坐标变换 参数读取init_tf_params(nh);//要发布的map话题pc_map_pub_ nh.advertisesensor_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.paramfloat(x, tf_x_, 0.0);nh.paramfloat(y, tf_y_, 0.0);nh.paramfloat(z, tf_z_, 0.0);nh.paramfloat(roll, tf_roll_, 0.0);nh.paramfloat(pitch, tf_pitch_, 0.0);nh.paramfloat(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::PointCloudpcl::PointXYZ::Ptr in_pc(new pcl::PointCloudpcl::PointXYZ);// 传感器点云格式转换为pcl点云格式pcl::fromROSMsg(in, *in_pc);pcl::PointCloudpcl::PointXYZ::Ptr transformed_pc_ptr(new pcl::PointCloudpcl::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::PointCloudpcl::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::PointCloudpcl::PointXYZ removePointsByRange(pcl::PointCloudpcl::PointXYZ scan, double min_range, double max_range)
{pcl::PointCloudpcl::PointXYZ narrowed_scan;narrowed_scan.header scan.header;if( min_rangemax_range ) {ROS_ERROR_ONCE(min_rangemax_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::PointCloudpcl::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::PointCloudpcl::PointXYZ scan;// 传感器点云转换为pcl点云pcl::fromROSMsg(*input, scan);// 移除距离原点中心过远的点scan removePointsByRange(scan, 0, MAX_MEASUREMENT_RANGE);pcl::PointCloudpcl::PointXYZ::Ptr scan_ptr(new pcl::PointCloudpcl::PointXYZ(scan));pcl::PointCloudpcl::PointXYZ::Ptr filtered_scan_ptr(new pcl::PointCloudpcl::PointXYZ());sensor_msgs::PointCloud2 filtered_msg;//体素滤波器过滤点云if (voxel_leaf_size 0.1){// Downsampling the velodyne scan using VoxelGrid filterpcl::VoxelGridpcl::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.paramdouble(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.advertisesensor_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.hNdtLocalizer::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_.advertisesensor_msgs::PointCloud2(points_aligned, 10);ndt_pose_pub_ nh_.advertisegeometry_msgs::PoseStamped(ndt_pose, 10);exe_time_pub_ nh_.advertisestd_msgs::Float32(exe_time_ms, 10);transform_probability_pub_ nh_.advertisestd_msgs::Float32(transform_probability, 10);iteration_num_pub_ nh_.advertisestd_msgs::Float32(iteration_num, 10);diagnostics_pub_ nh_.advertisediagnostic_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 initinit_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::NormalDistributionsTransformpcl::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::PointCloudpcl::PointXYZ::Ptr map_points_ptr(new pcl::PointCloudpcl::PointXYZ);pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);// 设置目标点云ndt_new.setInputTarget(map_points_ptr);// create Thread// detachpcl::PointCloudpcl::PointXYZ::Ptr output_cloud(new pcl::PointCloudpcl::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_guardstd::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_ptrpcl::PointCloudpcl::PointXYZ sensor_points_sensorTF_ptr(new pcl::PointCloudpcl::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().castfloat();boost::shared_ptrpcl::PointCloudpcl::PointXYZ sensor_points_baselinkTF_ptr(new pcl::PointCloudpcl::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().castfloat();//将重定位的发布赋值给pre_transpre_trans initial_pose_matrix;// 只执行一次的目的init_pose true;}else{//ndt初始计算位置初值 当前变换矩阵*当前与上一次之间的变换矩阵initial_pose_matrix pre_trans * delta_trans;}pcl::PointCloudpcl::PointXYZ::Ptr output_cloud(new pcl::PointCloudpcl::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_caststd::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.castdouble();//定位结果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_caststd::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.block3, 1(0, 3);std::coutdelta x: delta_translation(0) y: delta_translation(1) z: delta_translation(2)std::endl;
// 旋转向量Eigen::Matrix3f delta_rotation_matrix delta_trans.block3, 3(0, 0);Eigen::Vector3f delta_euler delta_rotation_matrix.eulerAngles(2,1,0);std::coutdelta 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_dsabase_frame_std::endl;// publish aligned point cloudpcl::PointCloudpcl::PointXYZ::Ptr sensor_points_mapTF_ptr(new pcl::PointCloudpcl::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
