日本无码免费高清在线|成人日本在线观看高清|A级片免费视频操逼欧美|全裸美女搞黄色大片网站|免费成人a片视频|久久无码福利成人激情久久|国产视频一二国产在线v|av女主播在线观看|五月激情影音先锋|亚洲一区天堂av

  • 手機(jī)站
  • 小程序

    汽車測試網(wǎng)

  • 公眾號

    • 汽車測試網(wǎng)

    • 在線課堂

    • 電車測試

基于NDT的自動駕駛高精度定位和ROS項目實戰(zhàn)

2020-08-11 17:56:15·  來源：焉知自動駕駛、hdmap  
　
對于高級自動駕駛系統(tǒng)而言，定位模塊通常會融合IMU、輪速計（車輛底盤）以及激光雷達(dá)odometry多種測量，使用濾波算法（EKF、UKF等）以獲得平滑、厘米級別的絕對
對于高級自動駕駛系統(tǒng)而言，定位模塊通常會融合IMU、輪速計（車輛底盤）以及激光雷達(dá)odometry多種測量，使用濾波算法（EKF、UKF等）以獲得平滑、厘米級別的絕對定位，其中基于高精度點云地圖和激光雷達(dá)的配準(zhǔn)定位（Lidar Odometry）因其精度高、可靠性好，在整個融合定位中通常占很大的權(quán)重，是自動駕駛定位系統(tǒng)中相對可靠的“絕對定位”數(shù)據(jù)來源，本文我們學(xué)習(xí)如何使用NDT配準(zhǔn)實現(xiàn)自動駕駛汽車的高精度定位，并且結(jié)合前面文章中使用SC-LEGO-LOAM生產(chǎn)的點云地圖，實踐單純NDT算法的自動駕駛高精度定位ROS項目，完成本文，你將能實現(xiàn)如下圖所示的點云配準(zhǔn)定位：
 
前言
在前面的文章中（無人駕駛汽車系統(tǒng)入門系列第13篇、自動駕駛系統(tǒng)進(jìn)階與項目實戰(zhàn)第2篇）有詳細(xì)介紹NDT算法和點云配準(zhǔn)的相關(guān)知識，但是一直沒有完整地介紹如何使用點云地圖和NDT配準(zhǔn)完成自動駕駛汽車定位，本文將給出完整介紹和一個干凈的NDT定位實現(xiàn)。
NDT定位是Autoware自動駕駛開源項目的核心定位算法，但是Autoware 1.x各個模塊耦合性很強(qiáng)，如果單純是為了學(xué)習(xí)激光雷達(dá)配準(zhǔn)定位，你可能需要編譯整個Autoware項目才能測試實驗NDT定位，另外，Autoware 1.x中實現(xiàn)的NDT算法采用面向過程編程，代碼相當(dāng)難讀，因此，本文中，我們將基于Autoware 1.x中的NDT配準(zhǔn)定位思路，實現(xiàn)一個相對干凈、清晰的NDT ROS項目，整個項目僅依賴ROS，面向?qū)ο缶幊蹋绻椖繉δ阌袔椭?，就來個小心心吧！
點云地圖準(zhǔn)備
自動駕駛汽車的激光雷達(dá)定位通常依賴于提前離線構(gòu)建好的高精度點云地圖，之所以這么做原因有以下幾個方面：
  • L4級別以上自動駕駛系統(tǒng)對定位精度和穩(wěn)定性要求很高，絕對誤差需要控制在20cm以內(nèi)
  • 純SLAM目前來說無法達(dá)到自動駕駛對于定位精度、可靠性的要求，即我們現(xiàn)在的研究很難實現(xiàn)自動駕駛車的在線制圖和定位（問題包括閉環(huán)優(yōu)化，全局優(yōu)化，誤差累計修正等等）
  • 高精度地圖制造商的完整生產(chǎn)流程需要較大的算力和人工，他們能夠生產(chǎn)非常理想的點云地圖和語義地圖，但是需要離線生產(chǎn)（時間和人力）；
  • 利用高精度地圖可以相對簡單地實現(xiàn)激光雷達(dá)定位，在融合了IMU和輪速計以后這類定位方法的精度和可靠性基本滿足自動駕駛汽車定位的需求。
