從零開始搭二維鐳射SLAM --- 基於PL-ICP的鐳射雷達里程計
上篇文章講了如何用PL-ICP演算法做雷達的幀間匹配，知道了每幀雷達資料間的座標變換，累加起來就可以做成鐳射雷達里程計。(雖然又是一個失敗的里程計…)
實現里程計，就不得不提及TF。所以，我將先簡要介紹一下ROS的TF2庫。
1 TF2
1.1 座標系
ROS中有幾個十分常用的座標系，其簡介如下：
- map: 地圖座標系，也被稱為世界座標系，是靜止不動的
- odom: 里程計座標系，相對於map來說一般情況下是靜止的，有些情況下會變動（定位節點為了修正機器人的位姿從而改變了map->odom間的座標變換）
- base_link: 代表機器人的旋轉中心的座標系，相對於odom來說base_link是運動的
- laser_link: 鐳射雷達的座標系，相對於base_link來說是靜止的，因為雷達裝在機器人上，雷達不會自己飛起來
可以看到，上邊的座標系，是單方向依賴的， laser_link 依賴於 base_link，base_link 依賴於 odom 。也可以說成是一個座標系指向下一個座標系的，連起來的話也成了：
map -> odom -> base_link -> laser_link
在ROS中，將這種能夠連線起來的座標系稱為 TF樹，是一個由座標系組成的樹。可以通過rqt軟體來視覺化TF樹，也可以通過Rviz的tf顯示模組來視覺化TF樹。
本篇文章對應的TF樹為：
1.2 TF2
這些座標系間的座標變化是十分複雜的。同時，不同機器人間這些座標系的計算方式又是相同的，因此，ROS中提供了座標系管理的庫 TF，用來幫助開發人員管理座標系。
之前，ROS中使用 TF 作為座標系管理的庫，後來，由於種種原因，TF庫被棄用了。ROS改用 TF2 作為新的管理座標系的庫。
tf2的wiki主頁為：http://wiki.ros.org/tf2
tf2的官方教程為：http://wiki.ros.org/tf2/Tutorials
1.3 TF2常用資料型別與常用函式彙總
官方教程展示了最基本的使用tf2的方法，但是tf2還有很多功能以及更高階的用法 教程裡是沒有提及的。如TF2提供了很多方便的轉換格式的函式等等．
現在網上的資源講tf2的還比較少，大概有2方面原因：
一是tf第一代還可以用，很多程式碼還是在用tf第一代。二是使用tf2的開源專案比較少，只有少數幾個開源專案使用了。這兩個原因導致tf2的教程還比較少。
為此，我特意讀了一遍TF2的原始碼，將其中的 常用資料型別與常用函式 摘出來做了個總結，以便作為寫程式碼的參考，也可以讓大家更好的使用TF2 ，文章連結如下：
tf2常用資料型別與常用函式彙總：https://blog.csdn.net/tiancailx/article/details/111312853
關於TF2 更多的介紹這裡就不多說了，更詳細的說明請去tf2的wiki中檢視。
本篇文章將使用 TF2 進行里程計的實現。
2 程式碼
2.1 獲取程式碼
程式碼已經提交在github上了，如果不知道github的地址的朋友, 請在我的公眾號: 從零開始搭鐳射SLAM 中回覆 開源地址 獲得。
推薦使用 git clone 的方式進行下載, 因為程式碼是正處於更新狀態的, git clone 下載的程式碼可以使用 git pull 很方便地進行更新.
本篇文章對應的程式碼為 lesson3/include/lesson3/plicp_odometry.h 與 lesson3/src/plicp_odometry.cc。
2.2 程式碼解析
本次程式碼比上篇文章多瞭如下幾個函式
void InitParams();
bool GetBaseToLaserTf(const std::string &frame_id);
void GetPrediction(double &prediction_change_x, double &prediction_change_y, double &prediction_change_angle, double dt);
void CreateTfFromXYTheta(double x, double y, double theta, tf2::Transform& t);
void PublishTFAndOdometry();
bool NewKeyframeNeeded(const tf2::Transform &d);
2.2.1 建構函式
注意，private_node_ 初始化為 “~” ，代表私有名稱空間，可以用來獲取節點內的引數。
ScanMatchPLICP::ScanMatchPLICP() : private_node_("~"), tf_listener_(tfBuffer_)
{
// \033[1;32m，\033[0m 終端顯示成綠色
ROS_INFO_STREAM("\033[1;32m----> PLICP odometry started.\033[0m");
laser_scan_subscriber_ = node_handle_.subscribe(
"laser_scan", 1, &ScanMatchPLICP::ScanCallback, this);
odom_publisher_ = node_handle_.advertise<nav_msgs::Odometry>("odom_plicp", 50);
// 引數初始化
InitParams();
// 第一幀雷達還未到來
initialized_ = false;
base_in_odom_.setIdentity();
base_in_odom_keyframe_.setIdentity();
input_.laser[0] = 0.0;
input_.laser[1] = 0.0;
input_.laser[2] = 0.0;
// Initialize output_ vectors as Null for error-checking
output_.cov_x_m = 0;
output_.dx_dy1_m = 0;
output_.dx_dy2_m = 0;
}
2.2.2 InitParams()
使用私有控制程式碼 private_node_ 獲取節點內部引數，引數寫在配置檔案中了，配置檔案稍後講解。
下面的很多引數是為了對 pl-icp 演算法 進行引數配置，如最大迭代次數等等。
/*
* ros與csm的引數初始化
*/
void ScanMatchPLICP::InitParams()
{
private_node_.param<std::string>("odom_frame", odom_frame_, "odom");
private_node_.param<std::string>("base_frame", base_frame_, "base_link");
// **** keyframe params: when to generate the keyframe scan
// if either is set to 0, reduces to frame-to-frame matching
private_node_.param<double>("kf_dist_linear", kf_dist_linear_, 0.1);
private_node_.param<double>("kf_dist_angular", kf_dist_angular_, 5.0 * (M_PI / 180.0));
kf_dist_linear_sq_ = kf_dist_linear_ * kf_dist_linear_;
// **** CSM 的引數 - comments copied from algos.h (by Andrea Censi)
// Maximum angular displacement between scans
if (!private_node_.getParam("max_angular_correction_deg", input_.max_angular_correction_deg))
input_.max_angular_correction_deg = 45.0;
// Maximum translation between scans (m)
if (!private_node_.getParam("max_linear_correction", input_.max_linear_correction))
input_.max_linear_correction = 0.50;
// Maximum ICP cycle iterations
if (!private_node_.getParam("max_iterations", input_.max_iterations))
input_.max_iterations = 10;
// A threshold for stopping (m)
if (!private_node_.getParam("epsilon_xy", input_.epsilon_xy))
input_.epsilon_xy = 0.000001;
// A threshold for stopping (rad)
if (!private_node_.getParam("epsilon_theta", input_.epsilon_theta))
input_.epsilon_theta = 0.000001;
// Maximum distance for a correspondence to be valid
if (!private_node_.getParam("max_correspondence_dist", input_.max_correspondence_dist))
input_.max_correspondence_dist = 0.3;
// Noise in the scan (m)
if (!private_node_.getParam("sigma", input_.sigma))
input_.sigma = 0.010;
// Use smart tricks for finding correspondences.
if (!private_node_.getParam("use_corr_tricks", input_.use_corr_tricks))
input_.use_corr_tricks = 1;
// Restart: Restart if error is over threshold
if (!private_node_.getParam("restart", input_.restart))
input_.restart = 0;
// Restart: Threshold for restarting
if (!private_node_.getParam("restart_threshold_mean_error", input_.restart_threshold_mean_error))
input_.restart_threshold_mean_error = 0.01;
// Restart: displacement for restarting. (m)
if (!private_node_.getParam("restart_dt", input_.restart_dt))
input_.restart_dt = 1.0;
// Restart: displacement for restarting. (rad)
if (!private_node_.getParam("restart_dtheta", input_.restart_dtheta))
input_.restart_dtheta = 0.1;
// Max distance for staying in the same clustering
if (!private_node_.getParam("clustering_threshold", input_.clustering_threshold))
input_.clustering_threshold = 0.25;
// Number of neighbour rays used to estimate the orientation
if (!private_node_.getParam("orientation_neighbourhood", input_.orientation_neighbourhood))
input_.orientation_neighbourhood = 20;
// If 0, it's vanilla ICP
if (!private_node_.getParam("use_point_to_line_distance", input_.use_point_to_line_distance))
input_.use_point_to_line_distance = 1;
// Discard correspondences based on the angles
if (!private_node_.getParam("do_alpha_test", input_.do_alpha_test))
input_.do_alpha_test = 0;
// Discard correspondences based on the angles - threshold angle, in degrees
if (!private_node_.getParam("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg))
input_.do_alpha_test_thresholdDeg = 20.0;
// Percentage of correspondences to consider: if 0.9,
// always discard the top 10% of correspondences with more error
if (!private_node_.getParam("outliers_maxPerc", input_.outliers_maxPerc))
input_.outliers_maxPerc = 0.90;
// Parameters describing a simple adaptive algorithm for discarding.
// 1) Order the errors.
// 2) Choose the percentile according to outliers_adaptive_order.
// (if it is 0.7, get the 70% percentile)
// 3) Define an adaptive threshold multiplying outliers_adaptive_mult
// with the value of the error at the chosen percentile.
// 4) Discard correspondences over the threshold.
// This is useful to be conservative; yet remove the biggest errors.
if (!private_node_.getParam("outliers_adaptive_order", input_.outliers_adaptive_order))
input_.outliers_adaptive_order = 0.7;
if (!private_node_.getParam("outliers_adaptive_mult", input_.outliers_adaptive_mult))
input_.outliers_adaptive_mult = 2.0;
// If you already have a guess of the solution, you can compute the polar angle
// of the points of one scan in the new position. If the polar angle is not a monotone
// function of the readings index, it means that the surface is not visible in the
// next position. If it is not visible, then we don't use it for matching.
if (!private_node_.getParam("do_visibility_test", input_.do_visibility_test))
input_.do_visibility_test = 0;
// no two points in laser_sens can have the same corr.
if (!private_node_.getParam("outliers_remove_doubles", input_.outliers_remove_doubles))
input_.outliers_remove_doubles = 1;
// If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov
if (!private_node_.getParam("do_compute_covariance", input_.do_compute_covariance))
input_.do_compute_covariance = 0;
// Checks that find_correspondences_tricks gives the right answer
if (!private_node_.getParam("debug_verify_tricks", input_.debug_verify_tricks))
input_.debug_verify_tricks = 0;
// If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the
// incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence.");
if (!private_node_.getParam("use_ml_weights", input_.use_ml_weights))
input_.use_ml_weights = 0;
// If 1, the field 'readings_sigma' in the second scan is used to weight the
// correspondence by 1/sigma^2
if (!private_node_.getParam("use_sigma_weights", input_.use_sigma_weights))
input_.use_sigma_weights = 0;
}
2.2.3 GetBaseToLaserTf()
這個函式是在初始化階段，用來獲取機器人座標系與雷達座標系間的座標變換。
/**
* 獲取機器人座標系與雷達座標系間的座標變換
*/
bool ScanMatchPLICP::GetBaseToLaserTf(const std::string &frame_id)
{
ros::Time t = ros::Time::now();
geometry_msgs::TransformStamped transformStamped;
// 獲取tf並不是瞬間就能獲取到的，要給1秒的緩衝時間讓其找到tf
try
{
transformStamped = tfBuffer_.lookupTransform(base_frame_, frame_id,
t, ros::Duration(1.0));
}
catch (tf2::TransformException &ex)
{
ROS_WARN("%s", ex.what());
ros::Duration(1.0).sleep();
return false;
}
// 將獲取的tf存到base_to_laser_中
tf2::fromMsg(transformStamped.transform, base_to_laser_);
laser_to_base_ = base_to_laser_.inverse();
return true;
}
2.2.4 ScanMatchWithPLICP()
這個函式的實現較上篇文章中略有增加，增加了 基於勻速模型的位姿預測，位姿累加，釋出TF以及odom話題，新建關鍵幀，這4個功能。
/**
* 使用PLICP進行幀間位姿的計算
*/
void ScanMatchPLICP::ScanMatchWithPLICP(LDP &curr_ldp_scan, const ros::Time &time)
{
// CSM is used in the following way:
// The scans are always in the laser frame
// The reference scan (prevLDPcan_) has a pose of [0, 0, 0]
// The new scan (currLDPScan) has a pose equal to the movement
// of the laser in the laser frame since the last scan
// The computed correction is then propagated using the tf machinery
prev_ldp_scan_->odometry[0] = 0.0;
prev_ldp_scan_->odometry[1] = 0.0;
prev_ldp_scan_->odometry[2] = 0.0;
prev_ldp_scan_->estimate[0] = 0.0;
prev_ldp_scan_->estimate[1] = 0.0;
prev_ldp_scan_->estimate[2] = 0.0;
prev_ldp_scan_->true_pose[0] = 0.0;
prev_ldp_scan_->true_pose[1] = 0.0;
prev_ldp_scan_->true_pose[2] = 0.0;
input_.laser_ref = prev_ldp_scan_;
input_.laser_sens = curr_ldp_scan;
// 勻速模型，速度乘以時間，得到預測的odom座標系下的位姿變換
double dt = (time - last_icp_time_).toSec();
double pr_ch_x, pr_ch_y, pr_ch_a;
GetPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);
tf2::Transform prediction_change;
CreateTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, prediction_change);
// account for the change since the last kf, in the fixed frame
// 將odom座標系下的座標變換 轉換成 base_in_odom_keyframe_座標系下的座標變換
prediction_change = prediction_change * (base_in_odom_ * base_in_odom_keyframe_.inverse());
// the predicted change of the laser's position, in the laser frame
// 將base_link座標系下的座標變換 轉換成 雷達座標系下的座標變換
tf2::Transform prediction_change_lidar;
prediction_change_lidar = laser_to_base_ * base_in_odom_.inverse() * prediction_change * base_in_odom_ * base_to_laser_;
input_.first_guess[0] = prediction_change_lidar.getOrigin().getX();
input_.first_guess[1] = prediction_change_lidar.getOrigin().getY();
input_.first_guess[2] = tf2::getYaw(prediction_change_lidar.getRotation());
// If they are non-Null, free covariance gsl matrices to avoid leaking memory
if (output_.cov_x_m)
{
gsl_matrix_free(output_.cov_x_m);
output_.cov_x_m = 0;
}
if (output_.dx_dy1_m)
{
gsl_matrix_free(output_.dx_dy1_m);
output_.dx_dy1_m = 0;
}
if (output_.dx_dy2_m)
{
gsl_matrix_free(output_.dx_dy2_m);
output_.dx_dy2_m = 0;
}
start_time_ = std::chrono::steady_clock::now();
// 呼叫csm進行plicp計算
sm_icp(&input_, &output_);
end_time_ = std::chrono::steady_clock::now();
time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
// std::cout << "PLICP計算用時: " << time_used_.count() << " 秒。" << std::endl;
tf2::Transform corr_ch;
if (output_.valid)
{
// 雷達座標系下的座標變換
tf2::Transform corr_ch_l;
CreateTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
// 將雷達座標系下的座標變換 轉換成 base_link座標系下的座標變換
corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;
// 更新 base_link 在 odom 座標系下 的座標
base_in_odom_ = base_in_odom_keyframe_ * corr_ch;
latest_velocity_.linear.x = corr_ch.getOrigin().getX() / dt;
latest_velocity_.angular.z = tf2::getYaw(corr_ch.getRotation()) / dt;
}
else
{
ROS_WARN("not Converged");
}
// 釋出tf與odom話題
PublishTFAndOdometry();
// 檢查是否需要更新關鍵幀座標
if (NewKeyframeNeeded(corr_ch))
{
// 更新關鍵幀座標
ld_free(prev_ldp_scan_);
prev_ldp_scan_ = curr_ldp_scan;
base_in_odom_keyframe_ = base_in_odom_;
}
else
{
ld_free(curr_ldp_scan);
}
last_icp_time_ = time;
}
上一篇程式碼有人評論說執行一次 sm_icp() 只花了0.5ms，是因為沒有進行迭代，執行迭代的用時大概在5ms左右．
所以在呼叫 sm_icp() 前後加了計算時間的功能，上次程式碼沒有設定迭代次數，用了預設的，不知道它預設的是進行一次還是幾次．
這次的程式碼設定了最大迭代次數10，將列印時間的語句解除註釋，將顯示如下內容
PLICP計算用時: 0.00638714 秒。
PLICP計算用時: 0.00417142 秒。
PLICP計算用時: 0.00804754 秒。
PLICP計算用時: 0.00697412 秒。
PLICP計算用時: 0.00545498 秒。
PLICP計算用時: 0.00731748 秒。
PLICP計算用時: 0.00811902 秒
可以看到，迭代10次的情況下，平均用時大概在6ms左右．
可見上次評論我的小哥是正確的，我在這裡對這位小哥表示感謝．
2.2.5 GetPrediction()
假設機器人在2幀雷達資料間的運動是勻速的，通過速度乘以時間來推測這段時間內的位移．
/**
* 推測從上次icp的時間到當前時刻間的座標變換
* 使用勻速模型，根據當前的速度，乘以時間，得到推測出來的位移
*/
void ScanMatchPLICP::GetPrediction(double &prediction_change_x,
double &prediction_change_y,
double &prediction_change_angle,
double dt)
{
// 速度小於 1e-6 , 則認為是靜止的
prediction_change_x = latest_velocity_.linear.x < 1e-6 ? 0.0 : dt * latest_velocity_.linear.x;
prediction_change_y = latest_velocity_.linear.y < 1e-6 ? 0.0 : dt * latest_velocity_.linear.y;
prediction_change_angle = latest_velocity_.linear.z < 1e-6 ? 0.0 : dt * latest_velocity_.linear.z;
if (prediction_change_angle >= M_PI)
prediction_change_angle -= 2.0 * M_PI;
else if (prediction_change_angle < -M_PI)
prediction_change_angle += 2.0 * M_PI;
}
2.2.6 CreateTfFromXYTheta()
為 tf2::Transform 賦值
/**
* 從x,y,theta建立tf
*/
void ScanMatchPLICP::CreateTfFromXYTheta(double x, double y, double theta, tf2::Transform &t)
{
t.setOrigin(tf2::Vector3(x, y, 0.0));
tf2::Quaternion q;
q.setRPY(0.0, 0.0, theta);
t.setRotation(q);
}
2.2.7 PublishTFAndOdometry()
釋出tf與odom話題，其形式如下
/**
* 釋出tf與odom話題
*/
void ScanMatchPLICP::PublishTFAndOdometry()
{
geometry_msgs::TransformStamped tf_msg;
tf_msg.header.stamp = current_time_;
tf_msg.header.frame_id = odom_frame_;
tf_msg.child_frame_id = base_frame_;
tf_msg.transform = tf2::toMsg(base_in_odom_);
// 釋出 odom 到 base_link 的 tf
tf_broadcaster_.sendTransform(tf_msg);
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = current_time_;
odom_msg.header.frame_id = odom_frame_;
odom_msg.child_frame_id = base_frame_;
tf2::toMsg(base_in_odom_, odom_msg.pose.pose);
odom_msg.twist.twist = latest_velocity_;
// 釋出 odomemtry 話題
odom_publisher_.publish(odom_msg);
}
2.2.8 NewKeyframeNeeded()
判斷是否需要建立一個關鍵幀，這裡的關鍵幀只是通過距離來進行簡單的判斷，當平移與角度的變換超過閾值時新新增一個關鍵幀．
/**
* 如果平移大於閾值，角度大於閾值，則創新新的關鍵幀
* @return 需要建立關鍵幀返回true, 否則返回false
*/
bool ScanMatchPLICP::NewKeyframeNeeded(const tf2::Transform &d)
{
if (fabs(tf2::getYaw(d.getRotation())) > kf_dist_angular_)
return true;
if (scan_count_++ == kf_scan_count_)
{
scan_count_ = 0;
return true;
}
double x = d.getOrigin().getX();
double y = d.getOrigin().getY();
if (x * x + y * y > kf_dist_linear_sq_)
return true;
return false;
}
3 執行
3.1 生成新bag檔案
本篇文章對應的資料包, 請在我的公眾號中回覆 lesson1 獲得。
由於 lesson1 這個資料包中已經有了tf，而本篇文章的程式碼也要釋出TF，為了讓流程更加清晰，現在我重新錄製一個沒有tf的資料包．
通過分別在3個終端中執行如下命令進行新bag的錄製．
# 終端1
cd ~/bagfiles
roscore
# 終端2
rosparam set use_sim_time true
cd ~/bagfiles
rosbag play --clock lesson1.bag
# 終端3
cd ~/bagfiles
rosbag record /laser_scan /odom
當bag跑完的時候，在終端3按 ctrl+c ，結束錄製。
並將所有終端關閉。
最後，再將生成的bag檔案重新命名為lesson3.bag，並確認處於 ~/bagfiles/ 資料夾下。
其實也可以不重新錄製包，只要將我們程式碼的TF的odom座標系，改個名字如odom_plicp，再將Rviz的Fixed Frame設定成odom_plicp也可以．
3.2 launch檔案
<launch>
<!-- bag的地址與名稱 -->
<arg name="bag_filename" default="/home/lx/bagfiles/lesson3.bag"/>
<!-- 使用bag的時間戳 -->
<param name="use_sim_time" value="true" />
<!-- base_link to front_laser_link -->
<node pkg="tf2_ros" type="static_transform_publisher" name="link_broadcaster"
args="0 0 0.254 0 0 3.1415926 base_link front_laser_link" />
<!-- 啟動 plicp_odometry 節點 -->
<node name="lesson3_plicp_odometry_node"
pkg="lesson3" type="lesson3_plicp_odometry_node" output="screen" >
<rosparam file="$(find lesson3)/config/plicp_odometry.yaml" command="load"/>
</node>
<!-- launch rviz -->
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find lesson3)/config/plicp_odometry.rviz" />
<!-- play bagfile -->
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
</launch>
3.3 靜態TF
在 launch 中釋出了一個從base_link到front_laser_link的靜態tf，代表從機器人旋轉中心到雷達座標系間的座標變換。
其視覺化效果如下圖所示．
可見，鐳射雷達是倒著安裝的，並處於base_link的正上方．
3.4 編譯與執行
下載程式碼後，請放入您自己的工作空間中，通過 catkin_make 進行編譯．
由於是新增的包，所以需要通過 rospack profile 命令讓ros找到這個新的包．
之後, 使用source命令，新增ros與工作空間的地址到當前終端下．
再通過如下命令執行節點．
roslaunch lesson3 plicp_odometry.launch
3.5 配置檔案
在啟動節點的時候，通過在節點內部使用如下命令載入了配置檔案．
<rosparam file="$(find lesson3)/config/plicp_odometry.yaml" command="load"/>
配置檔案位於 lesson3/config 的資料夾下，名為 plicp_odometry.yaml.
其內容為：
odom_frame: "odom"
base_frame: "base_link"
kf_dist_linear: 0.1 # m
kf_dist_angular: 0.5 # deg
配置了 odom座標系的名字，機器人座標系的名字，設定關鍵幀的平移閾值與角度閾值．
3.6 執行結果
紅色軌跡為通過小車自身編碼器累加出來的里程計，黃色軌跡為通過PL-ICP演算法累加出來的鐳射雷達里程計。
再來看一個更大的圖片，圖中細線格子的邊長為10米，所以整體軌跡最遠處大概走了30米。
這個資料包對應的環境為一個大概30米左右的長走廊．
由上圖可知，當編碼器累加出來的軌跡走完一圈並回到差不多原點的位置處，而黃色軌跡還差了半圈沒有回來．
這是因為走到走廊盡頭處掉頭要走第二遍的時候，出現的匹配錯誤與匹配失敗．
在走廊盡頭，旋轉時發生了匹配錯誤，機器人始終在前進，但是pl-icp計算出來的位姿並沒有，導致黃色里程計不動．
當匹配錯誤一段時間之後，機器人估計的位姿與實際位姿差別很大，匹配也就不能成功了，所以之後也就再沒有動．
當匹配失敗的時候終端會出現如下所示內容，前2行為csm的輸出，[warn] 為ROS節點的輸出，代表匹配失敗．
:err: : before trimming, only 0 correspondences.
:err: icp: ICP failed for some reason.
[ WARN] [1608712072.514938783, 1606455702.002871441]: not Converged
:err: : before trimming, only 0 correspondences.
:err: icp: ICP failed for some reason.
[ WARN] [1608712072.616003034, 1606455702.113580472]: not Converged
:err: : before trimming, only 0 correspondences.
:err: icp: ICP failed for some reason.
[ WARN] [1608712072.707236863, 1606455702.204212566]: not Converged
4 結果分析
4.1 現象分析
4.1.1 算出來的里程計比真實距離長
黃色軌跡比紅色軌跡(編碼器)長，猜測是在長走廊環境下匹配出的位姿產生了漂移．
因為我是用的勻速模型，在機器人減速時就會導致預測時使用的速度比實際速度大，就會導致預測的位移比實際位移大．而在走廊環境下的匹配可能 在機器人的前後兩個方向上沒有約束，所以導致算出來的里程計邊長．只是猜測．．．
4.1.2 在走到走廊盡頭處掉頭要走第二遍的時候，出現的匹配錯誤
這一現象我還不清楚原因，想要知道具體原因就需要去看csm的程式碼了，之前大致看過一次，只不過後來忘記了．．．
以為是匹配失敗後，導致關鍵幀與當前scan差距太大，所以後來更改了程式碼讓每5個scan就新增關鍵幀，但並沒有解決這個問題．．．
4.2 失敗原因猜測1: 環境
為了分析原因，特意去拍了 匹配失敗處的走廊 的圖片。
可以看到這個環境對於雷達來說真是非常苛刻了，基本上只能看到兩邊的牆，雖然後邊也有一點牆，但是還是失敗了．這也證明了這個演算法還是不夠魯棒．
4.2 失敗原因猜測2: 雷達最遠距離與雷達射線的強度
這個包是用鐳神ls01b的雷達錄製的，2000元，最遠距離25m，雷達頻率為10hz．
失敗的原因可能也是因為雷達的距離太短，看不到走廊另一頭．
猜測還有個原因是因為雷達射線的強度比較低，在大角度情況下沒有返回值，導致有效數量比較少．
4.3 失敗原因猜測4: 引數配置問題
引數使用的是 laser_scan_matcher 這個包使用的引數，並沒有進行額外的調參，可能這套引數不太適用於長走廊環境．
具體引數的意義就要去看csm的程式碼了．
我就不再進行深入的除錯了，有興趣的同學可以調調引數感受一下不同引數會導致的結果有何不同．因為接下來還有更多更精彩的操作等著我去實現，所以這個問題先放在這裡，等著我以後水平更高的時候再來解決．
5 一些想法
最近加我的碩士好多都說不知道做鐳射SLAM相關課題還能做啥，都是想著做鐳射和視覺融合，但是找不到具體的點．
對於上面的長走廊環境，單雷達的效果是非常不好的，而且，這個環境並不是全都是白牆這種沒紋理的場景，還存在一些類似門洞一樣的結構．
這種場景提取影像的特徵點應該還是很魯棒的，這就是一個結合點啊：有紋理的長走廊環境下雷達與單目視覺的融合．
網上好多人說二維鐳射SLAM已經很穩定了，我感覺雖然大體上穩定了，但是還是有很多不足的．
例如，現在應該沒有任何一套二維鐳射SLAM可以用一套引數適用於不同場景的，都是針對具體場景做適應性開發．而且，針對具體場景下的建圖也不是百分百成功的，建圖成功率還和機器人運動速度，旋轉速度，運動軌跡相關．
其他的，如長走廊環境的建圖，礦場這種特別空曠環境的建圖，室內結構相似環境下的迴環檢測，這些情況只使用鐳射SLAM基本是不行的，一定要進行多感測器融合，簡單的可以融合imu，里程計，更復雜一點的就是鐳射與視覺做融合．
對於二維鐳射SLAM來說，大多都是依賴於里程計，還沒有使用 IMU的預積分 進行感測器融合的，這也是一個可以寫論文的點．
6 Next
計劃中還有個NDT的scan-to-scan的方法沒有實現，這個演算法也是直接呼叫PCL中的庫函式就行，優先順序可以先放一放，之後再來對他進行實現。
接下來幾篇文章將進行柵格地圖的構建，構建地圖不像寫里程計的程式碼這樣簡單，需要好幾個檔案，估計要通過幾篇文章的長度才能構建完。
第一步先實現單純的柵格地圖的構建，之後再與這篇文章的PL-ICP里程計聯合到一起，做一個SLAM的前端。
文章將在 公眾號: 從零開始搭SLAM 進行同步更新，歡迎大家關注，可以在公眾號中新增我的微信，進鐳射SLAM交流群，大家一起交流SLAM技術。
同時，也希望您將這個公眾號推薦給您身邊做鐳射SLAM的人們，大家共同進步。
如果您對我寫的文章有什麼建議，或者想要看哪方面功能如何實現的，請直接在公眾號中回覆，我可以收到，並將認真考慮您的建議。
相關文章
- 由iphone12說說鐳射雷達 FMCW鐳射雷達iPhone
- 鐳射雷達線數 單線與多線鐳射雷達的區別
- 固態鐳射雷達優劣
- 用於鐳射雷達的 APD，SPAD 和 SiPM 分析
- 基於因子圖優化的鐳射IMU融合SLAM學習優化SLAM
- 大疆攪局鐳射雷達市場
- 深入瞭解基於鐳射雷達的機器人定位感測器機器人
- 高效能車規級鐳射雷達 - InnovizOne ™
- 當“思嵐”鐳射雷達邂逅盲人柺杖
- 鐳射雷達主要應用在哪些領域？
- 幾千塊也能買到鐳射雷達，大疆進軍自動駕駛，旗下公司鐳射雷達首亮相自動駕駛
- 成本僅100美元的鐳射雷達：這個專案教你零基礎DIY
- 北醒鐳射雷達模組 資料彙總
- 鐳射雷達常用的資料處理軟體推薦
- 鐳射雷達上車，到底應該做到什麼水平？
- 無人駕駛科技“大閱兵”之——鐳射雷達
- 單目線鐳射三維重建
- Ouster釋出最新REV7系列數字鐳射雷達
- 智慧汽車量產“排位賽”：鐳射雷達與毫米波雷達的角逐戰
- 無人機和鐳射雷達，會是一對好cp嗎？無人機
- 鐳射雷達點雲語義分割學習筆記之RangeNet++筆記
- 一種視覺慣性+鐳射感測器的SLAM系統視覺SLAM
- qt鐳射加工軟體開發（一）QT
- 新款iPad Pro「背後」暗藏玄機：鐳射雷達還能這麼小？iPad
- 與鐳射雷達死磕的毫米波雷達，除了無人駕駛還有更多歸宿
- bzoj1121: [POI2008]鐳射發射器SZK
- 華盛資本：2021年鐳射雷達行業研究報告（附下載）行業
- 自動駕駛感知系統路線之爭：純視覺or鐳射雷達自動駕駛視覺
- Velodyne稱將釋出固態汽車鐳射雷達，或在2018年量產
- 印表機噴墨和鐳射哪個好 噴墨印表機和鐳射印表機的區別
- CMOS鐳射感測器GV系列
- 鐳射雷達2021或迎量產上車元年，蔚來、小鵬、本田等助力
- 鐳射雷達點雲3D模型點雲資料之ply檔案格式3D模型
- WEY摩卡預售17.98萬元起，鐳射雷達L3版11月交付
- 與無人駕駛相比 鐳射雷達為何能在機器人身上更快落地？機器人
- Yole：預計2026年汽車和工業應用的鐳射雷達市場將達到57億美元
- 谷歌竟公開銷售鐳射雷達, 角色突變是自動駕駛商業化所迫？谷歌自動駕駛
- 超聲波、毫米波、ToF鐳射雷達——在低功耗場景的應用選型