大体实现思路
和liomapping不同,liomapping使用的是ceres求最小二乘法将预积分投入到系统,liosam直接使用的gtsam库将预积分部分放进去。
imu的预积分因子示意图
预积分gtsam库的使用
1.首先定义优化器
gtsam::ISAM2 optimizer;
2.定义非线性因子图
一般的图节点之间的关系由这个变量管理。
gtsam::NonlinearFactorGraph graphFactors;
2.1 插入先验因子进入图中
gtsam::PriorFactor定义的变量构造函数第一个是标识,第二个是位姿,第三个是噪声。这种主要是插入第一个因子图。
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
graphFactors.add(priorPose);
2.2 插入因子之间的图
这里的参数主要作用是,第一个图节点的编号,第二个图节点的编号,相对关系,噪声模型。
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
3.图节点值定义
一般就是给节点赋值。
gtsam::Values graphValues;
一般使用的方式:
initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);
参数中第一个是编号,第二个是pose。最后图估计的返回也是value类型
4.代码注释
代码中预积分的核心函数之一,从地图优化中得来的增量odom然后和imu预积分进行联合因子图优化,主要步骤是,计算雷达位姿作为先验,对两帧odom之间的imu进行预积分,将预积分加入因子图,进行优化。
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)//订阅地图过来的odomMsg
{
std::lock_guard<std::mutex> lock(mtx);
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;
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
//将里程计信息赋予给lidarPose
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)
{
resetOptimization();//重置优化器
// pop old IMU message
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)//队列是先进先出,把一定时间外的imu测量pop掉
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
// initial pose
prevPose_ = lidarPose.compose(lidar2Imu);//这里是相乘的作用,将雷达位姿转给imu
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);//初始bias
// 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();
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
key = 1;
systemInitialized = true;//初始化成功
return;
}
// reset graph for speed
//如果图超过100则优化然后进行重置,这里和liomapping不一样,liomapping还用了边缘化保留之前位姿。
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
//主要的优化过程,这段的目的是将imu测量放入gtsam中的imu因子中预积分。
while (!imuQueOpt.empty())
{
// pop and integrate imu data that is between two optimizations
//对两次优化间的imu数据进行积分
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
double imuTime = ROS_TIME(thisImu);
if (imuTime < currentCorrectionTime - delta_t)
{
//第一次是1/500后面都是dt。
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
//进行预积分得到新的状态值 注意用到的是imu数据的加速度 角速度 作者要求的9轴imu数据中欧拉角在本程序文件中没有任何用到 全在地图优化里用到的
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();//pop出来imu
}
else
break;
}
// add imu factor to graph
const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
//一个imu因子需要速度以及位姿的两帧间关系,以及上一帧的bias
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);//前面的因子图累加是一帧,就是一个key
graphFactors.add(imu_factor);
// 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)));
// 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();
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;
}
// 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;
}
第二个核心函数是imu的处理函数
步骤:
1.读入imu数据;
2.进行imu预积分;
3.预测状态;
4.发布速度以及位姿。
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);//拿到互斥锁
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
if (doneFirstOpt == false)
return;
double imuTime = ROS_TIME(&thisImu);
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);//第一帧的时候是1/500,后面帧
lastImuT_imu = imuTime;
// 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);
// 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;
odometry.child_frame_id = "odom_imu";
// transform imu pose to ldiar
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);
}
5.常见的IMU预积分使用函数
定义一个imu的因子
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
根据imu的数据做预积分
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);
进行优化预测state
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);//预测值,插入先验
优化得到结果
optimizer是isam2即优化器。
gtsam::Values result = optimizer.calculateEstimate();
重置预积分优化器并设置新的bias
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);