所以綜合以上的客觀原因，目前的L4和大部分L3自動駕駛系統(tǒng)定位模塊仍然是以事先構(gòu)建的高精度地圖為基礎(chǔ)進(jìn)行的配準(zhǔn)定位，這個配準(zhǔn)使用的傳感器，少數(shù)廠商使用的是camera(如mobileeye)，絕大多數(shù)廠商目前仍然采用的是激光雷達(dá)配準(zhǔn)思路。點云地圖就是激光雷達(dá)配準(zhǔn)所需事先構(gòu)建的“用來定位的地圖”。
在上一篇文章（自動駕駛系統(tǒng)進(jìn)階與項目實戰(zhàn)五）中，我們使用SC-LEGO-LOAM方法構(gòu)建了一張相對較大的點云圖，并且使用Scan Context方法對點云圖進(jìn)行了閉環(huán)檢測和姿態(tài)圖優(yōu)化，本文我們將直接使用上一篇文章中生產(chǎn)的點云圖作為定位地圖。
將上一篇文章中生成的pcd文件(偷懶的同學(xué)也可以直接通過文末鏈接下載該pcd文件)拷貝到本項目（倉庫地址見文末鏈接）的map目錄下，命名為kaist02.pcd，項目中的map_loader節(jié)點主要用于載入地圖：
MapLoader::MapLoader(ros::NodeHandle &nh){
std::string pcd_file_path, map_topic;
nh.param("pcd_path", pcd_file_path, "");
nh.param("map_topic", map_topic, "point_map");
init_tf_params(nh);
pc_map_pub_ = nh.advertise(map_topic, 10, true);
file_list_.push_back(pcd_file_path);
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);
}
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
構(gòu)造函數(shù)中讀取pcd文件的路徑和map topic，并且初始化map的變換參數(shù)（如果不需要對map進(jìn)行變換，則數(shù)值都設(shè)置為０），在map_load.launch中設(shè)置這些參數(shù)：
 
 
 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
函數(shù)CreatePcd()用于加載pcd，TransformMap()用于平移和旋轉(zhuǎn)地圖，我們使用Eigen和pcl::transformPointCloud()實現(xiàn)點云的變換：
sensor_msgs::PointCloud2 MapLoader::TransformMap(sensor_msgs::PointCloud2 & in){
pcl::PointCloud ::Ptr in_pc(new pcl::PointCloud );
pcl::fromROSMsg(in, *in_pc);
pcl::PointCloud ::Ptr transformed_pc_ptr(new pcl::PointCloud );
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;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
輸入點云降采樣
NDT算法優(yōu)化的目標(biāo)函數(shù)主要是輸入點云和目標(biāo)點云概率分布的相似性，這種配準(zhǔn)算法的計算復(fù)雜度和兩個要素正相關(guān)：
  • 輸入點云的點的密度
  • 初始姿態(tài)估計的偏差
輸入點云點越密集，NDT配準(zhǔn)所需的計算復(fù)雜度就越大；初始姿態(tài)估計越差（越偏離真實的姿態(tài)），相應(yīng)的計算復(fù)雜度也越大，初始姿態(tài)過差的話NDT甚至無法收斂。自動駕駛激光雷達(dá)定位對實時性有較高的要求，點云配準(zhǔn)所用的時間顯然越少越好，所以我們可以通過降采樣輸入點云以提高NDT配準(zhǔn)的速度，本文中我們采用VoxelGrid降采樣方法降低輸入點云的密度，代碼在項目的 voxel_grid_filter.cpp中，主要代碼如下：
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
pcl::PointCloud scan;
pcl::fromROSMsg(*input, scan);
if(measurement_range != MAX_MEASUREMENT_RANGE){
scan = removePointsByRange(scan, 0, measurement_range);
}
pcl::PointCloud ::Ptr scan_ptr(new pcl::PointCloud (scan));
pcl::PointCloud ::Ptr filtered_scan_ptr(new pcl::PointCloud ());
sensor_msgs::PointCloud2 filtered_msg;
// if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)
if (voxel_leaf_size >= 0.1)
{
// Downsampling the velodyne scan using VoxelGrid filter
pcl::VoxelGrid 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);
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
在得到點云以后，首先對點云進(jìn)行截取，我們只保留 MAX_MEASUREMENT_RANGE 距離以內(nèi)的點用于定位（本文中MAX_MEASUREMENT_RANGE = 120 米），VoxelGrid降采樣的主要參數(shù)就是voxel_leaf_size，該參數(shù)設(shè)定了降采樣選取的立方體的邊長（單位為米），在一個這樣的立方體內(nèi)只保留１個點，可以在 points_downsample.launch文件中配置該參數(shù)：
  • 1
如上所示，本文采用了３米的leaf size，這個參數(shù)可以根據(jù)你實際使用的激光雷達(dá)點的密度決定，雖然我們追求配準(zhǔn)的實時性，但同時我們也不希望犧牲太多定位的精度，所以對輸入點云降采樣的度需要平衡實時性和定位精度，根據(jù)經(jīng)驗，如果你猜用的是16線的激光雷達(dá)，那么降采樣的leaf size控制在１-２m較為合適，當(dāng)采用的激光雷達(dá)為32線及以上,可以將leaf size設(shè)置為2-3m。
降采樣后的點云將被輸出至 /filtered_points 話題，以供后續(xù)的NDT配準(zhǔn)定位使用。
使用NDT為自動駕駛車提供高精度定位
NDT定位的邏輯我主要實現(xiàn)在ndt.cpp中，下面我們仔細(xì)分析一下這一部分的代碼。
初始姿態(tài)獲取
一切使用預(yù)先構(gòu)建的地圖進(jìn)行配準(zhǔn)定位的方法都需要提供初始姿態(tài)，在工業(yè)界的實踐中，這一初始姿態(tài)通常是通過gnss獲得，本文中我們簡化這一步，在Rviz中手動指定初始姿態(tài)，Rviz中設(shè)定的初始姿態(tài)通常會被默認(rèn)發(fā)送至/initialpose topic上，在NdtLocalizer構(gòu)造函數(shù)中，寫一個subscriber監(jiān)聽該topic：
initial_pose_sub_ = nh_.subscribe("initialpose", 100, &NdtLocalizer::callback_init_pose, this);
  • 1
當(dāng)有初始姿態(tài)（geometry_msgs::PoseWithCovarianceStamped）傳來的時候，執(zhí)行的是以下回調(diào)：
void NdtLocalizer::callback_init_pose(
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & initial_pose_msg_ptr)
{
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);
// mapTF_initial_pose_msg_ptr->header.stamp = initial_pose_msg_ptr->header.stamp;
initial_pose_cov_msg_ = *mapTF_initial_pose_msg_ptr;
}
// if click the initpose again, re init！
init_pose = false;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
在NDT配準(zhǔn)中，我們主要關(guān)注四個坐標(biāo)系間的變化，分別是：
  • 世界坐標(biāo)系（frame_id = world）
  • 地圖坐標(biāo)系（frame_id = map）
  • 車輛基礎(chǔ)坐標(biāo)系（frame_id = base_link）
  • 激光雷達(dá)坐標(biāo)系（本文中frame_id = ouster，根據(jù)你使用的激光雷達(dá)不同，frame id也會不一樣）
