安徽省建设厅网站,女生做网站运营好吗,wordpress更改注册地址,办公室装修设计怎么设计0. 简介
我们刚刚了解过DLIO的整个流程#xff0c;我们发现相比于Point-LIO而言#xff0c;这个方法更适合我们去学习理解#xff0c;同时官方给出的结果来看DLIO的结果明显好于现在的主流方法#xff0c;当然指的一提的是#xff0c;这个DLIO是必须需要六轴IMU的#x…0. 简介
我们刚刚了解过DLIO的整个流程我们发现相比于Point-LIO而言这个方法更适合我们去学习理解同时官方给出的结果来看DLIO的结果明显好于现在的主流方法当然指的一提的是这个DLIO是必须需要六轴IMU的所以如果没有IMU的画那只有DLO可以使用了。 1. getNextPose–通过IMU S2M GEO获取下一个姿态
下面的函数作用主要是获取下一个姿态的函数主要是通过IMU、S2M和GEO三种方式获取。首先检查新子地图是否准备就绪如果准备好了并且子地图发生了变化则将当前全局子地图设置为目标点云并设置子图的kdtree以及将目标云的法线设置为子地图的法线。接着使用全局IMU变换作为初始猜测将当前子地图与全局地图对齐并在全局坐标系中获取最终变换。然后更新下一个全局位姿并进行几何观察器更新。最终该函数返回下一个姿态。
/*** brief 通过IMU S2M GEO获取下一个姿态**/
void dlio::OdomNode::getNextPose() {// 检查新子地图是否准备好可供使用this-new_submap_is_ready (this-submap_future.wait_for(std::chrono::seconds(0)) std::future_status::ready); //等待子地图准备好if (this-new_submap_is_ready this-submap_hasChanged) { //如果子地图准备好了,并且子地图发生了变化// 将当前全局子地图设置为目标点云this-gicp.registerInputTarget(this-submap_cloud);// 设置子图的kdtree,之前就是直接从target_kdtree_拿出来的,这个有必要嘛?this-gicp.target_kdtree_ this-submap_kdtree;// 将目标云的法线设置为子地图的法线this-gicp.setTargetCovariances(this-submap_normals);this-submap_hasChanged false;}// 使用全局IMU变换作为初始猜测将当前子地图与全局地图对齐pcl::PointCloudPointType::Ptr aligned(boost::make_sharedpcl::PointCloudPointType());this-gicp.align(*aligned); // 设置对齐后的地图// 在全局坐标系中获取最终变换this-T_corr this-gicp.getFinalTransformation(); // 根据对齐后的地图来校准转换this-T this-T_corr * this-T_prior;// 更新下一个全局位姿,现在源点云和目标点云都在全局坐标系中所以变换是全局的this-propagateGICP();// 几何观察器更新this-updateState();
}2. updateState–更新GEO信息
这段代码是通过GEO更新状态的主要分为以下几个步骤
锁定线程以防止状态被PropagateState访问保证线程安全。获取机器人当前的位置、四元数和时间差。通过拿到的四元数和预测的四元数构造误差的四元数然后根据误差的四元数构建四元数校正量对应公式7。将误差的四元数转换到机器人的body坐标系下。更新加速度偏差和陀螺仪偏差同时限制偏差的最大值。更新机器人的速度和位置以及四元数同时存储前一个姿态、方向和速度。
void dlio::OdomNode::updateState() {// 锁定线程以防止状态被PropagateState访问std::lock_guardstd::mutex lock(this-geo.mtx);Eigen::Vector3f pin this-lidarPose.p; // 位置Eigen::Quaternionf qin this-lidarPose.q; // 四元数double dt this-scan_stamp - this-prev_scan_stamp; // 时间差Eigen::Quaternionf qe, qhat, qcorr;qhat this-state.q;// 构造误差的四元数qe qhat.conjugate() * qin; //通过拿到的四元数和预测的四元数构造误差的四元数double sgn 1.;if (qe.w() 0) { // 如果误差的四元数的w小于0sgn -1;}// 构建四元数校正量,对应公式7qcorr.w() 1 - abs(qe.w()); // 误差的四元数的w部分qcorr.vec() sgn * qe.vec(); // 误差的四元数的向量部分qcorr qhat * qcorr; // 误差的四元数Eigen::Vector3f err pin - this-state.p;Eigen::Vector3f err_body;err_body qhat.conjugate()._transformVector(err); // 误差的四元数转换到body坐标系下double abias_max this-geo_abias_max_;double gbias_max this-geo_gbias_max_;// 更新加速度偏差this-state.b.accel - dt * this-geo_Kab_ * err_body;this-state.b.accel this-state.b.accel.array().min(abias_max).max(-abias_max);// 更新陀螺仪偏差this-state.b.gyro[0] - dt * this-geo_Kgb_ * qe.w() * qe.x();this-state.b.gyro[1] - dt * this-geo_Kgb_ * qe.w() * qe.y();this-state.b.gyro[2] - dt * this-geo_Kgb_ * qe.w() * qe.z();this-state.b.gyro this-state.b.gyro.array().min(gbias_max).max(-gbias_max);// 更新速度和位置this-state.p dt * this-geo_Kp_ * err;this-state.v.lin.w dt * this-geo_Kv_ * err;this-state.q.w() dt * this-geo_Kq_ * qcorr.w();this-state.q.x() dt * this-geo_Kq_ * qcorr.x();this-state.q.y() dt * this-geo_Kq_ * qcorr.y();this-state.q.z() dt * this-geo_Kq_ * qcorr.z();this-state.q.normalize();// 存储前一个姿态、方向和速度this-geo.prev_p this-state.p;this-geo.prev_q this-state.q;this-geo.prev_vel this-state.v.lin.w;
}3. updateKeyframes–更新关键帧
在做完位置更新后就会更新关键帧通过计算当前姿态与轨迹中所有关键帧姿态的差异然后根据距离和旋转角度是否超过一定阈值来判断是否需要更新关键帧。具体实现过程如下
首先遍历轨迹中所有关键帧计算当前姿态与每个关键帧之间的距离并记录最近的关键帧的索引和距离。同时计算当前姿态附近的关键帧数量。
然后获取最近关键帧的姿态和旋转并计算当前姿态与最近关键帧之间的距离和旋转角度。如果旋转角度超过了设定的阈值或者距离超过了设定的阈值且附近关键帧数量小于等于1则认为需要更新关键帧。
最后如果需要更新关键帧则将当前姿态和点云、时间戳、法向量、变换矩阵等信息存储到关键帧向量中。
整个函数实现了关键帧的自适应更新可以在SLAM系统中提高精度和效率。
void dlio::OdomNode::updateKeyframes() {// 计算轨迹中所有姿态和旋转的差异float closest_d std::numeric_limitsfloat::infinity();int closest_idx 0;int keyframes_idx 0;int num_nearby 0;for (const auto k : this-keyframes) {// 计算当前姿态与关键帧中的姿态之间的距离,这里和更新submap的操作一样float delta_d sqrt(pow(this-state.p[0] - k.first.first[0], 2) pow(this-state.p[1] - k.first.first[1], 2) pow(this-state.p[2] - k.first.first[2], 2));//计算当前姿态附近的数量if (delta_d this-keyframe_thresh_dist_ * 1.5) {num_nearby;}// 将其存储到变量中if (delta_d closest_d) {closest_d delta_d; //最近的距离closest_idx keyframes_idx; //最近的关键帧的索引}keyframes_idx;}// 获取最接近的姿势和相应的旋转Eigen::Vector3f closest_pose this-keyframes[closest_idx].first.first; //最近的关键帧的位置Eigen::Quaternionf closest_pose_r this-keyframes[closest_idx].first.second; //最近的关键帧的旋转// 计算当前姿势与最近的姿势之间的距离,和closest_d一致float dd sqrt(pow(this-state.p[0] - closest_pose[0], 2) pow(this-state.p[1] - closest_pose[1], 2) pow(this-state.p[2] - closest_pose[2], 2));// 使用SLERP计算方向差异Eigen::Quaternionf dq;if (this-state.q.dot(closest_pose_r) 0.) { //如果两个四元数的点积小于0说明两个四元数的方向相反Eigen::Quaternionf lq closest_pose_r; //将最近的关键帧的旋转赋值给lqlq.w() * -1.;lq.x() * -1.;lq.y() * -1.;lq.z() * -1.;dq this-state.q * lq.inverse(); //计算当前姿态与最近的姿态之间的旋转} else {dq this-state.q *closest_pose_r.inverse(); //计算当前姿态与最近的姿态之间的旋转}double theta_rad 2. * atan2(sqrt(pow(dq.x(), 2) pow(dq.y(), 2) pow(dq.z(), 2)),dq.w()); //计算当前姿态与最近的姿态之间的旋转角度double theta_deg theta_rad * (180.0 / M_PI); //将弧度转换为角度// 更新关键帧bool newKeyframe false;if (abs(dd) this-keyframe_thresh_dist_ ||abs(theta_deg) this-keyframe_thresh_rot_) { //如果距离或者旋转角度超过阈值newKeyframe true;}if (abs(dd) this-keyframe_thresh_dist_ abs(theta_deg) this-keyframe_thresh_rot_ num_nearby 1) { //如果距离小于阈值但是旋转角度超过阈值且附近的关键帧数量小于等于1newKeyframe true;}if (abs(dd) this-keyframe_thresh_dist_) { //如果距离小于阈值newKeyframe false;} else if (abs(dd) 0.5) {newKeyframe false;}if (newKeyframe) {// 更新关键帧向量std::unique_lockdecltype(this-keyframes_mutex) lock(this-keyframes_mutex);this-keyframes.push_back(std::make_pair(std::make_pair(this-lidarPose.p, this-lidarPose.q),this-current_scan)); //将当前的姿态和点云压入到keyframes中this-keyframe_timestamps.push_back(this-scan_header_stamp); //将当前的时间戳压入到keyframe_timestamps中this-keyframe_normals.push_back(this-gicp.getSourceCovariances()); //将当前的法向量压入到keyframe_normals中this-keyframe_transformations.push_back(this-T_corr); //将当前的变换矩阵压入到keyframe_transformations中lock.unlock();}
}4. propagateState–传播GEO状态
然后我们整个基本都过完了只有一个IMU的feedback中漏掉的propagateState函数。通过IMU测量值来更新机器人的状态。首先通过一个mutex锁来确保线程安全。然后获取IMU测量的时间间隔并将当前状态下的四元数和角速度赋值给变量qhat和omega。接下来将机体坐标系下的加速度转换到世界坐标系下并使用加速度传播公式更新机器人的位置和速度。同时将机器人的角速度更新为世界坐标系下的角速度。然后使用重力计传播公式更新四元数确保其归一化。最后将机器人的角速度和四元数更新到状态中。
…详情请参照古月居
