深蓝学院激光slam学习——第三章（激光slam运动畸变去除）习题
用插值的方法，从而得到每一个小时刻的位姿，从而去除运动畸变。
插值思想介绍
明确几个物理量：
（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文件相呼应：
一定要加关键字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
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
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< 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 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(&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<(&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 &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 frame_base_pose,
tf::Stamped frame_start_pose,
tf::Stamped frame_end_pose,
std::vector& ranges,
std::vector& 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 & ranges,
std::vector& 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 frame_start_pose;
tf::Stamped frame_mid_pose;
tf::Stamped frame_base_pose;
tf::Stamped 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 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;
}
最终效果
