從零搭建無人車(1)-鐳射雷達資料預處理
阿新 • • 發佈：2020-03-04
從零搭建無人車(1)-鐳射雷達資料預處理
一 原理簡述
1 鐳射雷達產生畸變的原因
通常鐳射雷達在工作的時候，我們認為其每一幀鐳射對應一個位姿（相當於認為這幀鐳射的時間裡機器人的座標沒變，也就是靜止），但是每幀鐳射的時間裡機器人實際上是在運動的。
2 常見的去除畸變的方法-里程計輔助方法
第一種是純估計方法：ICP類方法，VICP。
另外一種就是里程計輔助的方法，十分常用。
我們用里程的位姿來標定鐳射雷達。這麼說還是比較籠統，
其實就是以里程計座標為基準通過插值來求出每一鐳射束對應的機器人位姿（暫且認為里程計得到的座標準確）。
大體流程
由原始鐳射資料，我們可以知道每一束鐳射打在障礙物上的鐳射點距離里程計座標系原點的距離值和角度值。
轉換為距離值和角度值,去畸變之後,轉化為sensor_msgs::pointcloud重新發布
去畸變流程
那麼首先我們要做一個機器人運動的假設，假設機器人在一幀鐳射資料的時間裡做勻速運動。
然後把里程計得到的座標儲存到一個佇列裡面，和鐳射雷達資料的時間對齊之後要保證里程計佇列完全覆蓋這一幀的掃描資料。
我們以鐳射雷達掃描的一幀鐳射資料為例，這一幀資料裡有若干個鐳射束.
實際上我們去畸變的過程就是求解出具體每一束鐳射對應的機器人位姿，
求解出位姿再把這個位姿轉換回掃描資料的距離值和角度值，重新發布出去。
二 程式碼框架
==1 主函式==
初始化LidarMotionCalib節點
ros::init(argc,argv,"LidarMotionCalib");構建一個tf監聽者物件
tf::TransformListener tf(ros::Duration(10.0));構建 LidarMotionCalibration 物件 , 把tf作為建構函式的引數傳入
LidarMotionCalibrator tmpLidarMotionCalib(&tf);進入回撥函式
ros::spin();==2 LidarMotionCalibrator類==
先看變數這部分：
public: tf::TransformListener* tf_; ros::NodeHandle nh_; ros::Subscriber scan_sub_; ros::Publisher points_pub_;
- 定義了一個tf監聽者 tf_
- 定義了節點控制代碼 nh_
- 定義了一個訂閱者（ scan_sub_），訂閱 sensor_msgs::LaserScan
- 定義了一個點雲資料釋出者( points_pub_ ) 釋出 sensor_msgs::pointcloud
==3 LidarMotionCalibrator類的建構函式與解構函式==
- 引數是tf，初始化時監聽ros的座標變換
- 訂閱者 scan_sub_ 訂閱名為 /scan 的 topic ，註冊訊息回撥函式。這個地方加上引用，不是很懂
- 釋出者 points_pub_ 釋出型別為
LidarMotionCalibrator( tf::TransformListener* tf )
{
tf_ = tf;
scan_sub_ = nh_.subscribe("/scan", 10, &LidarMotionCalibrator::ScanCallBack, this);
points_pub_ = nh_.advertise<sensor_msgs::PointCloud>("/cloud", 10);
}
~LidarMotionCalibrator()
{
if(tf_!=NULL)
delete tf_;
}==4 回撥函式 void ScanCallBack()==
當接收到訂閱的訊息後，就會進入訊息回撥函式。
傳入引數為 LaserScan
void ScanCallBack(const sensor_msgs::LaserScanPtr &scan_msg);- 得到一幀鐳射的開始時間和結束時間
- 開始時間等於鐳射資料的第一個時間戳。
- 結束時間等於開始時間加上鐳射束的數目乘上每束鐳射之間的時間增量。
- 計算當前幀資料中鐳射束的數量
ros::Time startTime, endTime;
startTime = scan_msg->header.stamp;
sensor_msgs::LaserScan laserScanMsg = *scan_msg;
// 計算當前幀資料的結束時間
int beamNum = laserScanMsg.ranges.size();
endTime = startTime + ros::Duration( laserScanMsg.time_increment * beamNum);轉換為距離值和角度值
std::vector<double> angles,ranges;
double lidar_angle = laserScanMsg.angle_max;
for( int i = beamNum - 1; i > 0; i-- )
{
double lidar_dist = laserScanMsg.ranges[i];
// 計算每個距離值對應的角度資訊
lidar_angle -= laserScanMsg.angle_increment ;
if(lidar_dist < 0.05 || std::isnan(lidar_dist) || std::isinf(lidar_dist))
lidar_dist = 0.0;
ranges.push_back(lidar_dist);
angles.push_back(lidar_angle);
}呼叫去畸變的函式
Lidar_Calibration(ranges, angles, startTime, endTime, tf_);根據機器人當前的航向角,轉化為點雲
tf::Stamped<tf::Pose> visualPose;
if( !getLaserPose(visualPose, startTime, tf_))
{
ROS_WARN("Not visualPose,Can not Calib");
return ;
}
// 得到機器人的yaw角度
double visualYaw = tf::getYaw(visualPose.getRotation());
//轉換為 pointcloud
sensor_msgs::PointCloud cloud;
cloud.header.frame_id = "odom";
cloud.header.stamp = ros::Time::now();
cloud.points.resize( ranges.size() );
for (int i = 0; i < ranges.size(); i++)
{
if (ranges[i] < 0.05 || std::isnan(ranges[i]) || std::isinf(ranges[i]))
continue;
double x = ranges[i] * cos(angles[i]);
double y = ranges[i] * sin(angles[i]);
geometry_msgs::Point32 pt;
pt.x = x * cos(visualYaw) - y * sin(visualYaw) + visualPose.getOrigin().getX();
pt.y = x * sin(visualYaw) + y * cos(visualYaw) + visualPose.getOrigin().getY();
pt.z = 0.1;
cloud.points.push_back(pt);
}釋出點雲資料
points_pub_.publish(cloud);==5 得到里程計位姿的函式 bool getLaserPose()==
bool getLaserPose(tf::Stamped<tf::Pose> &odom_pose,
ros::Time dt,
tf::TransformListener * tf_)
{
odom_pose.setIdentity();
tf::Stamped < tf::Pose > robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = "base_footprint";
robot_pose.stamp_ = dt; //設定為ros::Time()表示返回最近的轉換關係
// get the global pose of the robot
try
{
if ( !tf_->waitForTransform("/odom", "/base_footprint", dt, ros::Duration(0.5)) ) // 0.15s 的時間可以修改
{
ROS_ERROR("LidarMotion-Can not Wait Transform()");
return false;
}
tf_->transformPose("/odom", robot_pose, odom_pose);
}
catch (tf::LookupException& ex)
{
ROS_ERROR("LidarMotion: No Transform available Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR("LidarMotion: Connectivity Error looking up looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR("LidarMotion: Extrapolation Error looking up looking up robot pose: %s\n", ex.what());
return false;
}
return true;
}==6 Lidar_Calibration()函式==
引數
- 儲存鐳射資料的距離值和角度值的vector
- 當前幀鐳射的開始時間
- 當前幀鐳射的結束時間
- tf監聽者。
void Lidar_Calibration(std::vector<double> &ranges, std::vector<double> &angles, ros::Time startTime,ros::Time endTime, tf::TransformListener *tf_)流程
- 統計鐳射束的數量，需要兩個vector的大小一致。
int beamNumber = ranges.size();
if (beamNumber != angles.size())
{
ROS_ERROR("Error:ranges not match to the angles");
return;
}- 定義一個int型變數，值為5000。
int interpolation_time_duration = 5 * 1000;- 設定起止時間，結束時間，，每束鐳射的間隔時間。
- 當前插值段的起始下標。
- 得到起始點的座標和終點的座標
tf::Stamped<tf::Pose> frame_start_pose;
tf::Stamped<tf::Pose> frame_mid_pose;
tf::Stamped<tf::Pose> frame_base_pose;
tf::Stamped<tf::Pose> frame_end_pose;
//起始時間 us
double start_time = startTime.toSec() * 1000 * 1000;
double end_time = endTime.toSec() * 1000 * 1000;
double time_inc = (end_time - start_time) / beamNumber; // 每束鐳射資料的時間間隔
//當前插值的段的起始座標
int start_index = 0;
//起始點的位姿 這裡要得到起始點位置的原因是 起始點就是我們的base_pose
//所有的鐳射點的基準位姿都會改成我們的base_pose
// ROS_INFO("get start pose");
if (!getLaserPose(frame_start_pose, ros::Time(start_time / 1000000.0), tf_))
{
ROS_WARN("Not Start Pose,Can not Calib");
return;
}
if (!getLaserPose(frame_end_pose, ros::Time(end_time / 1000000.0), tf_))
{
ROS_WARN("Not End Pose, Can not Calib");
return;
}把基準座標設定成得到的起始點座標
開始進入迴圈：
遍歷每一束鐳射，得到每一束鐳射的時間，如果這束鐳射的時間-上一個斷點點的時間大於5ms就分段，計數+1。
仍在for迴圈裡，得到每一個斷點的里程計下的座標，（肯定會得到，沒得到就報錯了）
通過子函式Lidar_MotionCalibration對當前段進行插值。當前段是指從start_index到i的段。
每次迴圈結束，start_index會+1，i不一定加幾。。所以插值的就是一小段。- 跳轉到子函式Lidar_MotionCalibration。引數：基準座標，這一段的開始座標，這一段的結束座標，儲存角度值和距離值的vector。起始下標和裡面的斷點數，這裡面都是2。
int cnt = 0;
//基準座標就是第一個位姿的座標
frame_base_pose = frame_start_pose;
for (int i = 0; i < beamNumber; i++)
{
// 分段線性,時間段的大小為interpolation_time_duration
// 分段時間點 = 上次插值結束的時間 + 上次插值結束的幀數
double mid_time = start_time + time_inc * (i - start_index);
// 如果兩次時間差大於 5ms 或者 i 已經到了最後一個鐳射束
if (mid_time - start_time > interpolation_time_duration || (i == beamNumber - 1))
{
cnt++;
//得到插值段終點的位姿
if (!getLaserPose(frame_mid_pose, ros::Time(mid_time / 1000000.0), tf_))
{
ROS_ERROR("Mid %d Pose Error", cnt);
return;
}
//對當前的起點和終點進行插值
//interpolation_time_duration中間有多少個點.
int interp_count = i - start_index + 1;
// 插值
Lidar_MotionCalibration(frame_base_pose,
frame_start_pose,
frame_mid_pose,
ranges,
angles,
start_index,
interp_count);
//更新時間
//就是下次從當前段的mid time 插值
start_time = mid_time;
start_index = i;
// 下次的開始位姿就是當前插值段的最終位姿
frame_start_pose = frame_mid_pose;
}
}==7Lidar_MotionCalibration函式（實際進行插值的函式==
最後一個引數其實就是輸出的位姿數，其實就是把每一束的鐳射點對應的位姿都求出來。
三 遇到的問題
1 座標轉換問題
在實現實際插值的函式時候，我們通過里程計插值得到的機器人位姿是在里程計座標系下的（里程計座標系是整個程式的固定座標系）。然後我們需要把這個座標轉換到機器人的基準座標系下，基準座標系的原點是這幀鐳射第一個鐳射束對應的機器人位姿。拿到每束鐳射在里程計下的座標首先要把它轉換到基準座標系下。
對每一小段的位姿線性插值得到的是每個鐳射束對應的點在里程計座標系下的座標。
原始資料視覺化過程的座標轉換：
每個鐳射束的距離值和角度值——>每一束鐳射點在雷達座標系下的座標——>每一束鐳射點在lidar基準座標系下的座標——>顯示
從雷達座標系向基準座標系轉化時，轉換矩陣就是由這一幀的第一個鐳射點時間對應的機器人位姿確定的。
這一幀的每一束鐳射點都是這樣轉化顯示的，而實際上，隨著機器人的運動，機器人在里程計下的位姿時刻在變化，所以顯示出來的點雲會有畸變。
去畸變過程的座標轉換：
鐳射束的距離值和角度值-——>每一束鐳射點在雷達座標系下的座標——>每個鐳射束的點在里程計座標系下的座標——>每個鐳射點在基準座標系中的座標——>計算在基準座標系下的角度值和距離值
鐳射點的位姿從lidar座標系到odom座標系的轉換，插值得到機器人在里程計座標系中的轉換。
這樣每一束鐳射點都對應了一個轉換關係。沒有去畸變的時候，這個轉化關係是不變的，只是把所有的點在雷達座標系下的座標都按著這一幀的第一個鐳射點對應的轉化關係轉換的。這樣其實就是忽略了機器人在這一幀鐳射資料的時間內的運動問題。
就這個問題，說一下運動機器人座標變換。幾種座標系，map、odom、base_link。odom是全域性座標系。
程式碼
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl-1.8/pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <iostream>
#include <dirent.h>
#include <fstream>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Core>
class LidarMotionCalibrator
{
public:
LidarMotionCalibrator( tf::TransformListener* tf )
{
tf_ = tf;
scan_sub_ = nh_.subscribe("/scan", 10, &LidarMotionCalibrator::ScanCallBack, this);
points_pub_ = nh_.advertise<sensor_msgs::PointCloud>("/cloud", 10);
}
~LidarMotionCalibrator()
{
if(tf_!=NULL)
delete tf_;
}
// 處理原始資料的函式
void ScanCallBack(const sensor_msgs::LaserScanPtr &scan_msg)
{
//轉換到矯正需要的資料
ros::Time startTime, endTime;
startTime = scan_msg->header.stamp;
sensor_msgs::LaserScan laserScanMsg = *scan_msg;
// 計算當前幀資料的結束時間
int beamNum = laserScanMsg.ranges.size();
endTime = startTime + ros::Duration( laserScanMsg.time_increment * beamNum);
// 將資料複製出來(由於)
std::vector<double> angles,ranges;
double lidar_angle = laserScanMsg.angle_max;
for( int i = beamNum - 1; i > 0; i-- )
{
double lidar_dist = laserScanMsg.ranges[i];
// 計算每個距離值對應的角度資訊
lidar_angle -= laserScanMsg.angle_increment ;
if(lidar_dist < 0.05 || std::isnan(lidar_dist) || std::isinf(lidar_dist))
lidar_dist = 0.0;
ranges.push_back(lidar_dist);
angles.push_back(lidar_angle);
}
Lidar_Calibration(ranges, angles, startTime, endTime, tf_);
//轉換為pcl::pointcloud for visuailization
tf::Stamped<tf::Pose> visualPose;
if( !getLaserPose(visualPose, startTime, tf_))
{
ROS_WARN("Not visualPose,Can not Calib");
return ;
}
double visualYaw = tf::getYaw(visualPose.getRotation());
//轉換為 pointcloud
sensor_msgs::PointCloud cloud;
cloud.header.frame_id = "odom";
cloud.header.stamp = ros::Time::now();
cloud.points.resize( ranges.size() );
for (int i = 0; i < ranges.size(); i++)
{
if (ranges[i] < 0.05 || std::isnan(ranges[i]) || std::isinf(ranges[i]))
continue;
double x = ranges[i] * cos(angles[i]);
double y = ranges[i] * sin(angles[i]);
geometry_msgs::Point32 pt;
pt.x = x * cos(visualYaw) - y * sin(visualYaw) + visualPose.getOrigin().getX();
pt.y = x * sin(visualYaw) + y * cos(visualYaw) + visualPose.getOrigin().getY();
pt.z = 0.1;
cloud.points.push_back(pt);
}
points_pub_.publish(cloud);
}
/**
* @name getLaserPose()
* @brief 得到機器人在里程計座標系中的位姿tf::Pose
* 得到dt時刻鐳射雷達在odom座標系的位姿
* @param odom_pos 機器人的位姿
* @param dt dt時刻
* @param tf_
*/
bool getLaserPose(tf::Stamped<tf::Pose> &odom_pose,
ros::Time dt,
tf::TransformListener * tf_)
{
odom_pose.setIdentity();
tf::Stamped < tf::Pose > robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = "base_footprint";
robot_pose.stamp_ = dt; //設定為ros::Time()表示返回最近的轉換關係
// get the global pose of the robot
try
{
if ( !tf_->waitForTransform("/odom", "/base_footprint", dt, ros::Duration(0.5)) ) // 0.15s 的時間可以修改
{
ROS_ERROR("LidarMotion-Can not Wait Transform()");
return false;
}
tf_->transformPose("/odom", robot_pose, odom_pose);
}
catch (tf::LookupException& ex)
{
ROS_ERROR("LidarMotion: No Transform available Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR("LidarMotion: Connectivity Error looking up looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR("LidarMotion: Extrapolation Error looking up looking up robot pose: %s\n", ex.what());
return false;
}
return true;
}
/**
* @brief Lidar_MotionCalibration
* 鐳射雷達運動畸變去除分段函式;
* 在此分段函式中，認為機器人是勻速運動；
* @param frame_base_pose 標定完畢之後的基準座標系
* @param frame_start_pose 本分段第一個鐳射點對應的位姿
* @param frame_end_pose 本分段最後一個鐳射點對應的位姿
* @param ranges 鐳射資料－－距離
* @param angles 鐳射資料－－角度
* @param startIndex 本分段第一個鐳射點在鐳射幀中的下標
* @param beam_number 本分段的鐳射點數量
*/
void Lidar_MotionCalibration(
tf::Stamped<tf::Pose> frame_base_pose,
tf::Stamped<tf::Pose> frame_start_pose,
tf::Stamped<tf::Pose> frame_end_pose,
std::vector<double> &ranges,
std::vector<double> &angles,
int startIndex,
int &beam_number)
{
//每個位姿進行線性插值時的步長
double beam_step = 1.0 / (beam_number - 1);
//機器人的起始角度 和 最終角度
tf::Quaternion start_angle_q = frame_start_pose.getRotation();
tf::Quaternion end_angle_q = frame_end_pose.getRotation();
//轉換到弧度
double start_angle_r = tf::getYaw(start_angle_q);
double base_angle_r = tf::getYaw(frame_base_pose.getRotation());
//機器人的起始位姿
tf::Vector3 start_pos = frame_start_pose.getOrigin();
start_pos.setZ(0);
//最終位姿
tf::Vector3 end_pos = frame_end_pose.getOrigin();
end_pos.setZ(0);
//基礎座標系
tf::Vector3 base_pos = frame_base_pose.getOrigin();
base_pos.setZ(0);
double mid_angle;
tf::Vector3 mid_pos;
tf::Vector3 mid_point;
double lidar_angle, lidar_dist;
//插值計算出來每個點對應的位姿
for (int i = 0; i < beam_number; i++)
{
//角度插值
mid_angle = tf::getYaw(start_angle_q.slerp(end_angle_q, beam_step * i));
//線性插值
mid_pos = start_pos.lerp(end_pos, beam_step * i);
//得到鐳射點在odom 座標系中的座標 根據
double tmp_angle;
//如果鐳射雷達不等於無窮,則需要進行矯正.
if (tfFuzzyZero(ranges[startIndex + i]) == false)
{
//計算對應的鐳射點在odom座標系中的座標
//得到這幀鐳射束距離和夾角
lidar_dist = ranges[startIndex + i];
lidar_angle = angles[startIndex + i];
//鐳射雷達座標系下的座標
double laser_x, laser_y;
laser_x = lidar_dist * cos(lidar_angle);
laser_y = lidar_dist * sin(lidar_angle);
//里程計座標系下的座標
double odom_x, odom_y;
odom_x = laser_x * cos(mid_angle) - laser_y * sin(mid_angle) + mid_pos.x();
odom_y = laser_x * sin(mid_angle) + laser_y * cos(mid_angle) + mid_pos.y();
//轉換到型別中去
mid_point.setValue(odom_x, odom_y, 0);
//把在odom座標系中的鐳射資料點 轉換到 基礎座標系
double x0, y0, a0, s, c;
x0 = base_pos.x();
y0 = base_pos.y();
a0 = base_angle_r;
s = sin(a0);
c = cos(a0);
/*
* 把base轉換到odom 為[c -s x0;
* s c y0;
* 0 0 1]
* 把odom轉換到base為 [c s -x0*c-y0*s;
* -s c x0*s - y0*c;
* 0 0 1]
*/
double tmp_x, tmp_y;
tmp_x = mid_point.x() * c + mid_point.y() * s - x0 * c - y0 * s;
tmp_y = -mid_point.x() * s + mid_point.y() * c + x0 * s - y0 * c;
mid_point.setValue(tmp_x, tmp_y, 0);
//然後計算以起始座標為起點的 dist angle
double dx, dy;
dx = (mid_point.x());
dy = (mid_point.y());
lidar_dist = sqrt(dx * dx + dy * dy);
lidar_angle = atan2(dy, dx);
//鐳射雷達被矯正
ranges[startIndex + i] = lidar_dist;
angles[startIndex + i] = lidar_angle;
}
//如果等於無窮,則隨便計算一下角度
else
{
//鐳射角度
lidar_angle = angles[startIndex + i];
//里程計座標系的角度
tmp_angle = mid_angle + lidar_angle;
tmp_angle = tfNormalizeAngle(tmp_angle);
//如果資料非法 則只需要設定角度就可以了。把角度換算成start_pos座標系內的角度
lidar_angle = tfNormalizeAngle(tmp_angle - start_angle_r);
angles[startIndex + i] = lidar_angle;
}
}
}
//鐳射雷達資料 分段線性進行插值 分段的週期為10ms
//這裡會呼叫Lidar_MotionCalibration()
/**
* @name Lidar_Calibration()
* @brief 鐳射雷達資料 分段線性進行差值 分段的週期為5ms
* @param ranges 鐳射束的距離值集合
* @param angle 鐳射束的角度值集合
* @param startTime 第一束鐳射的時間戳
* @param endTime 最後一束鐳射的時間戳
* @param *tf_
*/
void Lidar_Calibration(std::vector<double> &ranges,
std::vector<double> &angles,
ros::Time startTime,
ros::Time endTime,
tf::TransformListener *tf_)
{
//統計鐳射束的數量
int beamNumber = ranges.size();
if (beamNumber != angles.size())
{
ROS_ERROR("Error:ranges not match to the angles");
return;
}
// 5ms來進行分段
int interpolation_time_duration = 5 * 1000;
tf::Stamped<tf::Pose> frame_start_pose;
tf::Stamped<tf::Pose> frame_mid_pose;
tf::Stamped<tf::Pose> frame_base_pose;
tf::Stamped<tf::Pose> frame_end_pose;
//起始時間 us
double start_time = startTime.toSec() * 1000 * 1000;
double end_time = endTime.toSec() * 1000 * 1000;
double time_inc = (end_time - start_time) / beamNumber; // 每束鐳射資料的時間間隔
//當前插值的段的起始座標
int start_index = 0;
//起始點的位姿 這裡要得到起始點位置的原因是 起始點就是我們的base_pose
//所有的鐳射點的基準位姿都會改成我們的base_pose
// ROS_INFO("get start pose");
if (!getLaserPose(frame_start_pose, ros::Time(start_time / 1000000.0), tf_))
{
ROS_WARN("Not Start Pose,Can not Calib");
return;
}
if (!getLaserPose(frame_end_pose, ros::Time(end_time / 1000000.0), tf_))
{
ROS_WARN("Not End Pose, Can not Calib");
return;
}
int cnt = 0;
//基準座標就是第一個位姿的座標
frame_base_pose = frame_start_pose;
for (int i = 0; i < beamNumber; i++)
{
//分段線性,時間段的大小為interpolation_time_duration
double mid_time = start_time + time_inc * (i - start_index);
if (mid_time - start_time > interpolation_time_duration || (i == beamNumber - 1))
{
cnt++;
//得到起點和終點的位姿
//終點的位姿
if (!getLaserPose(frame_mid_pose, ros::Time(mid_time / 1000000.0), tf_))
{
ROS_ERROR("Mid %d Pose Error", cnt);
return;
}
//對當前的起點和終點進行插值
//interpolation_time_duration中間有多少個點.
int interp_count = i - start_index + 1;
Lidar_MotionCalibration(frame_base_pose,
frame_start_pose,
frame_mid_pose,
ranges,
angles,
start_index,
interp_count);
//更新時間
start_time = mid_time;
start_index = i;
frame_start_pose = frame_mid_pose;
}
}
}
public:
tf::TransformListener* tf_;
ros::NodeHandle nh_;
ros::Subscriber scan_sub_;
ros::Publisher points_pub_;
};
int main(int argc,char ** argv)
{
ros::init(argc,argv,"LidarMotionCalib");
tf::TransformListener tf(ros::Duration(10.0));
LidarMotionCalibrator tmpLidarMotionCalib(&tf);
ros::spin();
return 0;
}