0. 简介
我们刚刚了解过DLIO的整个流程,我们发现相比于Point-LIO而言,这个方法更适合我们去学习理解,同时官方给出的结果来看DLIO的结果明显好于现在的主流方法,当然指的一提的是,这个DLIO是必须需要六轴IMU的,所以如果没有IMU的画,那只有DLO可以使用了。
1. initializeDLIO–初始化参数
这段代码非常简单,主要作用是等待IMU数据的接收和校准,如果条件不满足则直接返回,否则将dlio_initialized变量设置为true表示初始化完成。其中,first_imu_received和imu_calibrated是两个布尔类型的变量,用于判断是否接收到IMU数据和是否已经校准。如果条件不满足,函数直接返回,不进行后续的初始化操作。
void dlio::OdomNode::initializeDLIO() {// Wait for IMUif (!this->first_imu_received ||!this->imu_calibrated) { //如果没有接收到IMU数据或者IMU没有校准return;}this->dlio_initialized = true; //初始化完成std::cout << std::endl << " DLIO initialized!" << std::endl;
}
2. getScanFromROS–ROS数据包转换与激光类型判断
这个函数也是比较简单的函数,将ROS消息类型的点云数据转换为PCL点云类型并进行预处理。函数首先将ROS消息类型的点云数据转换为PCL点云类型,并去除无效点。然后函数使用crop滤波器对点云进行框选范围内的点的筛选。接着,函数通过检测点云数据的字段来自动确定传感器类型,如果无法确定则将deskew_标志设置为false。最后,函数将原始点云数据保存到original_scan变量中,并将点云数据的时间戳保存到scan_header_stamp变量中。
void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr &pc) {pcl::PointCloud<PointType>::Ptr original_scan_(boost::make_shared<pcl::PointCloud<PointType>>());pcl::fromROSMsg(*pc, *original_scan_);// 去除无效点std::vector<int> idx;original_scan_->is_dense = false;pcl::removeNaNFromPointCloud(*original_scan_, *original_scan_, idx);// crop框选范围内的点this->crop.setInputCloud(original_scan_);this->crop.filter(*original_scan_);// 自动检测传感器类型this->sensor = dlio::SensorType::UNKNOWN;for (auto &field : pc->fields) {if (field.name == "t") {this->sensor = dlio::SensorType::OUSTER;break;} else if (field.name == "time") {this->sensor = dlio::SensorType::VELODYNE; // velodyne雷达break;}}if (this->sensor == dlio::SensorType::UNKNOWN) {this->deskew_ = false;}this->scan_header_stamp = pc->header.stamp;this->original_scan = original_scan_;
}
3. preprocessPoints—逐点去畸变与统一预积分
下面的代码对激光雷达点云数据进行预处理。首先,如果激光雷达可以逐点去畸变,将通过去畸变函数deskewPointcloud()进行处理,否则会使用统一预积分。如果是第一次处理扫描数据,需要等待IMU数据到达后才能进行处理。如果已经有了IMU数据,会通过IMU积分得到先验位姿,然后将原始点云转换到baselink坐标系下。接下来,如果开启了Voxel Grid Filter,则会对点云进行滤波处理,否则直接使用转换后的点云。最终,处理后的点云数据将作为当前激光雷达扫描的输入数据。
void dlio::OdomNode::preprocessPoints() {// 取消原始数据类型扫描if (this->deskew_) { //如果可以去畸变。也就是不为dlio::SensorType::UNKNOWNthis->deskewPointcloud(); //去畸变if (!this->first_valid_scan) {return;}} else {this->scan_stamp = this->scan_header_stamp.toSec();// 在IMU数据到达之前不要处理扫描数据if (!this->first_valid_scan) { // tips:这个和下面的deskewPointcloud函数中判断类似if (this->imu_buffer.empty() ||this->scan_stamp <= this->imu_buffer.back().stamp) {return;}this->first_valid_scan = true;this->T_prior = this->T; // 假设第一次扫描没有运动} else {// 第二个及以后的扫描的IMU先验std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>frames;frames = this->integrateImu(this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p,this->geo.prev_vel.cast<float>(), {this->scan_stamp}); // IMU积分if (frames.size() > 0) {this->T_prior = frames.back();} else {this->T_prior = this->T;}}pcl::PointCloud<PointType>::Ptr deskewed_scan_(boost::make_shared<pcl::PointCloud<PointType>>()); //创建一个点云指针pcl::transformPointCloud(*this->original_scan, *deskewed_scan_,this->T_prior *this->extrinsics.baselink2lidar_T); //将原始点云转换到baselink坐标系下this->deskewed_scan = deskewed_scan_;this->deskew_status = false;}// Voxel Grid Filterif (this->vf_use_) {pcl::PointCloud<PointType>::Ptr current_scan_(boost::make_shared<pcl::PointCloud<PointType>>(*this->deskewed_scan)); //创建一个点云指针this->voxel.setInputCloud(current_scan_); //设置输入点云this->voxel.filter(*current_scan_); //滤波this->current_scan = current_scan_;} else {this->current_scan = this->deskewed_scan;}
}
4. deskewPointcloud–雷达逐点预积分
这个函数的作用是去除点云数据的畸变,使得点云数据的时间戳相对于一个参考时间戳。该函数的输入是一个原始的点云数据,输出是一个去畸变的点云数据。
首先,该函数创建了一个pcl::PointCloud::Ptr类型的指针deskewed_scan_,用于存储去畸变后的点云数据。然后,该函数按照时间戳对点进行排序并构建时间戳列表。具体实现是通过定义比较函数point_time_cmp和不等于函数point_time_neq,将点按照时间戳排序。如果传感器类型是OUSTER,则时间戳存储在点的t字段中;否则,时间戳存储在点的time字段中。extract_point_time函数用于从点中提取时间戳。
接下来,该函数将点按照时间戳的顺序复制到deskewed_scan_中。此处使用std::partial_sort_copy函数,将原始点云按照时间戳排序后存储在deskewed_scan_中。
随后,该函数调用了一个名为points_unique_timestamps的函数。这个函数的作用是从deskewed_scan_点云数据中提取出时间戳不相同的点,并将它们存储在一个名为points_unique_timestamps的变量中。这个函数使用了Boost库中的adaptors,首先使用indexed()将点序号和点本身组合成一个pair,然后使用adjacent_filtered()过滤掉时间戳相邻的相同点,最终得到时间戳不相同的点。
接下来,该函数从点中提取时间戳并将它们放入一个独立的列表中。unique_time_indices存储了每个时间戳的点云在deskewed_scan_中的起始和终止索引。median_pt_index为时间戳列表的中位数。
然后,该函数检查IMU数据是否已经到达,如果没有到达,则不处理扫描数据。如果传感器类型为OUSTER,则将原始点云转换到baselink坐标系下。然后,将T_prior设置为T,将原始点云转换到baselink坐标系下,并将deskewed_scan_设为原始点云。
如果IMU数据已经到达,则使用IMU先验和去斜校正。该函数调用了integrateImu函数,该函数使用IMU数据对lidar的运动进行积分,传入timestamps这个vector来获取每个点时间的IMU积分。如果积分成功,则时间戳的数量应该等于timestamps.size()。
void dlio::OdomNode::deskewPointcloud() {pcl::PointCloud<PointType>::Ptr deskewed_scan_(boost::make_shared<pcl::PointCloud<PointType>>());deskewed_scan_->points.resize(this->original_scan->points.size()); //设置点云大小// 各个点的时间戳应该相对于此时间double sweep_ref_time = this->scan_header_stamp.toSec();// 按时间戳对点进行排序并构建时间戳列表std::function<bool(const PointType &, const PointType &)>point_time_cmp; //比较函数std::function<bool(boost::range::index_value<PointType &, long>,boost::range::index_value<PointType &, long>)>point_time_neq; //不等于函数std::function<double(boost::range::index_value<PointType &, long>)>extract_point_time; //提取时间if (this->sensor == dlio::SensorType::OUSTER) {point_time_cmp = [](const PointType &p1, const PointType &p2) {return p1.t < p2.t;}; //定义内容point_time_neq = [](boost::range::index_value<PointType &, long> p1,boost::range::index_value<PointType &, long> p2) {return p1.value().t != p2.value().t;};extract_point_time =[&sweep_ref_time](boost::range::index_value<PointType &, long> pt) {return sweep_ref_time + pt.value().t * 1e-9f;};} else {point_time_cmp = [](const PointType &p1, const PointType &p2) {return p1.time < p2.time;};point_time_neq = [](boost::range::index_value<PointType &, long> p1,boost::range::index_value<PointType &, long> p2) {return p1.value().time != p2.value().time;};extract_point_time =[&sweep_ref_time](boost::range::index_value<PointType &, long> pt) {return sweep_ref_time + pt.value().time;};}// 按照时间戳的顺序将点复制到deskewed_scan_中std::partial_sort_copy(this->original_scan->points.begin(),this->original_scan->points.end(),deskewed_scan_->points.begin(),deskewed_scan_->points.end(), point_time_cmp);// 这个函数的作用是从deskewed_scan_点云数据中提取出时间戳不相同的点,并将它们存储在一个名为points_unique_timestamps的变量中。// 这个函数使用了Boost库中的adaptors,首先使用indexed()将点序号和点本身组合成一个pair,然后使用adjacent_filtered()过滤掉时间戳相邻的相同点,最终得到时间戳不相同的点。auto points_unique_timestamps =deskewed_scan_->points | boost::adaptors::indexed() |boost::adaptors::adjacent_filtered(point_time_neq);// 从点中提取时间戳并将它们放入一个独立的列表中std::vector<double> timestamps;std::vector<int> unique_time_indices;for (auto it = points_unique_timestamps.begin();it != points_unique_timestamps.end(); it++) {timestamps.push_back(extract_point_time(*it));unique_time_indices.push_back(it->index());}unique_time_indices.push_back(deskewed_scan_->points.size()); //最后一个存点的个数int median_pt_index = timestamps.size() / 2;this->scan_stamp =timestamps[median_pt_index]; // 将this->scan_stamp设置为中位点的时间戳//在IMU数据到达之前不要处理扫描数据if (!this->first_valid_scan) {if (this->imu_buffer.empty() ||this->scan_stamp <= this->imu_buffer.back().stamp) {return;}this->first_valid_scan = true;this->T_prior = this->T; // 假设第一次扫描时没有运动pcl::transformPointCloud(*deskewed_scan_, *deskewed_scan_,this->T_prior * this->extrinsics.baselink2lidar_T);this->deskewed_scan = deskewed_scan_;this->deskew_status = true;return;}//从第二次扫描开始,使用IMU先验和去斜校正std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>frames;frames = this->integrateImu(this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p,this->geo.prev_vel.cast<float>(),timestamps); // IMU积分,传入timestamps这个vector来获取每个点时间的IMU积分this->deskew_size =frames.size(); // 如果积分成功,则时间戳的数量应该等于timestamps.size()// 如果扫描开始和结束之间没有帧,则可能意味着存在同步问题if (frames.size() != timestamps.size()) {ROS_FATAL("Bad time sync between LiDAR and IMU!");this->T_prior = this->T; //那直接将T_prior设置为Tpcl::transformPointCloud(*deskewed_scan_, *deskewed_scan_,this->T_prior *this->extrinsics.baselink2lidar_T); //将原始点云转换到baselink坐标系下this->deskewed_scan = deskewed_scan_;this->deskew_status = false;return;}// 将先验更新为扫描中间时间的估计姿态(对应于this->scan_stamp)this->T_prior = frames[median_pt_index];#pragma omp parallel for num_threads(this->num_threads_) //并行计算for (int i = 0; i < timestamps.size(); i++) {Eigen::Matrix4f T =frames[i] * this->extrinsics.baselink2lidar_T; //设置变换矩阵// transform point to world framefor (int k = unique_time_indices[i]; k < unique_time_indices[i + 1]; k++) {auto &pt = deskewed_scan_->points[k]; //取出点云中的点pt.getVector4fMap()[3] = 1.; //将点云中的点转换为齐次坐标pt.getVector4fMap() =T * pt.getVector4fMap(); //将点云中的点转换到baselink坐标系下}}this->deskewed_scan = deskewed_scan_;this->deskew_status = true;
}
4. integrateImu—IMU积分与点云去畸变入口
这个代码主要用来对IMU数据进行积分,得到每个点的IMU积分。首先,函数接受了一些参数,包括上一帧扫描的时间戳、上一帧估算的姿态、位置和速度,以及当前帧对应点(帧)扫描的时间戳。然后,函数会检查时间戳是否为空或者时间戳大于第一个时间戳,如果是,则返回一个空向量。接着,函数会从IMU数据中提取出时间戳不相同的点,如果IMU测量不足,则返回一个空向量。
接下来,函数会通过反向整合,找到第一个IMU样本的姿态,并计算两个IMU样本之间的时间间隔。然后,函数会根据公式4克罗内克积,推算出开始时间对应的姿态。接着,函数会计算第一和第二个IMU样本之间的平均角速度,并根据公式4计算出第二个惯性测量单元样本的方向。然后,函数会计算第一个和第二个IMU样本之间的加速度,并减去重力加速度。接着,函数会计算在前两个IMU采样之间的jerk,并根据公式4计算出第一个IMU样本的速度和位置。最后,函数会调用integrateImuInternal函数,对IMU数据进行积分,并返回一个包含每个点IMU积分的向量。
/*** @brief* 这个函数主要是用来处理IMU数据的,主要是对IMU数据进行积分,得到每个点的IMU积分** @param start_time 上一帧扫描的时间戳* @param q_init 上一帧估算的姿态* @param p_init 上一帧估算的位置* @param v_init 上一帧估算的速度* @param sorted_timestamps 当前帧对应点(帧)扫描的时间戳* @return std::vector<Eigen::Matrix4f,* Eigen::aligned_allocator<Eigen::Matrix4f>>*/
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init,Eigen::Vector3f p_init, Eigen::Vector3f v_init,const std::vector<double> &sorted_timestamps) {const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>empty;if (sorted_timestamps.empty() || start_time > sorted_timestamps.front()) {// 如果时间戳为空或者时间戳大于第一个时间戳,认为无效的输入,返回空向量return empty;}boost::circular_buffer<ImuMeas>::reverse_iterator begin_imu_it;boost::circular_buffer<ImuMeas>::reverse_iterator end_imu_it;if (this->imuMeasFromTimeRange(start_time, sorted_timestamps.back(),begin_imu_it, end_imu_it) == false) {//从IMU数据中提取出时间戳不相同的点,IMU测量不足,返回空向量。return empty;}// 反向整合以找到第一个IMU样本的姿态const ImuMeas &f1 = *begin_imu_it;const ImuMeas &f2 = *(begin_imu_it + 1);// 两个IMU样本之间的时间double dt = f2.dt;// 第一个IMU样本和开始时间之间的时间间隔double idt = start_time - f1.stamp;//第一和第二个IMU样本之间的角加速度Eigen::Vector3f alpha_dt = f2.ang_vel - f1.ang_vel;Eigen::Vector3f alpha = alpha_dt / dt;// 首个IMU样本和起始时间之间的平均角速度(反向)Eigen::Vector3f omega_i = -(f1.ang_vel + 0.5 * alpha * idt);// 将q_init设置为第一个IMU样本的方向q_init = Eigen::Quaternionf(q_init.w() - 0.5 *(q_init.x() * omega_i[0] + q_init.y() * omega_i[1] +q_init.z() * omega_i[2]) *idt,q_init.x() + 0.5 *(q_init.w() * omega_i[0] - q_init.z() * omega_i[1] +q_init.y() * omega_i[2]) *idt,q_init.y() + 0.5 *(q_init.z() * omega_i[0] + q_init.w() * omega_i[1] -q_init.x() * omega_i[2]) *idt,q_init.z() +0.5 *(q_init.x() * omega_i[1] - q_init.y() * omega_i[0] +q_init.w() * omega_i[2]) *idt); //根据第一个IMU推算出开始时间,对应公式4克罗内克积,省略了后面一项q_init.normalize();// 第一和第二个IMU样本之间的平均角速度Eigen::Vector3f omega = f1.ang_vel + 0.5 * alpha_dt;// 第二个惯性测量单元样本的方向Eigen::Quaternionf q2(q_init.w() - 0.5 *(q_init.x() * omega[0] + q_init.y() * omega[1] +q_init.z() * omega[2]) *dt,q_init.x() + 0.5 *(q_init.w() * omega[0] - q_init.z() * omega[1] +q_init.y() * omega[2]) *dt,q_init.y() + 0.5 *(q_init.z() * omega[0] + q_init.w() * omega[1] -q_init.x() * omega[2]) *dt,q_init.z() + 0.5 *(q_init.x() * omega[1] - q_init.y() * omega[0] +q_init.w() * omega[2]) *dt);q2.normalize();// 首个IMU样本的加速度Eigen::Vector3f a1 =q_init._transformVector(f1.lin_accel); //根据线加速度求出f1时刻的加速度a1[2] -= this->gravity_; //减去重力加速度// 第二个IMU样本的加速度Eigen::Vector3f a2 = q2._transformVector(f2.lin_accel);a2[2] -= this->gravity_;// 在前两个IMU采样之间的jerkEigen::Vector3f j = (a2 - a1) / dt;// 将 v_init 设置为第一个IMU样本的速度(从 start_time 开始向后倒退),公式4v_init -= a1 * idt + 0.5 * j * idt * idt;//将p_init设置为第一个IMU样本的位置(从start_time往回走,公式4p_init -=v_init * idt + 0.5 * a1 * idt * idt + (1 / 6.) * j * idt * idt * idt;return this->integrateImuInternal(q_init, p_init, v_init, sorted_timestamps,begin_imu_it, end_imu_it);
}
5. imuMeasFromTimeRange–根据时间范围获取IMU测量值
下面的代码根据给定的时间范围获取IMU测量值。它接受起始时间和结束时间,以及一个IMU缓冲区的起始和结束迭代器。该函数首先检查IMU缓冲区是否为空或第一个IMU数据的时间戳是否小于结束时间。如果是,则等待最新的IMU数据。然后,函数使用迭代器找到最后一个时间戳小于结束时间的IMU数据,以及第一个时间戳大于等于开始时间的IMU数据。最后,该函数将找到的IMU数据存储在反向迭代器中,并返回true。如果找不到IMU数据,则返回false。
/*** @brief 根据时间范围获取IMU测量值** @param start_time 需要的IMU测量值的开始时间* @param end_time 需要的IMU测量值的结束时间* @param begin_imu_it 对应的imu_buffer的起始索引* @param end_imu_it 对应的imu_buffer的结束索引* @return true* @return false*/
bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time,boost::circular_buffer<ImuMeas>::reverse_iterator &begin_imu_it,boost::circular_buffer<ImuMeas>::reverse_iterator &end_imu_it) {//如果IMU缓冲区为空或者IMU缓冲区的第一个IMU数据的时间戳小于end_timeif (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) {// 等待最新的IMU数据std::unique_lock<decltype(this->mtx_imu)> lock(this->mtx_imu); //互斥锁this->cv_imu_stamp.wait(lock, [this, &end_time] {return this->imu_buffer.front().stamp >= end_time; //等待最新的IMU数据}); //等待最新的IMU数据}auto imu_it =this->imu_buffer.begin(); // IMU数据的迭代器,直到IMU数据要在end_time之后auto last_imu_it = imu_it; //设置最新的imu作为最后的imuimu_it++;while (imu_it != this->imu_buffer.end() && imu_it->stamp >= end_time) {last_imu_it = imu_it; //不断迭代拿到之前时间的IMU数据imu_it++;}while (imu_it != this->imu_buffer.end() && imu_it->stamp >= start_time) {imu_it++; //不断迭代拿到之前时间的IMU数据,并拿到imu_it和last_imu_it之间的IMU数据}if (imu_it == this->imu_buffer.end()) {// 测量不足,返回false,因为没有值在end_time之后return false;}imu_it++;// 设置反向迭代器(以正向时间迭代)end_imu_it = boost::circular_buffer<ImuMeas>::reverse_iterator(last_imu_it);begin_imu_it = boost::circular_buffer<ImuMeas>::reverse_iterator(imu_it);return true;
}
6. integrateImuInternal–单点去畸变
下面码实现了一个IMU积分方法,可以根据所有的点以及IMU时间完成计算,并对其进行插值,以便在任何时间点都可以获得姿态。该方法接受初始姿态、位置和速度、所有点的时间戳以及IMU测量值和时间戳等参数,并返回一个表示IMU测量值对应的位姿变换矩阵的向量。
在代码中,首先对初始姿态、位置和速度进行初始化,并计算出初始加速度。然后遍历IMU测量值和时间戳,根据公式4计算四元数,并更新加速度、角加速度和平均角速度等参数。随后,根据给定的时间戳进行插值,计算出对应的四元数和位置,并将其转换为位姿变换矩阵的形式,存储在一个向量中。最后,更新位置和速度,循环直到遍历完所有的IMU测量值和时间戳。
该方法的主要思路是根据IMU测量值和时间戳计算出对应的姿态和位置信息,并进行插值,以便在任何时间点都可以获得姿态。该方法的优点是可以提高定位的精度和鲁棒性,尤其是在存在运动模糊或者缺失数据等情况下。但是该方法也存在一些缺点,例如需要较高的计算资源和较高的计算精度,同时还需要对IMU测量误差进行校准和补偿,否则可能会导致积分误差的累积。