VINS-Monoソース分析4—vins_estimator 3(非線形最適化)
108091 ワード
VINS-Monoソース分析4—vins_estimator3
初期化に成功すると、プログラムは次のようなコードに入ります.
if(result)
{
solver_flag = NON_LINEAR;
solveOdometry();
slideWindow();
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
solver_flagの状態をNON_にするLINEAR後、バックエンド非線形最適化solveOdometry()関数とスライドウィンドウ処理slideWindow()関数が実行され、フィーチャーポイントマネージャf_が削除されます.managerの深さが負の特徴点で、最後にlast_を記録します.R、last_P、last_R 0とlast_P0.
次にsolveOdometry()関数を詳しく分析します.
void Estimator::solveOdometry()
{
if (frame_count < WINDOW_SIZE)
return;
if (solver_flag == NON_LINEAR)
{
TicToc t_tri;
f_manager.triangulate(Ps, tic, ric);
ROS_DEBUG("triangulation costs %f", t_tri.toc());
optimization();
}
}
triangulate()関数は『VINS-Monoソース解析3-vins_estimator 2』で既に解析されており、ここではこれ以上説明しないが、ここでこの関数を再び呼び出す目的は、特徴点の深さを世界座標系下の画像フレームに更新することであることを強調する.次にoptimization()関数を解析し、
ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);
ceresライブラリ最適化問題を構築し、損失関数をコシコア関数として定義します.
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
//
if (!ESTIMATE_EXTRINSIC)
{
ROS_DEBUG("fix extinsic param");
problem.SetParameterBlockConstant(para_Ex_Pose[i]);
}
else
ROS_DEBUG("estimate extinsic param");
}
スライド内のすべてのフレームの姿勢、IMUの速度、加算ジャイロバイアス情報、およびIMUカメラ間の外部パラメトリック姿勢を含むパラメータブロックを追加し、IMUカメラ間の外部パラメトリック姿勢を固定パラメータブロックとする.
if (ESTIMATE_TD)
{
problem.AddParameterBlock(para_Td[0], 1);
}
このコードはデフォルトでは実行されません.
vector2double();
void Estimator::vector2double()
{
......
VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < f_manager.getFeatureCount(); i++)
para_Feature[i][0] = dep(i);
if (ESTIMATE_TD)
para_Td[0][0] = td;
}
このコードはpara_にPose、para_SpeedBiasとpara_Ex_Pose付与、最も重要なのは逆深さpara_を得ることですFeature.実はここはずっとよく分からないので、理にかなってparaにあげるべきです.Pose、para_SpeedBiasとpara_Ex_Poseは値を付けてからパラメータブロックを追加するのが正しいのですが、なぜかこのようにプログラムを書いても正常に実行できますか?
if (last_marginalization_info)
{
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}
エッジ化残差情報を定義し、エッジ化残差ブロックを追加します.エッジ化は最適化の過程で制約の役割を果たすだけであることに注意してください.エッジ化については、ブログ「VINS-Monoソース分析5-vins_estimator 4」で紹介したが、ここではあまり話さない.
for (int i = 0; i < WINDOW_SIZE; i++)
{
int j = i + 1;
//
if (pre_integrations[j]->sum_dt > 10.0)
continue;
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
IMU残差情報を定義し、IMU残差ブロックを追加します.この部分の具体的な実装コードはfactorフォルダの下のIMU_factor.hファイルには、
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
//
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);
Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);
//
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
//sqrt_info.setIdentity();
residual = sqrt_info * residual;
//
if (jacobians)
{
......
}
return true;
}
上のコードはintegration_と結合しています.base.hのコード読解、理論知識は崔神の文章「VINS論文導出及びコード解析」の3.3節の内容を参考にする.
int f_m_cnt = 0;
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if (imu_i == imu_j)
{
continue;
}
Vector3d pts_j = it_per_frame.point;
//
if (ESTIMATE_TD)
{
......
}
else
{
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
}
f_m_cnt++;
}
}
視覚再投影残差情報を定義し、視覚再投影残差ブロックを追加します.この部分の具体的な実装コードはfactorフォルダの下のprojection_factor.cppファイルでは、
bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
TicToc tic_toc;
//
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
double inv_dep_i = parameters[3][0];
// ,
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
Eigen::Map<Eigen::Vector2d> residual(residuals);
// else
#ifdef UNIT_SPHERE_ERROR
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif
residual = sqrt_info * residual;
//
if (jacobians)
{
......
}
sum_t += tic_toc.toc();
return true;
}
理論知識は崔神の文章「VINS論文の導出とコード解析」の3.4節の内容を参考にした.
if(relocalization_info)
{
......
}
パラメータファイルの下にfast_を置かない限り、関連する処理を再配置します.デフォルトでは実行されません.relocalizationの値を1に設定すると、ループが検出されたときに実行されます.
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
if (marginalization_flag == MARGIN_OLD)
options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
ceres解のいくつかのパラメータを構成し、反復最適化を行い、最適化されるパラメータの最適結果を探す.
double2vector();
void Estimator::double2vector()
{
......
// ， 0
double y_diff = origin_R0.x() - origin_R00.x();
//
Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0));
if (abs(abs(origin_R0.y()) - 90) < 1.0 || abs(abs(origin_R00.y()) - 90) < 1.0)
{
ROS_DEBUG("euler singular point!");
rot_diff = Rs[0] * Quaterniond(para_Pose[0][6],
para_Pose[0][3],
para_Pose[0][4],
para_Pose[0][5]).toRotationMatrix().transpose();
}
for (int i = 0; i <= WINDOW_SIZE; i++)
{
......
// 0 ，
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
......
//
}
//
VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < f_manager.getFeatureCount(); i++)
dep(i) = para_Feature[i][0];
f_manager.setDepth(dep);
//
if (ESTIMATE_TD)
td = para_Td[0][0];
// ， euroc_config.yaml fast_relocalization 0
if(relocalization_info)
{
......
}
}
コード分析は注釈に書かれています.
if (marginalization_flag == MARGIN_OLD)
{
......
}
else
{
......
}
この中のコードはとても重要で、次のエッジ化のために準備をして、主に線形化の残差r p r_を更新しますp rpと線形化ヤコビ行列J p J_p Jp,それらはエッジ化の制約と理解でき,崔神の文章「VINS論文の導出とコード解析」の3.2節の目標関数の中のr p r_p rpとJ p J_p Jp.この部分について専門的にブログ「VINS-Monoソース分析5-vins_estimator 4」を書いて紹介したが、ここではあまり話さない.
次にprocessImage()関数に戻り、slideWindow()関数を解析し、最も古いフレームをエッジ化すると、
double t_0 = Headers[0].stamp.toSec();
back_R0 = Rs[0];
back_P0 = Ps[0];
スライドウィンドウのフレーム0のタイムスタンプ、回転、およびパン情報を格納します.
for (int i = 0; i < WINDOW_SIZE; i++)
{
Rs[i].swap(Rs[i + 1]);
std::swap(pre_integrations[i], pre_integrations[i + 1]);
dt_buf[i].swap(dt_buf[i + 1]);
linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);
Headers[i] = Headers[i + 1];
Ps[i].swap(Ps[i + 1]);
Vs[i].swap(Vs[i + 1]);
Bas[i].swap(Bas[i + 1]);
Bgs[i].swap(Bgs[i + 1]);
}
Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
スライドウィンドウが順次移動し、このとき10フレーム目はまだ到来していないので、10フレーム目の初期処理方式に注意する.
if (true || solver_flag == INITIAL)
{
map<double, ImageFrame>::iterator it_0;
it_0 = all_image_frame.find(t_0);
delete it_0->second.pre_integration;
it_0->second.pre_integration = nullptr;
for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it)
{
if (it->second.pre_integration)
delete it->second.pre_integration;
it->second.pre_integration = NULL;
}
all_image_frame.erase(all_image_frame.begin(), it_0);
all_image_frame.erase(t_0);
}
窓の外のすべてのフレームをスライドさせるプリ積分pre_integration情報削除空にし、窓の外のすべてのフレームを削除します.
slideWindowOld();
void Estimator::slideWindowOld()
{
sum_of_back++;//
bool shift_depth = solver_flag == NON_LINEAR ? true : false;
//
if (shift_depth)
{
Matrix3d R0, R1;
Vector3d P0, P1;
R0 = back_R0 * ric[0];// 0
R1 = Rs[0] * ric[0];// 1 ， 0
P0 = back_P0 + back_R0 * tic[0];
P1 = Ps[0] + Rs[0] * tic[0];
f_manager.removeBackShiftDepth(R0, P0, R1, P1);
}
//
else
f_manager.removeBack();
}
スライドウィンドウ終了後、フィーチャーポイントマネージャf_managerに格納されている関連フレーム情報は更新する必要があり、具体的なコード実装はremoveBackShiftDepth()とremoveBack()関数である.
void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P)
{
for (auto it = feature.begin(), it_next = feature.begin();
it != feature.end(); it = it_next)
{
it_next++;
// it 0 ,
if (it->start_frame != 0)
it->start_frame--;
else
{
// it
Eigen::Vector3d uv_i = it->feature_per_frame[0].point;
it->feature_per_frame.erase(it->feature_per_frame.begin());
// it 2, it
if (it->feature_per_frame.size() < 2)
{
feature.erase(it);
continue;
}
else
{
// it
Eigen::Vector3d pts_i = uv_i * it->estimated_depth;
Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;
Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);
double dep_j = pts_j(2);
if (dep_j > 0)
it->estimated_depth = dep_j;
else
it->estimated_depth = INIT_DEPTH;
}
}
}
}
void FeatureManager::removeBack()
{
for (auto it = feature.begin(), it_next = feature.begin();
it != feature.end(); it = it_next)
{
it_next++;
// it 0 ,
if (it->start_frame != 0)
it->start_frame--;
else
{
// it
it->feature_per_frame.erase(it->feature_per_frame.begin());
// it 0, it
if (it->feature_per_frame.size() == 0)
feature.erase(it);
}
}
}
簡単な分析は注釈に書いてあるので、続けて読みましょう.
セカンダリの新しいフレームをエッジ化すると、
for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
{
double tmp_dt = dt_buf[frame_count][i];
Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];
pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);
dt_buf[frame_count - 1].push_back(tmp_dt);
linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);
angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
}
Headers[frame_count - 1] = Headers[frame_count];
Ps[frame_count - 1] = Ps[frame_count];
Vs[frame_count - 1] = Vs[frame_count];
Rs[frame_count - 1] = Rs[frame_count];
Bas[frame_count - 1] = Bas[frame_count];
Bgs[frame_count - 1] = Bgs[frame_count];
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
スライド処理前の第10フレームと第9フレームのいくつかの情報を一つにして、スライド処理後の第9フレームのいくつかの情報として、その後、スライド処理前の第10フレームのいくつかの他の情報をスライド処理後の第9フレームに割り当て、最後にスライド処理後の第10フレームに対していくつかの初期処理を行う.
slideWindowNew()
void Estimator::slideWindowNew()
{
sum_of_front++;
f_manager.removeFront(frame_count);
}
void FeatureManager::removeFront(int frame_count)
{
for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)
{
it_next++;
// it 10 ,
if (it->start_frame == frame_count)
{
it->start_frame--;
}
else
{
// 9 it
int j = WINDOW_SIZE - 1 - it->start_frame;
if (it->endFrame() < frame_count - 1)
continue;
it->feature_per_frame.erase(it->feature_per_frame.begin() + j);
// it 0, it
if (it->feature_per_frame.size() == 0)
feature.erase(it);
}
}
}
スライドウィンドウ終了後、フィーチャーポイントマネージャf_managerに格納されている関連フレーム情報は更新する必要があります.
次にprocessImage()関数に戻り、下に分析します.
f_manager.removeFailures();
void FeatureManager::removeFailures()
{
for (auto it = feature.begin(), it_next = feature.begin();
it != feature.end(); it = it_next)
{
it_next++;
if (it->solve_flag == 2)
feature.erase(it);
}
}
フィーチャーポイントマネージャのすべての深度値が負のフィーチャーポイントをクリアします.