在本項目中，我們使用static_tf.launch發(fā)布world 到 map以及ouster到base_link這兩個固定變換，如下：
  • 1
  • 2
注：在這里我們假定map和world為同一坐標(biāo)系以簡化問題，在具體的自動駕駛系統(tǒng)研發(fā)中，你需要根據(jù)WGS84坐標(biāo)系下的經(jīng)緯度配合通用橫軸墨卡托投影（Universal Transverse Mercator，UTM）以獲得當(dāng)前Map到世界坐標(biāo)系的平移關(guān)系以及東北天（East North Up, ENU）坐標(biāo)系下的旋轉(zhuǎn)量。
localizer_to_base_link即激光雷達(dá)到base link的變換關(guān)系，是激光雷達(dá)的外參之一，也是一個靜態(tài)變換。
回到初始姿態(tài)獲取，得到Rviz上手動指定的初始姿態(tài)后，首先對坐標(biāo)系進(jìn)行統(tǒng)一，如果該pose是在地圖坐標(biāo)系，那么保存用于后續(xù)使用，如果是其他坐標(biāo)系，則先將該pose轉(zhuǎn)換至地圖坐標(biāo)系，通過函數(shù) get_transform 獲取變換關(guān)系，該函數(shù)定義如下：
bool NdtLocalizer::get_transform(
const std::string & target_frame, const std::string & source_frame,
const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr)
{
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;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
在獲得到map的tf以后，直接通過tf2::doTransform將初始姿態(tài)轉(zhuǎn)換到地圖坐標(biāo)系下。
初始化地圖
NDT配準(zhǔn)中的目標(biāo)點云就是我們事先使用SC-LEGO-LOAM構(gòu)建的點云地圖了，編寫Subscriber監(jiān)聽mapLoader節(jié)點發(fā)來的點云地圖massage，執(zhí)行如下回調(diào)：
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
ndt_new.setTransformationEpsilon(trans_epsilon);
ndt_new.setStepSize(step_size);
ndt_new.setResolution(resolution);
ndt_new.setMaximumIterations(max_iterations);
pcl::PointCloud ::Ptr map_points_ptr(new pcl::PointCloud );
pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);
ndt_new.setInputTarget(map_points_ptr);
// create Thread
// detach
pcl::PointCloud ::Ptr output_cloud(new pcl::PointCloud );
ndt_new.align(*output_cloud, Eigen::Matrix4f::Identity());
// swap
ndt_map_mtx_.lock();
ndt_ = ndt_new;
ndt_map_mtx_.unlock();
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
其中最關(guān)鍵的一步就是ndt_new.setInputTarget(map_points_ptr);在獲取點云地圖以后，設(shè)置ndt的目標(biāo)點云為該點云地圖，同時也是這里NDT算法的基本參數(shù)：
ndt_new.setTransformationEpsilon(trans_epsilon);搜索的最小變化ndt_new.setStepSize(step_size);搜索的步長ndt_new.setResolution(resolution);目標(biāo)點云的ND體素的尺寸，單位為米ndt_new.setMaximumIterations(max_iterations);使用牛頓法優(yōu)化的迭代次數(shù)
具體參數(shù)的意義以及NDT算法的理論基礎(chǔ)可以參考我前面的兩篇博客自動駕駛系統(tǒng)進(jìn)階與項目實戰(zhàn)（二） 和 無人駕駛汽車系統(tǒng)入門（十三）
NDT配準(zhǔn)定位
配準(zhǔn)定位主要實現(xiàn)于以下回調(diào)中：
void callback_pointcloud(const sensor_msgs::PointCloud2::ConstPtr & sensor_points_sensorTF_msg_ptr);
  • 1
