先把系统topic 数据流图搬过来,能更加清晰的理解这个模块。
图2.ros topic 发布与订阅关系图关系图(还是叫图2,因为是同一个图)
这个cpp文件主要有两个类,一个叫IMUPreintegration类,一个叫TransformFusion类。从数据流向和依赖程度的角度分析,应该先分析IMUPreintegration类,但是TransformFusion类写在该模块的前头,且这两个模块的数据流相互依赖,这里先分析TransformFusion类。
TransformFusion类
讲解之前,二话不说,先上该类的成员变量列表。
变量名 | 类型 | 注释 | 备注 |
mtx | std::mutex | 因为ros回调函数是异步运行,在回调函数操作成员变量或者队列时,要先上锁 | - |
subImuOdometry | ros::Subscriber | 订阅"odometry/imu_incremental" 虽然名字上有incremental ,但里程计,他就是里程计,代表着该时刻固定的frame 的位姿信息,由下面的imuPreintegration 发出, 虽然 由 gtsam 的预积分器算出来,但完全可以理解为不与预积分有关系,单纯文imu 数据的位姿积分解算 在这个类中 其中一个主要的目的就是 在优化过后的lidar 里程计基础上,叠加上lidar时刻到至今时刻的位姿差,以发出更高频率的里程计定位数据 | - |
subLaserOdometry | ros::Subscriber | 订阅"lio_sam/mapping/odometry" 就是经过MapOptimization优化后的里程计信息,接收周期比慢一个量级,与 "odometry/imu_incremental" 的 frameid 应该是一致的,都是 odomframe,child_frame_id 都是odom_lidar 注意,这里/mapping/odometry和/mapping/odometry_incremental的区别,前者是激光里程计部分直接优化得到的激光位姿,后者根据运动模型 计算的超前一个lidar 周期的里程计信息。 |
- |
pubImuOdometry | ros::Publisher | 发布imu 里程计 "odometry/imu" 结合了lidar 里程计的IMU 里程计# IMU pre-preintegration odometry, same frequency as IMU | - |
pubImuPath | ros::Publisher | 消息名为 "lio_sam/imu/path" ,在图2 中由 rviz 订阅,其发布imu 里程计 "odometry/imu" 对应的轨迹,发布频率是10hz,每次发布都只是一小段,即lidar时刻到当前imu 时刻的位姿数组 | - |
lidarOdomAffine | Eigen::Affine3f | 就是把"lio_sam/mapping/odometry" 的消息转化为位姿变换矩阵,代表但前lidar时刻 (机器人)的位姿 | - |
lidarOdomAffine | Eigen::Affine3f | 就是把"lio_sam/mapping/odometry" 的消息转化为位姿变换矩阵,代表但前lidar时刻 的位姿 | - |
imuOdomAffineFront | Eigen::Affine3f | 在loam 系列代码里看到这一对 front /back 的变换矩阵,就应该想到作差 相对位姿变换,这个变换矩阵是由lidar 时刻的最近的imu时刻的"odometry/imu_incremental" 的里程计消息转化而来,代表那个时刻的位姿 | - |
imuOdomAffineBack | Eigen::Affine3f | 这个变换矩阵是由当前时刻imu时刻的"odometry/imu_incremental" 的里程计消息转化而来,代表那个时刻的位姿,作用是 与imuOdomAffineFront 作差后,附加到lidarOdomAffine变换上,得到结合地图优化的位姿的更高频率的位姿信息 | - |
tfListener | tf::TransformListener | 从tf 树中得到 lidar2Baselink 的外参关系 | - |
lidar2Baselink | tf::StampedTransform | 如果imu里程计与lidar 里程计的坐标系不是同一个坐标系,需要转化到同一个坐标系两个里程计数据才能融合 | - |
lidarOdomTime | float | lidar时刻的时间戳,每次lidar 里程计来了都更换一次,imu 里程计队列imuOdomQueue的数据,都保留这个时刻之后的数据 | - |
imuOdomQueue | deque | imu 里程计队列,都保留lidar时刻开始之后的数据,便于寻找lidar时刻imu的里程计,和当前时刻里程计,作差,附加到lidar 里程计中去 | - |
纵观TransformFusion类,该类只干一件事,就是融合lidar里程计(经过mapOptimization 优化过的)和imu 积分的里程计,以比lidar里程计更高的频率分布出去,一个是odometry/imu,一个是odom 到 baselink 的tf 关系,这个就是整个系统的主要输出。
主要的处理逻辑在imu 里程计的回调函数中,imuOdometryHandler
,在函数里面监听的是"odometry/imu_incremental"(在下面imuPreintegration中发出,是imu 独立解算的里程计),把imu里程计放到imuodomQueue里面保存,然后把lidarOdomTime之前的数据扔掉,之后用最新和最旧时刻的mu里程计的计算差异,如图3所示,再加上lidarOdomAffine的基础进行发布。
另外还会发布一个path,用来rviz显示,名字叫lio-sam/imu/path。
/**
* 激光雷达里程计就保存当前的雷达里程计的数据到lidarOdomAffine里面,
* 把时间戳存到lidarOdomTime里面去,没干别的,
* 具体的主要逻辑在imu 里程计的回调函数中处理。
*
* */
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
lidarOdomAffine = odom2affine(*odomMsg);
lidarOdomTime = odomMsg->header.stamp.toSec();
}
/**
* 主要的处理逻辑在imu 里程计的回调函数中,`imuOdometryHandler`,
* 在函数里面监听的是/imu/odometry_incremental(在下面imuPreintegration中发出,
* 是imu 独立解算的里程计),把imu里程计放到imuodomQueue里面保存,
* 然后把lidarOdomTime之前的数据扔掉,之后用最新和最旧时刻的mu里程计的计算差异,
* 再加上lidarOdomAffine的基础进行发布。
* */
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
// static tf 这个地方究竟是在干什么?
static tf::TransformBroadcaster tfMap2Odom;
static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
std::lock_guard<std::mutex> lock(mtx);
imuOdomQueue.push_back(*odomMsg);
// get latest odometry (at current IMU stamp)
if (lidarOdomTime == -1)
return;
while (!imuOdomQueue.empty())
{
if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
imuOdomQueue.pop_front();
else
break;
}
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());//前
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());// 后
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
// publish latest odometry
nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
laserOdometry.pose.pose.position.x = x;
laserOdometry.pose.pose.position.y = y;
laserOdometry.pose.pose.position.z = z;
laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
pubImuOdometry.publish(laserOdometry);
// publish tf
static tf::TransformBroadcaster tfOdom2BaseLink;
tf::Transform tCur;
tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
if(lidarFrame != baselinkFrame)
tCur = tCur * lidar2Baselink;
tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
// 发布 odom 到 baselink 的tf 关系
tfOdom2BaseLink.sendTransform(odom_2_baselink);
// publish IMU path
static nav_msgs::Path imuPath; // 静态数据,Path 实际是pose数组,
static double last_path_time = -1;
double imuTime = imuOdomQueue.back().header.stamp.toSec();
// 控制发送频率 10HZ
if (imuTime - last_path_time > 0.1)
{
last_path_time = imuTime;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
pose_stamped.header.frame_id = odometryFrame;
pose_stamped.pose = laserOdometry.pose.pose;
imuPath.poses.push_back(pose_stamped);
// lidar 时间戳1秒钟以内的数据,以前的数据删掉
while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
imuPath.poses.erase(imuPath.poses.begin());
if (pubImuPath.getNumSubscribers() != 0)
{
imuPath.header.stamp = imuOdomQueue.back().header.stamp;
imuPath.header.frame_id = odometryFrame;
pubImuPath.publish(imuPath);
}
}
}
IMUPreintegration 类
成员变量列表:
变量名 | 类型 | 注释 | 备注 |
mtx | std::mutex | 因为ros回调函数是异步运行,在回调函数操作成员变量或者队列时,要先上锁 | - |
subImu | ros::Subscriber | //订阅"imu_raw",imu源数据 | - |
subOdometry | ros::Subscriber | 订阅 "lio_sam/mapping/odometry_incremental" 这个消息是从MapOptimization 发布,经过地图优化且通过匀速运动模型 计算的朝一个lidar 优化周期的里程计信息 |
- |
pubImuOdometry | ros::Publisher | 发布 "odometry/imu_incremental" 实际 TransformFusion是其中一个订阅者,在一个订阅者是imageProjection 虽然 frame_id = odometryFrame;child_frame_id = "odom_imu"; 但代码里实现应该是“odom_lidar”,因为IMU的数据转换成与Lidar坐标系同方向的数据,最后又右乘一个变换(imu2Lidar 实际不是) ;即T_ol = T_oi*T_il | - |
systemInitialized | bool | 初始化标志,因子图优化器初始化完成后,就设置为true,或者积分出来的数据超过阈值,需要重新因子图优化器初始化 | - |
预积分因子模型噪声模型参数 | - | ||
priorPoseNoise | gtsam::noiseModel::Diagonal::shared_ptr | 位姿噪声模型 | - |
priorVelNoise | gtsam::noiseModel::Diagonal::shared_ptr | 速度噪声模型 | - |
priorBiasNoise | gtsam::noiseModel::Diagonal::shared_ptr | 偏差噪声模型 | - |
correctionNoise | gtsam::noiseModel::Diagonal::shared_ptr | 激光雷达位姿噪声模型 | - |
correctionNoise2 | gtsam::noiseModel::Diagonal::shared_ptr | 激光雷达位姿噪声模型,协方差是否为单位阵,决定使用哪个噪声模型 | - |
noiseModelBetweenBias | gtsam::Vector | 两帧的bias 形成一个betweenFactor, 也需要提供一个噪声模型 | - |
*imuIntegratorOpt_ | gtsam::PreintegratedImuMeasurements | 预积分器,这个预积分提供预积分数据给因子图优化器 | - |
*imuIntegratorImu_ | gtsam::PreintegratedImuMeasurements | 预积分器,这个预积分提供预积分数据给里程计累加用 | - |
imuQueOpt | std::deque | 预积分器对应的imu积分数据队列 | - |
imuQueImu | std::deque | 预积分器对应的imu积分数据队列 | - |
prevPose_ | gtsam::Pose3 | 上一帧lidar数据到来时因子图优化后的位姿 | - |
prevVel_ | gtsam::Vector3 | 上一帧lidar数据到来时因子图优化后的速度 | - |
prevState_ | gtsam::NavState | 上一帧lidar数据到来时因子图优化后的状态,包含pose、vel | - |
prevBias_ | gtsam::imuBias::ConstantBias | 上一帧lidar数据到来时因子图优化后的IMU偏差 | - |
prevStateOdom | gtsam::NavState | 上一帧lidar数据到来时因子图优化后的状态,包含pose、vel;从prevState_ 赋值 | - |
prevBiasOdom | gtsam::imuBias::ConstantBias | 上一帧lidar数据到来时因子图优化后的IMU偏差;从prevBias_ 赋值 | - |
doneFirstOpt | bool | 只有经过因子图优化后,才进行imuIntegratorImu_预积分累积里程计 | - |
lastImuT_imu | double | 上一次的imu 的时间戳,当前帧imu 来时,用当前imu时间戳 减去 这个值,得到dt,imu积分时需要这个值 | - |
lastImuT_opt | double | 意思同上,就是对应不同的预积分器而已 | - |
optimizer | gtsam::ISAM2 | 因子图优化器,这里用ISAM2作为优化器的后端 | - |
graphFactors | gtsam::NonlinearFactorGraph | 因子图容器,用于添加各种因子 | - |
graphValues | gtsam::Values | 因子图中各个节点,也就是待优化的变量 | - |
delta_t | const double | 常量,距离lidar 时间戳的距离,这里固定为0 | - |
key | int | 计数器,每100次 重新初始化 因子图优化器,这个跟gtsam的实现有关系,每次优化保留一些东西在贝叶斯树里头,多了会影响速度 | - |
key | int | 计数器,每100次 重新初始化 因子图优化器,这个跟gtsam的实现有关系,每次优化保留一些东西在贝叶斯树里头,多了会影响速度 | - |
imu2Lidar | gtsam::Pose3 | 原文注释:T_bl: tramsform points from lidar frame to imu frame 这个注释跟命名感觉不对劲,应该表示成Lidar2Imu ;在params.yaml 文件中 # Extrinsics: T_lb (lidar -> imu) 个人觉得也有问题 应该表示成T_lb (imu -> lidar) 但是 包括下边的两个变量的命名 应该是反着来的 | - |
lidar2Imu | gtsam::Pose3 | 原文注释:T_bl: T_lb: tramsform points from imu frame to lidar frame 这个注释跟命名感觉不对劲,应该表示成imu2Lidar ,这个注释没有说到点子上,在整个系统中这两个变量只有在里程计变换中用到,而里程计变换的计算跟坐标系上的点坐标变换是有区别,具体看代码处的注释 | - |
关于IMU原始数据,送入imuhandle中:
1.1 imuhandle:
1.1.1 imu原始数据,会先被坐标系转换,通过调用头文件里的imuConverter函数,转换到一个“雷达中间系”中,其实和真正的雷达系差了一个平移。这里的操作就像对IMU 的数据进行修正,让IMU的坐标系与Lidar 坐标系的方向一致,然后 IMU 与 Lidar外参就只有位移关系了。乍一看,感觉作者故意写复杂了,但是个人猜想,1.由于IMU数据里面没有位移数据,不能进行直接变换到Lidar坐标系;2.速度变化很小,进行位移变换后是否影响积分精度。
1.1.2 转换后,会存入两个队列,一个imuQueOpt队列,在另一个回调函数odometryHandler会被处理,用于预积分因子源数据,用来优化系统状态(R,P,V,B)和imu的bias。一个imuQueImu队列,imuQueImu是这里真正要用的数据,每到一个imu数据,就用imuIntegratorImu_这个预积分器,把这一帧imu数据累积进去,然后预测当前时刻的状态:currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 其中prevStateOdom就是之前系统所保持的状态。
1.1.3 把currentState,通过imu2Lidar(其实是Lidar2Imu T_il)T_ol = T_oi * T_il,把IMU 积分得到的里程计转成真正的雷达里程计,然后发布出去。发布的话题就叫odometry/imu_incremental(实际是lidar 里程计),这也就是imageProjection.cpp的总结部分的第2点部分提到的“imu”里程计。虽然作者的变量命名很疑惑,但是代码实现得还是很老实的。
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
// 这个函数这个地方写的非常绕,非值得注意的是,这里是把imu的数据转换到lidar坐标系下,
// 但是又不完全转,只是保持与lidar坐标系的方向一致,差一个位移
// 这里的操作就像对IMU 的数据进行修正,让IMU的坐标系与Lidar 坐标系的方向一致,
// 然后 IMU 与 Lidar外参就只有位移关系了。
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
// 转换后的数据压入队列中
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
// 如果因子图还没有优化过,就直接返回了
if (doneFirstOpt == false)
return;
// 获取该IMU数据的时间戳
double imuTime = ROS_TIME(&thisImu);
// 与上一帧IMU 数据时间戳作差,
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
lastImuT_imu = imuTime;
// 把当前帧的IMU 数据添加入预积分器
// integrate this single imu message
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);
// 以上一帧lidar时刻因子图优化出来的状态为基准,预积分器预测出当前IMU时刻的位姿状态
// predict odometry
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
// publish odometry
nav_msgs::Odometry odometry;
odometry.header.stamp = thisImu.header.stamp;
odometry.header.frame_id = odometryFrame;
// 这里实际应该是“odom_lidar”
odometry.child_frame_id = "odom_imu";
// transform imu pose to ldiar
gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
// imu2Lidar 变量的值给得应该表示成T_il,整个式子 则为 T_ol = T_oi*T_il
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);
}
1.2. odomHandle:这部分订阅的是/lio_sam/mapping/odometry_incremental,这个话题是由mapOptmization.cpp发布的,就是经过了地图匹配优化后的位姿信息,里程计就是从odom到lidar的变换关系而不是两帧激光之间的变换,它和imu里程计性质类似,就是相对世界坐标系的位姿。
1.2.1 初始化系统:
从imuQueOpt队列里,删掉当前这帧激光里程计时间上之前的数据,取两个Lidar时刻之间的IMU 数据进行预积分累积,然后把雷达的pose变换到IMU “坐标系”(这里的IMU坐标经过了修正,方向与Lidar坐标系保持一致,外参只差一个位移),然后初始化因子图优化器,相关优化变量prevPose等。因为预积分器需要bias ,而bias每次优化都会发生变化,所以需要重新设置,所以把两个预积分器imuIntegratorImu_,imuIntegratorOpt_给reset一下。(之后简称imu积分器和opt积分器)
1.2.2 清理缓存:
100帧激光里程计数据了,就把优化器给reset一下(用前面的先验协方差给reset一下)。每100 次 重新设置优化器,这个是gtsam 的实现要求的,在每次优化的过程中, gtsam 会缓存一些数据在optimizer贝叶斯树中,随着优化次数增大,该树会越来越大,执行效率就会变慢,所以要周期性清理。 在清理之前,需要把优化变量的协方差给保存起来,给新的优化器使用,在最开始的时候 每个因子的噪声模型都是人工经验值,这里需要把协方差矩阵传递下去,给新的优化器
1.2.3 正式处理:
1.2.3.1 把两个Lidar 时刻的IMU数据拿出来,送入opt积分器。这样得到两个时刻之间的IMU预积分,构建imu因子。
1.2.3.2 然后把关联Xkey-1 和Xkey的IMU 因子加入因子图 以及 激光里程计提供的pose先验因子,还有Bias BetweenFactor,整体做一个优化。优化的结果就是当前Lidar时刻的状态量(R,P,V,B)。
1.2.3.3 把优化的结果(主要是bias),重置opt积分器和imu积分器。由于imuIntegratorImu_被重置了BIAS ,需要对之前的IMU 数据进行重新积分,并删掉使用过的数据,这就是在imuHandler 中积分却不删数据的原因。
/**
* @brief 激光雷达里程计的回调函数
*
* @param odomMsg
*/
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
// 获取lidar 时间戳
double currentCorrectionTime = ROS_TIME(odomMsg);
// make sure we have imu data to integrate
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;
// 是否退化,什么情况下会退化? 初始情况下odomMsg->pose.covariance[0] == 1
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. initialize system
if (systemInitialized == false)
{
// 重置Isam2 优化器
积分
// 优化器
// optimizer = gtsam::ISAM2(optParameters);
// 因子图容器
// graphFactors = newGraphFactors;
// 待优化的变量
// graphValues = NewGraphValues;
resetOptimization();
// pop old IMU message
while (!imuQueOpt.empty())
{
// imuQueOpt数据中小于lidar时刻的数据全部pop 出去
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
// 因子图是贝叶斯公式推导过来的,P(x|z) = a*P(z|x)P(x) 其中a 为系数,P(z|x)为似然函数,P(x) 则为先验
// 所以 因子图里都要添加进 先验P(x)
// 这里把 因子图中第一个优化变量设为先验
// initial pose
prevPose_ = lidarPose.compose(lidar2Imu);// compose 是什么运算??转到IMU 系 todo:T_oi = T_ol * T_li
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
graphFactors.add(priorPose);
// initial velocity
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel);
// initial bias
prevBias_ = gtsam::imuBias::ConstantBias();
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
graphFactors.add(priorBias);
// add values
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// optimize once
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
//预积分器需要bias ,而bias每次优化都会发生变化,所以需要重新设置
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
key = 1;
systemInitialized = true;
return;
}
// 每100 次 重新设置优化器,这个是gtsam 的实现要求的,在每次优化的过程中,
// gtsam 会缓存一些数据在贝叶斯树中,随着优化次数增大,该树会越来越大,执行效率就会变慢,
// 所以要周期性清理
// reset graph for speed
if (key == 100)
{
// 在最开始的时候 每个因子的噪声模型都是人工经验值,这里需要把协方差矩阵传递下去,给新的优化器
// get updated noise before reset
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)));
// reset graph
resetOptimization();
// 新的因子图需要新的先验
// add pose
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
graphFactors.add(priorPose);
// add velocity
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
graphFactors.add(priorVel);
// add bias
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
graphFactors.add(priorBias);
// add values
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// optimize once 预热一次
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
key = 1;
}
// 从这里开始 真正处理数据因子图优化了
// 1. integrate imu data and optimize
while (!imuQueOpt.empty())
{
// pop and integrate imu data that is between two optimizations
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
double imuTime = ROS_TIME(thisImu);
// 取两个lidar 时间戳之间的IMU 数据提供给IMU积分器进行积分,
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(); // 用一个 删一个,每一次优化周期内,没有上一周期的IMU数据
}
else
break;
}
// add imu factor to graph
const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
// IMU 因子 提供 K-1 时刻的状态变量 与K 时刻的状态变量,与预积分器
// 在gtsam::ImuFactor 中定义好了 残差计算和雅可比的计算
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
graphFactors.add(imu_factor);
// BIAS 残差 用gtsam::BetweenFactor 因子,表示两帧lidar时刻的BIAS 应该保持一致,对因子图进行约束
// add imu bias between factor
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
// lidar 位姿约束因子只跟当前 状态量有关系,属于一个先验因子
// add pose factor
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);
// 因子相当于给因子图优化问题提供了观测值,
// 还需要给优化变量提供初始值
// 以上一时刻的状态为基准,从预积分器中预测当前的状态值,作为因子图优化变量的初值
// insert predicted values
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);
// optimize
optimizer.update(graphFactors, graphValues);
optimizer.update();
// 优化完之后,因子图把相关约束 添加到优化器optimizer的贝叶斯树中,所以在因子图上可以删除
graphFactors.resize(0);
graphValues.clear();
// Overwrite the beginning of the preintegration for the next step.
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));
// Reset the optimization preintegration object.
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
// check optimization
if (failureDetection(prevVel_, prevBias_))
{
resetParams();
return;
}
//因子图优化完之后,得到当前优化状态,需要设置imuIntegratorImu_ 的BIAS 重新预积分
// 2. after optiization, re-propagate imu odometry preintegration
prevStateOdom = prevState_;
prevBiasOdom = prevBias_;
// first pop imu message older than current correction data
double lastImuQT = -1;
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}
// repropogate
if (!imuQueImu.empty())
{
// reset bias use the newly optimized bias
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// integrate imu message from the beginning of this optimization
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;
}