本节继续2D图优化过程，在讲解三轴IMU的预积分前，先看看直接积分对优化会产生哪些不良影响，并且为了简化计算过程，主要以欧拉角推导，三轴imu输入（ax, ay, gz)；并且忽略外参影响，假设所有传感器都在一个坐标系下，时间同步也已同步好， 先以简单的例子来说明问题，由于本系列以教学为主，因此数据以仿真生成，均放置与代码中。
相关说明请看仓库内的README.md
本篇博客对应完整代码放置在个人仓库：https://gitee.com/zl_vslam/slam_optimizer/tree/master/2d_optimize/ch4
一、位姿图表示
开始以一个简单的图来表示，只有一个节点的一元边表示该优化问题，如下所示：
上图中蓝色矩形为当前观测，下方圆圈表示当前状态
二、 三轴IMU的积分方程
IMU当前状态：
x=(p, v, theta, ba, bg)；大小为8x1; 当前状态指的是t+1时刻，t时刻为上一状态,本节统一省略下标
位置更新:
（1）
速度更新:
（2）
角度更新:
（3）
偏置更新：
（4）
（5）
三、当前观测
P=(,
,
)
四、残差项
为了减化，统一采用预测减观测,这样求解的雅可比可以不含负号
位置误差：
速度误差：
角度误差：
五、雅可比矩阵
由于都是误差项都是状态的直接观测，因此该三项的雅可比为5x5的单位矩阵，还有两个偏置项，预当前状态无关，因此雅可比为0， 大小为5x3
六、算法实现
主调用流程
ImuIntegration imu_integration(init_bias);
State state;
memset(&state, 0.0, sizeof(state));
std::vector<Eigen::Vector3d> pose_gt = pose_data;
double linear_vel = 0.5;
double true_vx = linear_vel * std::cos(pose_gt[0][2]);
double true_vy = linear_vel * std::sin(pose_gt[0][2]);
state.v = Eigen::Vector2d(true_vx,true_vy);
state.ba = Eigen::Vector2d(0,0);
state.bg = init_bias;
Optimizer optimizer;
Eigen::Vector3d last_odometry = odometry_data[0];
for(unsigned int i = 1; i < timestamps.size(); i++) {
const Eigen::Vector3d &last_imu = imu_data[i-1];
const Eigen::Vector3d &curr_imu = imu_data[i];
state.last_theta = state.theta;
// imu state and cov update
imu_integration.Integrate(true, last_imu, curr_imu, timestamps[i]-timestamps[i-1], state);
Eigen::Vector3d delta_odometry = odometry_data[i] - last_odometry;
// 此处没用，可控制多久优化一次
if(delta_odometry.block<2,1>(0,0).norm() < 0.00001) {
continue;
}
last_odometry = odometry_data[i];
double vx = vel_data[i][0];
double vy = vel_data[i][1];
const Eigen::Vector2d velocity(vx, vy);
const Eigen::Vector3d &pose = pose_data[i];
// 此处为基于高斯牛顿求解的优化算法
optimizer.OptimizeGN(state, velocity, pose);
usleep(150000);
}
IMU的积分方程
state.last_imu = last_imu;
state.curr_imu = curr_imu;
Eigen::Vector3d last_imu_unbias(last_imu[0], last_imu[1], last_imu[2]);
Eigen::Vector3d curr_imu_unbias(curr_imu[0], curr_imu[1], curr_imu[2]);
if(is_init_bias) {
last_imu_unbias[2] -= init_bias_;
curr_imu_unbias[2] -= init_bias_;
} else {
last_imu_unbias[0] -= state.ba[0];
curr_imu_unbias[0] -= state.ba[0];
last_imu_unbias[1] -= state.ba[1];
curr_imu_unbias[1] -= state.ba[1];
last_imu_unbias[2] -= state.bg;
curr_imu_unbias[2] -= state.bg;
}
Eigen::Vector3d imu_unbias_hat = 0.5 * (last_imu_unbias + curr_imu_unbias);
double dtheta = imu_unbias_hat(2) * dt;
Eigen::Matrix<double,2,2> R = math_utils::rotation(state.theta);
// update q, also plus dtheta, reason of this data
state.theta += dtheta;
state.theta = atan2(sin(state.theta), cos(state.theta));
// update p
Eigen::Vector2d acc_hat(imu_unbias_hat[0], imu_unbias_hat[1]);
state.p += state.v * dt + 0.5 * R.transpose() * acc_hat * dt * dt;
// update v
state.v += R.transpose() * acc_hat * dt;
// update F matrix，实际暂时并未使用，貌似还存在bug
Eigen::Matrix<double, 8, 8> F = Eigen::Matrix<double, 8 ,8>::Identity();
// Derivative of Pose
F.block<2, 2>(0, 0) = Eigen::Matrix<double, 2 ,2>::Identity(); // dp/dp
F.block<2, 2>(0, 2) = dt * Eigen::Matrix<double, 2 ,2>::Identity(); // dp/dv
Eigen::Matrix<double, 2, 2> dR = Eigen::Matrix<double, 2 ,2>::Identity();
dR(0, 0) = R(0, 1);
dR(1, 1) = dR(0, 0);
dR(0, 1) = -R(0, 0);
dR(1, 0) = R(0, 0);
F.block<2, 1>(0, 4) = 0.5 * dR * acc_hat * dt * dt; // dp/dtheta
F.block<2, 2>(0, 5) = -0.5 * R * dt * dt; // dp/dba
F.block<2, 1>(0, 7) = Eigen::Matrix<double, 2 ,1>::Zero(); // dp/dbg
// Derivative of V
F.block<2, 2>(2, 0) = Eigen::Matrix<double, 2 ,2>::Zero(); // dv/dp
F.block<2, 2>(2, 2) = Eigen::Matrix<double, 2 ,2>::Identity(); // dv/dv
F.block<2, 1>(2, 4) = dR * acc_hat * dt; // dv/dtheta
F.block<2, 2>(2, 5) = -R * dt; // dv/dba
F.block<2, 1>(2, 7) = Eigen::Matrix<double, 2 ,1>::Zero(); // dv/dbg
// Derivative of Q
F.block<1, 4>(4, 0) = Eigen::Matrix<double, 1, 4>::Zero(); // dQ/dp/dv
F(4, 4) = 1; // dQ/dtheta
F.block<1, 2>(4, 5) = Eigen::Matrix<double, 1, 2>::Zero(); // dQ/dba
F(4, 7) = -dt; // dQ/dbg
// Derivative of bias
F.block<3, 5>(5, 0) = Eigen::Matrix<double, 3, 5>::Zero();
F.block<3, 3>(5, 5) = Eigen::Matrix<double, 3, 3>::Identity();
// update update noise
Eigen::Matrix<double, 8, 8> G = std::pow(0.001,2) * Eigen::Matrix<double, 8, 8>::Identity();
G(0,0) = std::pow(0.1,2);
G(1,1) = std::pow(0.1,2);
G(2,2) = std::pow(0.1,2);
G(3,3) = std::pow(0.1,2);
state.cov = F * state.cov * F.transpose() + G;
state.dt = dt;
优化算法
int iterations = 100;
double cost = 0, last_cost = 0;
Eigen::Matrix<double, 5, 5> pose_covariance = Eigen::Matrix<double, 5, 5>::Identity();
pose_covariance(0,0) = std::pow(0.01,2);
pose_covariance(1,1) = std::pow(0.01,2);
pose_covariance(2,2) = std::pow(0.01,2);
pose_covariance(3,3) = std::pow(0.01,2);
pose_covariance(4,4) = std::pow(0.01,2);
Eigen::Matrix<double, 5, 5> information_matrix = pose_covariance.inverse();
for (int iter = 0; iter < iterations; iter++) {
cost = 0;
Eigen::Matrix<double, 5, 1> residual;
Eigen::Matrix<double, 5, 8> jacobian;
PoseObserve::ComputeResidualAndJacobian(state, pose, velocity, residual, jacobian);
cost += Chi2(residual, pose_covariance.inverse());
Eigen::Matrix<double, 8, 8> H = Eigen::Matrix<double, 8, 8>::Zero();
Eigen::Matrix<double, 8, 1> b = Eigen::Matrix<double, 8, 1>::Zero();
H += jacobian.transpose() * information_matrix * jacobian;
b += -jacobian.transpose() * information_matrix * residual;
// solve dx
Eigen::Matrix<double, 8, 1> dx = H.ldlt().solve(b);
// Eigen::Matrix<double, 8, 1> dx = H.inverse() * (b);
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= last_cost) {
// 误差增长了，说明近似的不够好
cout << "cost: " << cost << ", last cost: " << last_cost << endl;
break;
}
update_state(state, dx);
last_cost = cost;
}
七、总结：
经过上述高斯牛顿，实际结果并未收敛，也可以从结果分析出，一元边相对于IMU优化算法来讲，并非一个好的图优化设计，不同于卡尔曼滤波，卡尔曼滤波可以融合当前帧的原理，来源于其马尔可夫性，即当前帧只跟上一帧相关，当前结果受上一帧影响，而优化算法就不同了，跟imu偏置相关的雅可比均为0,也就是说，优化过程不更新imu偏置，那imu的状态，长时间依靠一个不准确的初始偏置，会影响效果，因此下一讲继续看看，如何将图设计的更合理些
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