該回調(diào)監(jiān)聽降采樣后的點云，首先解析PointCloud2消息為pcl的PointCloud結(jié)構(gòu)：
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
new pcl::PointCloud );
  • 1
  • 2
  • 3
  • 4
  • 5
該點云是在激光雷達(dá)坐標(biāo)系下，所以接著將數(shù)據(jù)投射到base_link坐標(biāo)系下：
// get TF base to sensor
geometry_msgs::TransformStamped::Ptr TF_base_to_sensor_ptr(new geometry_msgs::TransformStamped);
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();
boost::shared_ptr
new pcl::PointCloud );
pcl::transformPointCloud(
*sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix);
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
設(shè)置為NDT的輸入點云：
// set input point cloud
ndt_.setInputSource(sensor_points_baselinkTF_ptr);
if (ndt_.getInputTarget() == nullptr) {
ROS_WARN_STREAM_THROTTLE(1, "No MAP!");
return;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
最后我們還需要設(shè)定這次配準(zhǔn)的初始姿態(tài)估計，這里需要分為以下兩種情況：
// align
Eigen::Matrix4f initial_pose_matrix;
if (!init_pose){
Eigen::Affine3d initial_pose_affine;
tf2::fromMsg(initial_pose_cov_msg_.pose.pose, initial_pose_affine);
initial_pose_matrix = initial_pose_affine.matrix().cast();
// for the first time, we don't know the pre_trans, so just use the init_trans,
// which means, the delta trans for the second time is 0
pre_trans = initial_pose_matrix;
init_pose = true;
}else
{
// use predicted pose as init guess (currently we only impl linear model)
initial_pose_matrix = pre_trans * delta_trans;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
如果是第一次配準(zhǔn)，則使用我們在Rviz中手工指定的初始姿態(tài)，否則使用線性模型（勻速勻角速度）預(yù)測的初始估計。pcl實現(xiàn)的NDT要求初始姿態(tài)估計使用Eigen::Matrix4f表示(也就是標(biāo)準(zhǔn)的齊次變換矩陣)，所以上面的代碼中，如果是初次配準(zhǔn)，需要將Pose轉(zhuǎn)換為Eigen::Matrix4f，使用tf2::fromMsg()函數(shù)完成，對于非初次配準(zhǔn)，我們的思路是用上一次NDT的定位結(jié)果（變換矩陣pre_trans） + 線性變換量（變換矩陣delta_trans）。在線性代數(shù)中，如果用向量 ABABAB 描述上一次定位的變換（即上一次定位base_link到地圖原點的變換，即pre_trans）， BCBCBC 表示當(dāng)前一次定位到上一次定位的變換（即delta_trans）,那么當(dāng)前的定位 ACACAC就可以表示為：
AC=AB∗BC
AC = AB * BC
A
C
=
A
B
∗
B
C
所以對當(dāng)前位置的初始姿態(tài)估計就可以用 pre_trans * delta_trans表示。接著我們設(shè)置該初始估計，并且使用ndt進(jìn)行配準(zhǔn)：
ndt_.align(*output_cloud, initial_pose_matrix);
const Eigen::Matrix4f result_pose_matrix = ndt_.getFinalTransformation();
  • 1
  • 2
最終我們獲得了定位的變換矩陣（base_link 到map的變換）result_pose_matrix，將之轉(zhuǎn)換為Pose msg以及TF發(fā)布出去，完成本次定位：
Eigen::Affine3d result_pose_affine;
result_pose_affine.matrix() = result_pose_matrix.cast();
const geometry_msgs::Pose result_pose_msg = tf2::toMsg(result_pose_affine);
// 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);
}
// publish tf(map frame to base frame)
publish_tf(map_frame_, base_frame_, result_pose_stamped_msg);
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
此外，我們還需要計算下一次用于初始姿態(tài)估計的delta_trans:
// calculate the delta tf from pre_trans to current_trans
delta_trans = pre_trans.inverse() * result_pose_matrix;
pre_trans = result_pose_matrix;
  • 1
  • 2
  • 3
  • 4
delta_trans即當(dāng)前變換和上一次變換的差值（平移量和旋轉(zhuǎn)量的差值）。最后將當(dāng)前的變換保存為pre_trans供下一次初始姿態(tài)估計使用。至此NDT配準(zhǔn)的流程結(jié)束。
可視化和車輛描述模型
為了可視化定位結(jié)果，我們使用一個URDF模型來可視化車輛，該模型主要包含以下三個文件：
  • lexus.urdf: 即車輛模型的URDF描述文件
  • lexus.dae: 即車輛的3D模型文件
  • lexus.jpg: 3D模型的表面材料
以上文件均包含于項目的urdf文件夾下，使用lexus.launch啟動joint_state_publisher和robot_state_publisher兩個ros node以啟用車輛模型，launch文件定義如下：
 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
NDT激光雷達(dá)定位項目實戰(zhàn)
首先下載ndt_localizer代碼,將ndt_localizer倉庫克隆至你的ros_workspace/src/目錄下：
git clone https://github.com/AbangLZU/ndt_localizer.git
  • 1
準(zhǔn)備數(shù)據(jù)，這里我們?nèi)匀皇褂煤蜕弦黄┛屯瑯拥臄?shù)據(jù)（Mulran數(shù)據(jù)集的KAIST02數(shù)據(jù)包），下載路徑見文末百度網(wǎng)盤鏈接。該數(shù)據(jù)包使用方法見我上一篇博客自動駕駛系統(tǒng)進(jìn)階與項目實戰(zhàn)（五）中的“基于SC-Lego-LOAM構(gòu)建較大規(guī)模的城區(qū)三維地圖”一節(jié)，因為使用數(shù)據(jù)包播包方式完全相同，在此不贅述，保證你能夠使用Mulran 的KAIST02數(shù)據(jù)集播出rostopic為/os1_points的點云數(shù)據(jù)即可。
使用我上一篇博客最后實踐生產(chǎn)出的點云地圖（偷懶的同學(xué)也可以通過此鏈接下載），當(dāng)然還是建議同學(xué)們跟著我上一篇博文的內(nèi)容自行生產(chǎn)地圖，這樣能夠?qū)嵺`點云地圖的生產(chǎn)和使用的全流程。將點云地圖命名為kaist02.pcd，并且復(fù)制到項目的map目錄下。編譯整個項目：
# inside your catkin workspace
catkin_make
  • 1
  • 2
編譯完成后，運(yùn)行l(wèi)aunch文件啟動所有節(jié)點：
source devel/setup.bash
roslaunch ndt_localizer ndt_localizer.launch
  • 1
  • 2
此時Rviz會被打開，等待點云地圖加載完成，如下圖所示：
 
在TopDownOrtho視角下，縮放到地圖原點位置，點擊rviz上方的2D Pose Estimate按鈕，在地圖中指定初始姿態(tài)，如下圖所示：
 
打開file_player開始播包：
 
收到點云數(shù)據(jù)后，ndt_localizer開始工作，Rviz中顯示的定位結(jié)果如下所示:
 
配置得當(dāng)?shù)脑挘ê线m的降采樣），NDT對計算資源要求很低，甚至能夠在TX2上穩(wěn)定運(yùn)行，如下圖NDT執(zhí)行一次點云配準(zhǔn)的耗時：
 
基本在30ms左右，滿足自動駕駛配準(zhǔn)定位的要求。 
分享到：
 
反對 0 舉報 0 收藏 0 評論 0
滬ICP備11026917號-25