用插值的方法,從而得到每一個小時刻的位姿,從而去除運動畸變。
插值思想介紹
明確幾個物理量:
(1)start_time : 激光數據開始的時刻。
(2)end_time : endTime = startTime + ros::Duration(laserScanMsg.time_increment * beamNum); 激光數據結束的時刻。
(3)frame_start_pose : 雷達數據開始時刻,從odom獲取的start_time時刻的機器人位姿。
(4)frame_mid_pose : 中間某一幀雷達時刻,從odom獲取的mid_time時刻的機器人位姿。
(5)frame_end_pose : 雷達數據結束時刻,從odom獲取的end_time時刻的機器人位姿。
(6)frame_base_pose : 基準座標,就是將所有去畸變的電雲放在這個座標系下。這裏我們將第一個雷達數據所在位姿作爲基準座標。(基準位姿)
(7)interpolation_time_duration : 插值間隔。如果mid_time - start_time > interpolation_time_duration,則進行插值.
(8)interp_count : 在插值間隔中有幾個激光點。
座標轉換流程:
激光雷達座標系 --> 視覺里程計座標系 --> 基礎座標系
插值思想:
每次都選取中間的一個超過插值時間的激光時間段,起點爲start,終點爲middle,這個段的起點終點位姿由里程計獲取,可以用!tf_->waitForTransform("/odom", “/base_laser”, dt, ros::Duration(0.5)))等上一個,避免時間不同步的問題。對內部進行插值,有幾個點插值幾個,使得每個激光點都有屬於自己的位姿。
作業代碼
注意:
(1)需要安裝pcl。(我還改了pcl的搜索eigen的路徑。當然,大多數人不需要)
(2)原答案有bug。
一開始在第六十八行的時候爲:
if(!getLaserPose(visualPose, StartTime, tf_)) 這一句會報錯,因爲getLaserPose是要去等一個odom位姿,這個位姿是StartTime時刻而來的,而StartTime時刻爲數據包的起點,maybe是因爲時間延遲你永遠等不到這個時刻到來,而是直接過去了。
而這行代碼的含義是將激光雷達第一幀位姿作爲可視化位姿,也就是pcl點雲顯示的時候以這個座標系爲基準進行可視化。因此沒必要非得用第一幀數據的位姿,因此Time_debug表示用第二幀數據的位姿。
一開始確實會報錯,因爲還沒等到第二幀數據。
[ERROR] [1587482472.798144840, 1530887778.944067126]: LidarMotion-Can not Wait Transform()
[ WARN] [1587482472.798181747, 1530887778.944067126]: Not visualPose,Can not Calib
[ERROR] [1587482473.302531759, 1530887779.448513971]: LidarMotion-Can not Wait Transform()
[ WARN] [1587482473.302551750, 1530887779.448513971]: Not visualPose,Can not Calib
[ERROR] [1587482473.806249672, 1530887779.953135051]: LidarMotion-Can not Wait Transform()
[ WARN] [1587482473.806268484, 1530887779.953135051]: Not visualPose,Can not Calib
補充更新:並不是這麼解釋!!!!!
之所以等不到,是因爲在回放功能包的時候,必須加–clock關鍵字,否則程序無法運行。而不是之前說的那個問題。
原因:加上–clock關鍵字,使得功能包能夠發佈仿真時間(sim_time),從而與launch文件相呼應:
<launch>
<param name="use_sim_time" value="true"/>
一定要加關鍵字clock:
rosbag play --clock laser.bag
報錯減少
[ERROR] [1587483868.271416359, 1530887778.439721070]: LidarMotion-Can not Wait Transform()
[ WARN] [1587483868.271447715, 1530887778.439721070]: Not visualPose,Can not Calib
但是後來就不報錯了
-0.433416 -0.0370854
-0.431128 -0.027431
-0.4285 -0.0206992
-0.425851 -0.0112671
-0.421983 -0.00379708
-0.419985 0.00355133
-0.417903 0.00900548
-0.415609 0.0180333
-0.413341 0.0233592
-0.411938 0.0295971
-0.40829 0.0374057
-0.405288 0.0469622
-0.402425 0.0537635
-0.399579 0.0596078
-0.398331 0.0674416
-0.395447 0.0722904
-0.392796 0.0806986
-0.390352 0.0873235
-0.388203 0.0921946
。。。。。。。
附上代碼(深藍學院的課程實例代碼經過我debug)
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/LaserScan.h>
#include <champion_nav_msgs/ChampionNavLaserScan.h>
#include <pcl-1.7/pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl-1.7/pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <dirent.h>
#include <fstream>
#include <iostream>
pcl::visualization::CloudViewer g_PointCloudView("PointCloud View");
class LidarMotionCalibrator
{
public:
LidarMotionCalibrator(tf::TransformListener* tf)
{
tf_ = tf;
scan_sub_ = nh_.subscribe("champion_scan", 10, &LidarMotionCalibrator::ScanCallBack, this);
}
~LidarMotionCalibrator()
{
if(tf_!=NULL)
delete tf_;
}
// 拿到原始的激光數據來進行處理
void ScanCallBack(const champion_nav_msgs::ChampionNavLaserScanPtr& scan_msg)
{
//轉換到矯正需要的數據
ros::Time startTime, endTime;
startTime = scan_msg->header.stamp;
// cout<<startTime.useSystemTime()<<endl;
champion_nav_msgs::ChampionNavLaserScan laserScanMsg = *scan_msg;
//得到最終點的時間
int beamNum = laserScanMsg.ranges.size();
endTime = startTime + ros::Duration(laserScanMsg.time_increment * beamNum);
// 將數據複製出來
std::vector<double> angles,ranges;
for(int i = beamNum - 1; i > 0;i--)
{
double lidar_dist = laserScanMsg.ranges[i];
double lidar_angle = laserScanMsg.angles[i];
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);
}
//轉換爲pcl::pointcloud for visuailization
tf::Stamped<tf::Pose> visualPose;
ros::Time Time_debug = startTime + ros::Duration(laserScanMsg.time_increment * 1);
if(!getLaserPose(visualPose, Time_debug, tf_)) // Time_debug
{
ROS_WARN("Not visualPose,Can not Calib");
return ;
}
double visualYaw = tf::getYaw(visualPose.getRotation());
visual_cloud_.clear();
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]);
pcl::PointXYZRGB 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 = 1.0;
// pack r/g/b into rgb
unsigned char r = 255, g = 0, b = 0; //red color
unsigned int rgb = ((unsigned int)r << 16 | (unsigned int)g << 8 | (unsigned int)b);
pt.rgb = *reinterpret_cast<float*>(&rgb);
visual_cloud_.push_back(pt);
}
std::cout << std::endl;
//進行矯正
Lidar_Calibration(ranges,angles,
startTime,
endTime,
tf_);
//轉換爲pcl::pointcloud for visuailization
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]);
cout<<x<<" "<< y <<endl;
pcl::PointXYZRGB 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 = 1.0;
unsigned char r = 0, g = 255, b = 0; // green color
unsigned int rgb = ((unsigned int)r << 16 | (unsigned int)g << 8 | (unsigned int)b);
pt.rgb = *reinterpret_cast<float*>(&rgb);
visual_cloud_.push_back(pt);
}
//進行顯示
g_PointCloudView.showCloud(visual_cloud_.makeShared());
}
/**
* @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_laser";
robot_pose.stamp_ = dt; //設置爲ros::Time()表示返回最近的轉換關係
// get the global pose of the robot
try
{
// ROS_INFO("debug!!!");
if(!tf_->waitForTransform("/odom", "/base_laser", 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)
{
//TODO --------------------------------------------------------------------
// 每個位姿進行線性插值時的步長
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++)
{
// 角度插值// 球形插值。最後一個參數可以理解爲線性插值後取哪個位置的數字(0-1之間的0.25,就是值插值後的四分之一高度)
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;
}
}
//end of TODO
}
//激光雷達數據 分段線性進行插值 分段的週期爲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_;
pcl::PointCloud<pcl::PointXYZRGB> visual_cloud_;
};
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;
}