马上注册，结交更多好友，享用更多功能，让你轻松玩转社区。
您需要 登录 才可以下载或查看，没有账号？立即注册
x
1.imuHandler
imuHandler 函数用于处理IMU（惯性丈量单元）数据，通过积分IMU丈量值预测当前的位姿与速度，并将IMU位姿转换到激光雷达坐标系，最终发布惯导里程计数据。
- 接收并处理IMU丈量值；
- 对IMU数据进行积分，预测当前位姿与速度；
- 将IMU位姿转换到激光雷达坐标系；
- 发布激光雷达坐标系下的惯导里程计。
- // IMU 数据处理回调函数
- void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
- {
- // 加锁，确保线程安全
- std::lock_guard<std::mutex> lock(mtx);
- // 转换原始 IMU 数据
- sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
- // 将转换后的 IMU 数据加入优化队列和 IMU 队列
- imuQueOpt.push_back(thisImu);
- imuQueImu.push_back(thisImu);
- // 如果尚未完成第一次优化，则退出函数
- if (doneFirstOpt == false)
- return;
- // 获取当前 IMU 数据的时间戳
- double imuTime = ROS_TIME(&thisImu);
-
- // 计算时间间隔 dt
- double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
- lastImuT_imu = imuTime;
- // 积分当前 IMU 测量数据
- imuIntegratorImu_->integrateMeasurement(
- gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
- gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z),
- dt
- );
- // 使用积分后的状态预测当前的导航状态
- gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
- // 创建并初始化里程计消息
- nav_msgs::Odometry odometry;
- odometry.header.stamp = thisImu.header.stamp;
- odometry.header.frame_id = odometryFrame;
- odometry.child_frame_id = "odom_imu";
- // 将 IMU 位姿转换到激光雷达坐标系
- gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
- gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
- // 填充里程计位置信息
- odometry.pose.pose.position.x = lidarPose.translation().x();
- odometry.pose.pose.position.y = lidarPose.translation().y();
- odometry.pose.pose.position.z = lidarPose.translation().z();
- odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
- odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
- odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
- odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
-
- // 填充里程计速度信息
- odometry.twist.twist.linear.x = currentState.velocity().x();
- odometry.twist.twist.linear.y = currentState.velocity().y();
- odometry.twist.twist.linear.z = currentState.velocity().z();
- odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
- odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
- odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
-
- // 发布里程计消息
- pubImuOdometry.publish(odometry);
- }
IMU数据包含线加速度 a imu \mathbf{a}_{\text{imu}} aimu 和角速度 ω imu \boldsymbol{\omega}_{\text{imu}} ωimu，函数通过GTSAM库的 integrateMeasurement 方法对IMU丈量值进行积分，公式为：
v k + 1 = v k + ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t , p k + 1 = p k + v k ⋅ Δ t + 1 2 ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t 2 , R k + 1 = R k ⋅ exp ( ( ω imu − b ω ) ⋅ Δ t ) , \begin{aligned} \mathbf{v}_{k+1} &= \mathbf{v}_k + (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t, \\ \mathbf{p}_{k+1} &= \mathbf{p}_k + \mathbf{v}_k \cdot \Delta t + \frac{1}{2} (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t^2, \\ \mathbf{R}_{k+1} &= \mathbf{R}_k \cdot \exp((\boldsymbol{\omega}_{\text{imu}} - \mathbf{b}_\omega) \cdot \Delta t), \end{aligned} vk+1pk+1Rk+1=vk+(Rk⋅(aimu−ba)+g)⋅Δt,=pk+vk⋅Δt+21(Rk⋅(aimu−ba)+g)⋅Δt2,=Rk⋅exp((ωimu−bω)⋅Δt),
此中：
- v k \mathbf{v}_k vk 和 p k \mathbf{p}_k pk 分别为速度和位置；
- R k \mathbf{R}_k Rk 为姿态的旋转矩阵；
- b a \mathbf{b}_a ba 和 b ω \mathbf{b}_\omega bω 分别为加速度和角速度的偏置；
- g \mathbf{g} g 为重力加速度；
- Δ t \Delta t Δt 为时间隔断。
2. 位姿预测
通过上述积分盘算，预测得到当前状态：
State k + 1 = [ p k + 1 v k + 1 R k + 1 ] . \text{State}_{k+1} = \begin{bmatrix} \mathbf{p}_{k+1} \\ \mathbf{v}_{k+1} \\ \mathbf{R}_{k+1} \end{bmatrix}. Statek+1= pk+1vk+1Rk+1 .
函数中调用 imuIntegratorImu_->predict 实现此预测。
3. 位姿转换（IMU到激光雷达坐标系）
通过以下公式将IMU的位姿 T imu \mathbf{T}_{\text{imu}} Timu 转换为激光雷达位姿 T lidar \mathbf{T}_{\text{lidar}} Tlidar：
T lidar = T imu ⋅ T imu2lidar , \mathbf{T}_{\text{lidar}} = \mathbf{T}_{\text{imu}} \cdot \mathbf{T}_{\text{imu2lidar}}, Tlidar=Timu⋅Timu2lidar,
此中 T imu2lidar \mathbf{T}_{\text{imu2lidar}} Timu2lidar 是IMU到激光雷达的静态外参。
对于位姿 T \mathbf{T} T，其包含旋转 R \mathbf{R} R 宁静移 t \mathbf{t} t，即：
T = [ R t 0 1 ] . \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}. T=[R0t1].
4. 惯导里程计发布
最终通过以下公式将预测得到的激光雷达位姿和速度发布为惯导里程计数据：
- 位置：
p odom = T lidar . translation() . \mathbf{p}_{\text{odom}} = \mathbf{T}_{\text{lidar}}.\text{translation()}. podom=Tlidar.translation().
- 姿态：
q odom = T lidar . rotation() . \mathbf{q}_{\text{odom}} = \mathbf{T}_{\text{lidar}}.\text{rotation()}. qodom=Tlidar.rotation().
- 线速度：
v odom = v k + 1 . \mathbf{v}_{\text{odom}} = \mathbf{v}_{k+1}. vodom=vk+1.
- 角速度：
ω odom = ω imu + b ω . \boldsymbol{\omega}_{\text{odom}} = \boldsymbol{\omega}_{\text{imu}} + \mathbf{b}_\omega. ωodom=ωimu+bω.
2. odometryHandler
odometryHandler 函数的重要功能是接收激光里程计数据，结合IMU预积分对系统状态进行优化，并更新系统的位姿、速度和偏置。以下为具体的数学过程。
- 里程计与IMU初始对齐，构建优化图；
- 集成IMU数据，构建IMU因子；
- 添加里程计因子，调用优化器求解；
- 提取优化效果，更新系统状态。
- void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
- {
- std::lock_guard<std::mutex> lock(mtx); // 保护共享资源，防止多线程访问时出现竞争条件
- double currentCorrectionTime = ROS_TIME(odomMsg); // 获取当前里程计消息的时间戳
- // 确保有IMU数据可供积分
- if (imuQueOpt.empty())
- return;
- // 从里程计消息中提取位置和姿态信息
- float p_x = odomMsg->pose.pose.position.x;
- float p_y = odomMsg->pose.pose.position.y;
- float p_z = odomMsg->pose.pose.position.z;
- float r_x = odomMsg->pose.pose.orientation.x;
- float r_y = odomMsg->pose.pose.orientation.y;
- float r_z = odomMsg->pose.pose.orientation.z;
- float r_w = odomMsg->pose.pose.orientation.w;
- bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false; // 检查是否为退化情况
- gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z)); // 构造激光雷达的位姿
- // 0. 初始化系统
- if (systemInitialized == false)
- {
- resetOptimization(); // 重置优化器
- // 弹出旧的IMU消息
- while (!imuQueOpt.empty())
- {
- if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
- {
- lastImuT_opt = ROS_TIME(&imuQueOpt.front());
- imuQueOpt.pop_front();
- }
- else
- break;
- }
- // 初始位姿
- prevPose_ = lidarPose.compose(lidar2Imu); // 将激光雷达位姿转换为IMU位姿
- gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise); // 添加初始位姿先验因子
- graphFactors.add(priorPose);
- // 初始速度
- prevVel_ = gtsam::Vector3(0, 0, 0); // 初始速度为零
- gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise); // 添加初始速度先验因子
- graphFactors.add(priorVel);
- // 初始偏差
- prevBias_ = gtsam::imuBias::ConstantBias(); // 初始偏差为零
- gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise); // 添加初始偏差先验因子
- graphFactors.add(priorBias);
- // 添加初始值
- graphValues.insert(X(0), prevPose_);
- graphValues.insert(V(0), prevVel_);
- graphValues.insert(B(0), prevBias_);
- // 优化一次
- optimizer.update(graphFactors, graphValues);
- graphFactors.resize(0);
- graphValues.clear();
- imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_); // 重置IMU积分器并设置偏差
- imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
- key = 1;
- systemInitialized = true; // 标记系统已初始化
- return;
- }
- // 重置图以提高速度
- if (key == 100)
- {
- // 获取重置前的噪声模型
- gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
- gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
- gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
- // 重置图
- resetOptimization();
- // 添加位姿
- gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
- graphFactors.add(priorPose);
- // 添加速度
- gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
- graphFactors.add(priorVel);
- // 添加偏差
- gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
- graphFactors.add(priorBias);
- // 添加值
- graphValues.insert(X(0), prevPose_);
- graphValues.insert(V(0), prevVel_);
- graphValues.insert(B(0), prevBias_);
- // 优化一次
- optimizer.update(graphFactors, graphValues);
- graphFactors.resize(0);
- graphValues.clear();
- key = 1;
- }
- // 1. 积分IMU数据并优化
- while (!imuQueOpt.empty())
- {
- // 弹出并积分两个优化之间的IMU数据
- sensor_msgs::Imu *thisImu = &imuQueOpt.front();
- double imuTime = ROS_TIME(thisImu);
- if (imuTime < currentCorrectionTime - delta_t)
- {
- double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
- imuIntegratorOpt_->integrateMeasurement(
- gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
- gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
-
- lastImuT_opt = imuTime;
- imuQueOpt.pop_front();
- }
- else
- break;
- }
- // 向图中添加IMU因子
- const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
- gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
- graphFactors.add(imu_factor);
- // 添加IMU偏差之间的因子
- graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
- gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
- // 添加位姿因子
- gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
- gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
- graphFactors.add(pose_factor);
- // 插入预测值
- gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
- graphValues.insert(X(key), propState_.pose());
- graphValues.insert(V(key), propState_.v());
- graphValues.insert(B(key), prevBias_);
- // 优化
- optimizer.update(graphFactors, graphValues);
- optimizer.update();
- graphFactors.resize(0);
- graphValues.clear();
- // 为下一步重置预积分
- gtsam::Values result = optimizer.calculateEstimate();
- prevPose_ = result.at<gtsam::Pose3>(X(key));
- prevVel_ = result.at<gtsam::Vector3>(V(key));
- prevState_ = gtsam::NavState(prevPose_, prevVel_);
- prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
- // 重置优化预积分对象
- imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
- // 检查优化是否失败
- if (failureDetection(prevVel_, prevBias_))
- {
- resetParams();
- return;
- }
- // 2. 优化后，重新传播IMU里程计预积分
- prevStateOdom = prevState_;
- prevBiasOdom = prevBias_;
- // 首先弹出比当前校正数据旧的IMU消息
- double lastImuQT = -1;
- while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
- {
- lastImuQT = ROS_TIME(&imuQueImu.front());
- imuQueImu.pop_front();
- }
- // 重新传播
- if (!imuQueImu.empty())
- {
- // 使用新优化的偏差重置偏差
- imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
- // 从本次优化的开始处积分IMU消息
- for (int i = 0; i < (int)imuQueImu.size(); ++i)
- {
- sensor_msgs::Imu *thisImu = &imuQueImu[i];
- double imuTime = ROS_TIME(thisImu);
- double dt = (lastImuQT < 0) ? (1.0 / 500.0) : (imuTime - lastImuQT);
- imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
- gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
- lastImuQT = imuTime;
- }
- }
- ++key;
- doneFirstOpt = true;
- }
接收到的里程计位姿由以下参数表现：
- 位置： p lidar = [ p x , p y , p z ] T \mathbf{p}_{\text{lidar}} = [p_x, p_y, p_z]^T plidar=[px,py,pz]T，
- 姿态：四元数 q lidar = [ r w , r x , r y , r z ] T \mathbf{q}_{\text{lidar}} = [r_w, r_x, r_y, r_z]^T qlidar=[rw,rx,ry,rz]T。
激光雷达的位姿用 GTSAM 表现为：
T lidar = [ R lidar p lidar 0 1 ] , \mathbf{T}_{\text{lidar}} = \begin{bmatrix} \mathbf{R}_{\text{lidar}} & \mathbf{p}_{\text{lidar}} \\ \mathbf{0} & 1 \end{bmatrix}, Tlidar=[Rlidar0plidar1],
此中 R lidar \mathbf{R}_{\text{lidar}} Rlidar 由四元数 q lidar \mathbf{q}_{\text{lidar}} qlidar 转换得到。
激光雷达位姿转换为IMU位姿：
T imu = T lidar ⋅ T lidar2imu , \mathbf{T}_{\text{imu}} = \mathbf{T}_{\text{lidar}} \cdot \mathbf{T}_{\text{lidar2imu}}, Timu=Tlidar⋅Tlidar2imu,
此中 T lidar2imu \mathbf{T}_{\text{lidar2imu}} Tlidar2imu 为外参。
2. 系统初始化
初始位姿、速度和偏置，初始位姿 T 0 \mathbf{T}_0 T0：
T 0 = T imu . \mathbf{T}_0 = \mathbf{T}_{\text{imu}}. T0=Timu.
初始速度 v 0 \mathbf{v}_0 v0 和偏置 b 0 \mathbf{b}_0 b0 设为零向量：
v 0 = [ 0 , 0 , 0 ] T , b 0 = [ b a b ω ] = 0. \mathbf{v}_0 = [0, 0, 0]^T, \quad \mathbf{b}_0 = \begin{bmatrix} \mathbf{b}_a \\ \mathbf{b}_\omega \end{bmatrix} = \mathbf{0}. v0=[0,0,0]T,b0=[babω]=0.
初始优化图： 利用先验因子初始化优化图：
- 位姿先验因子：
PriorFactor ( T 0 , T prior , Q pose ) , \text{PriorFactor}(\mathbf{T}_0, \mathbf{T}_{\text{prior}}, \mathbf{Q}_{\text{pose}}), PriorFactor(T0,Tprior,Qpose),
此中 Q pose \mathbf{Q}_{\text{pose}} Qpose 为位姿噪声协方差。
- 速度先验因子：
PriorFactor ( v 0 , v prior , Q vel ) . \text{PriorFactor}(\mathbf{v}_0, \mathbf{v}_{\text{prior}}, \mathbf{Q}_{\text{vel}}). PriorFactor(v0,vprior,Qvel).
- 偏置先验因子：
PriorFactor ( b 0 , b prior , Q bias ) . \text{PriorFactor}(\mathbf{b}_0, \mathbf{b}_{\text{prior}}, \mathbf{Q}_{\text{bias}}). PriorFactor(b0,bprior,Qbias).
- // 初始位姿
- prevPose_ = lidarPose.compose(lidar2Imu); // 将激光雷达位姿转换为IMU位姿
- gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise); // 添加初始位姿先验因子
- graphFactors.add(priorPose);
- // 初始速度
- prevVel_ = gtsam::Vector3(0, 0, 0); // 初始速度为零
- gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise); // 添加初始速度先验因子
- graphFactors.add(priorVel);
- // 初始偏差
- prevBias_ = gtsam::imuBias::ConstantBias(); // 初始偏差为零
- gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise); // 添加初始偏差先验因子
- graphFactors.add(priorBias);
- // 添加初始值
- graphValues.insert(X(0), prevPose_);
- graphValues.insert(V(0), prevVel_);
- graphValues.insert(B(0), prevBias_);
3. IMU积分与因子添加
IMU预积分，将IMU丈量值 a imu , ω imu \mathbf{a}_{\text{imu}}, \boldsymbol{\omega}_{\text{imu}} aimu,ωimu 进行预积分，得到状态更新：
v k + 1 = v k + ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t , p k + 1 = p k + v k ⋅ Δ t + 1 2 ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t 2 , R k + 1 = R k ⋅ exp ( ( ω imu − b ω ) ⋅ Δ t ) . \begin{aligned} \mathbf{v}_{k+1} &= \mathbf{v}_k + (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t, \\ \mathbf{p}_{k+1} &= \mathbf{p}_k + \mathbf{v}_k \cdot \Delta t + \frac{1}{2} (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t^2, \\ \mathbf{R}_{k+1} &= \mathbf{R}_k \cdot \exp((\boldsymbol{\omega}_{\text{imu}} - \mathbf{b}_\omega) \cdot \Delta t). \end{aligned} vk+1pk+1Rk+1=vk+(Rk⋅(aimu−ba)+g)⋅Δt,=pk+vk⋅Δt+21(Rk⋅(aimu−ba)+g)⋅Δt2,=Rk⋅exp((ωimu−bω)⋅Δt).
基于预积分丈量值构建：
ImuFactor ( T k , v k , T k + 1 , v k + 1 , b k , PreintegratedIMU ) . \text{ImuFactor}(\mathbf{T}_{k}, \mathbf{v}_k, \mathbf{T}_{k+1}, \mathbf{v}_{k+1}, \mathbf{b}_k, \text{PreintegratedIMU}). ImuFactor(Tk,vk,Tk+1,vk+1,bk,PreintegratedIMU).
- const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
- gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
- graphFactors.add(imu_factor);
- // 添加IMU偏差之间的因子
- graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
- gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
位姿因子，激光里程计位姿转换为IMU位姿后，构造先验因子：
PriorFactor ( T k + 1 , T cur , Q pose ) , \text{PriorFactor}(\mathbf{T}_{k+1}, \mathbf{T}_{\text{cur}}, \mathbf{Q}_{\text{pose}}), PriorFactor(Tk+1,Tcur,Qpose),
此中 T cur \mathbf{T}_{\text{cur}} Tcur 为当前里程计位姿。
- // 添加位姿因子
- gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
- gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
- graphFactors.add(pose_factor);
x ∗ = arg min x ∑ Factors . \mathbf{x}^* = \arg\min_{\mathbf{x}} \sum \text{Factors}. x∗=argxmin∑Factors.
调用优化器 optimizer.update 进行求解。
- // 优化
- optimizer.update(graphFactors, graphValues);
- optimizer.update();
- graphFactors.resize(0);
- graphValues.clear();
5. 优化后更新状态
优化完成后，从效果中提取最新状态：
T k + 1 = result ( X k + 1 ) , v k + 1 = result ( V k + 1 ) , b k + 1 = result ( B k + 1 ) . \begin{aligned} \mathbf{T}_{k+1} &= \text{result}(X_{k+1}), \\ \mathbf{v}_{k+1} &= \text{result}(V_{k+1}), \\ \mathbf{b}_{k+1} &= \text{result}(B_{k+1}). \end{aligned} Tk+1vk+1bk+1=result(Xk+1),=result(Vk+1),=result(Bk+1).
并重置IMU预积分器：
imuIntegratorOpt − > resetIntegrationAndSetBias ( b k + 1 ) . \text{imuIntegratorOpt}->\text{resetIntegrationAndSetBias}(\mathbf{b}_{k+1}). imuIntegratorOpt−>resetIntegrationAndSetBias(bk+1).
- // 为下一步重置预积分
- gtsam::Values result = optimizer.calculateEstimate();
- prevPose_ = result.at<gtsam::Pose3>(X(key));
- prevVel_ = result.at<gtsam::Vector3>(V(key));
- prevState_ = gtsam::NavState(prevPose_, prevVel_);
- prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
- // 重置优化预积分对象
- imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
- // 检查优化是否失败
在重新传播之前，需要移除时间戳早于当前校正时间 t currentCorrectionTime − Δ t t_{\text{currentCorrectionTime}} - \Delta t tcurrentCorrectionTime−Δt 的IMU消息：
t imu < t currentCorrectionTime − Δ t . t_{\text{imu}} < t_{\text{currentCorrectionTime}} - \Delta t. timu<tcurrentCorrectionTime−Δt.
通过循环移除队列中满足上述条件的IMU消息，并记录最后一条被移除的IMU时间戳 t lastImuQT t_{\text{lastImuQT}} tlastImuQT：
t lastImuQT = t imu . t_{\text{lastImuQT}} = t_{\text{imu}}. tlastImuQT=timu.
- // 首先弹出比当前校正数据旧的IMU消息
- double lastImuQT = -1;
- while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
- {
- lastImuQT = ROS_TIME(&imuQueImu.front());
- imuQueImu.pop_front();
- }
在IMU重新传播前，利用最新优化得到的偏置 b odom \mathbf{b}_{\text{odom}} bodom 对IMU积分器重置：
- // 使用新优化的偏差重置偏差
- imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
v k + 1 = v k + ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t , p k + 1 = p k + v k ⋅ Δ t + 1 2 ( R k ⋅ ( a imu − b a ) + g ) ⋅ Δ t 2 , R k + 1 = R k ⋅ exp ( ( ω imu − b ω ) ⋅ Δ t ) . \begin{aligned} \mathbf{v}_{k+1} &= \mathbf{v}_k + (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t, \\ \mathbf{p}_{k+1} &= \mathbf{p}_k + \mathbf{v}_k \cdot \Delta t + \frac{1}{2} (\mathbf{R}_k \cdot (\mathbf{a}_{\text{imu}} - \mathbf{b}_a) + \mathbf{g}) \cdot \Delta t^2, \\ \mathbf{R}_{k+1} &= \mathbf{R}_k \cdot \exp((\boldsymbol{\omega}_{\text{imu}} - \mathbf{b}_\omega) \cdot \Delta t). \end{aligned} vk+1pk+1Rk+1=vk+(Rk⋅(aimu−ba)+g)⋅Δt,=pk+vk⋅Δt+21(Rk⋅(aimu−ba)+g)⋅Δt2,=Rk⋅exp((ωimu−bω)⋅Δt).
此中：
- v k \mathbf{v}_k vk 和 p k \mathbf{p}_k pk 分别为速度和位置；
- R k \mathbf{R}_k Rk 为姿态旋转矩阵；
- a imu \mathbf{a}_{\text{imu}} aimu 和 ω imu \boldsymbol{\omega}_{\text{imu}} ωimu 分别为IMU的线加速度和角速度；
- b a \mathbf{b}_a ba 和 b ω \mathbf{b}_\omega bω 分别为加速度和角速度的偏置；
- Δ t \Delta t Δt 为时间隔断：
Δ t = { 1 500 , 若 t lastImuQT < 0 ; t imu − t lastImuQT , 否则 . \Delta t = \begin{cases} \frac{1}{500}, & \text{若 } t_{\text{lastImuQT}} < 0; \\ t_{\text{imu}} - t_{\text{lastImuQT}}, & \text{否则}. \end{cases} Δt={5001,timu−tlastImuQT,若 tlastImuQT<0;否则.
- // 从本次优化的开始处积分IMU消息
- for (int i = 0; i < (int)imuQueImu.size(); ++i)
- {
- sensor_msgs::Imu *thisImu = &imuQueImu[i];
- double imuTime = ROS_TIME(thisImu);
- double dt = (lastImuQT < 0) ? (1.0 / 500.0) : (imuTime - lastImuQT);
- imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
- gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
- lastImuQT = imuTime;
- }
|
