Added LM options

This commit is contained in:
Vladyslav Usenko 2019-08-12 19:14:47 +02:00
parent 4e4901dfa4
commit aaa82fba3c
5 changed files with 137 additions and 64 deletions

View File

@ -20,6 +20,9 @@
"config.vio_filter_iteration": 4, "config.vio_filter_iteration": 4,
"config.vio_max_iterations": 7, "config.vio_max_iterations": 7,
"config.vio_enforce_realtime": false, "config.vio_enforce_realtime": false,
"config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2,
"config.mapper_obs_std_dev": 0.25, "config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,
@ -33,6 +36,9 @@
"config.mapper_second_best_test_ratio": 1.2, "config.mapper_second_best_test_ratio": 1.2,
"config.mapper_bow_num_bits": 16, "config.mapper_bow_num_bits": 16,
"config.mapper_min_triangulation_dist": 0.07, "config.mapper_min_triangulation_dist": 0.07,
"config.mapper_no_factor_weights": false "config.mapper_no_factor_weights": false,
"config.mapper_use_lm": false,
"config.mapper_lm_lambda_min": 1e-32,
"config.mapper_lm_lambda_max": 1e3
} }
} }

View File

@ -21,6 +21,9 @@
"config.vio_filter_iteration": 4, "config.vio_filter_iteration": 4,
"config.vio_max_iterations": 7, "config.vio_max_iterations": 7,
"config.vio_enforce_realtime": false, "config.vio_enforce_realtime": false,
"config.vio_use_lm": false,
"config.vio_lm_lambda_min": 1e-32,
"config.vio_lm_lambda_max": 1e2,
"config.mapper_obs_std_dev": 0.25, "config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,
@ -34,6 +37,9 @@
"config.mapper_second_best_test_ratio": 1.2, "config.mapper_second_best_test_ratio": 1.2,
"config.mapper_bow_num_bits": 16, "config.mapper_bow_num_bits": 16,
"config.mapper_min_triangulation_dist": 0.07, "config.mapper_min_triangulation_dist": 0.07,
"config.mapper_no_factor_weights": false "config.mapper_no_factor_weights": false,
"config.mapper_use_lm": false,
"config.mapper_lm_lambda_min": 1e-32,
"config.mapper_lm_lambda_max": 1e3
} }
} }

View File

@ -68,6 +68,10 @@ struct VioConfig {
bool vio_enforce_realtime; bool vio_enforce_realtime;
bool vio_use_lm;
double vio_lm_lambda_min;
double vio_lm_lambda_max;
double mapper_obs_std_dev; double mapper_obs_std_dev;
double mapper_obs_huber_thresh; double mapper_obs_huber_thresh;
int mapper_detection_num_points; int mapper_detection_num_points;
@ -81,5 +85,9 @@ struct VioConfig {
int mapper_bow_num_bits; int mapper_bow_num_bits;
double mapper_min_triangulation_dist; double mapper_min_triangulation_dist;
bool mapper_no_factor_weights; bool mapper_no_factor_weights;
bool mapper_use_lm;
double mapper_lm_lambda_min;
double mapper_lm_lambda_max;
}; };
} // namespace basalt } // namespace basalt

View File

@ -68,6 +68,10 @@ VioConfig::VioConfig() {
vio_enforce_realtime = false; vio_enforce_realtime = false;
vio_use_lm = false;
vio_lm_lambda_min = 1e-32;
vio_lm_lambda_max = 1e2;
mapper_obs_std_dev = 0.25; mapper_obs_std_dev = 0.25;
mapper_obs_huber_thresh = 1.5; mapper_obs_huber_thresh = 1.5;
mapper_detection_num_points = 800; mapper_detection_num_points = 800;
@ -81,6 +85,10 @@ VioConfig::VioConfig() {
mapper_bow_num_bits = 16; mapper_bow_num_bits = 16;
mapper_min_triangulation_dist = 0.07; mapper_min_triangulation_dist = 0.07;
mapper_no_factor_weights = false; mapper_no_factor_weights = false;
mapper_use_lm = false;
mapper_lm_lambda_min = 1e-32;
mapper_lm_lambda_max = 1e2;
} }
void VioConfig::save(const std::string& filename) { void VioConfig::save(const std::string& filename) {
@ -132,6 +140,10 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
ar(CEREAL_NVP(config.vio_enforce_realtime)); ar(CEREAL_NVP(config.vio_enforce_realtime));
ar(CEREAL_NVP(config.vio_use_lm));
ar(CEREAL_NVP(config.vio_lm_lambda_min));
ar(CEREAL_NVP(config.vio_lm_lambda_max));
ar(CEREAL_NVP(config.mapper_obs_std_dev)); ar(CEREAL_NVP(config.mapper_obs_std_dev));
ar(CEREAL_NVP(config.mapper_obs_huber_thresh)); ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
ar(CEREAL_NVP(config.mapper_detection_num_points)); ar(CEREAL_NVP(config.mapper_detection_num_points));
@ -145,5 +157,9 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
ar(CEREAL_NVP(config.mapper_bow_num_bits)); ar(CEREAL_NVP(config.mapper_bow_num_bits));
ar(CEREAL_NVP(config.mapper_min_triangulation_dist)); ar(CEREAL_NVP(config.mapper_min_triangulation_dist));
ar(CEREAL_NVP(config.mapper_no_factor_weights)); ar(CEREAL_NVP(config.mapper_no_factor_weights));
ar(CEREAL_NVP(config.mapper_use_lm));
ar(CEREAL_NVP(config.mapper_lm_lambda_min));
ar(CEREAL_NVP(config.mapper_lm_lambda_max));
} }
} // namespace cereal } // namespace cereal

View File

@ -54,9 +54,9 @@ KeypointVioEstimator::KeypointVioEstimator(
g(g), g(g),
initialized(false), initialized(false),
config(config), config(config),
lambda(1e-10), lambda(config.vio_lm_lambda_min),
min_lambda(1e-32), min_lambda(config.vio_lm_lambda_min),
max_lambda(100), max_lambda(config.vio_lm_lambda_max),
lambda_vee(2) { lambda_vee(2) {
this->obs_std_dev = config.vio_obs_std_dev; this->obs_std_dev = config.vio_obs_std_dev;
this->huber_thresh = config.vio_obs_huber_thresh; this->huber_thresh = config.vio_obs_huber_thresh;
@ -945,18 +945,107 @@ void KeypointVioEstimator::optimize() {
double error_total = double error_total =
rld_error + imu_error + marg_prior_error + ba_error + bg_error; rld_error + imu_error + marg_prior_error + ba_error + bg_error;
std::cout << "[LINEARIZE] Error: " << error_total << " num points " if (config.vio_debug)
<< std::endl; std::cout << "[LINEARIZE] Error: " << error_total << " num points "
<< std::endl;
lopt.accum.setup_solver(); lopt.accum.setup_solver();
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
bool converged = false; bool converged = false;
bool step = false;
int max_iter = 10;
while (!step && max_iter > 0 && !converged) { if (config.vio_use_lm) { // Use LevenbergMarquardt
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda; bool step = false;
int max_iter = 10;
while (!step && max_iter > 0 && !converged) {
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
for (int i = 0; i < Hdiag_lambda.size(); i++)
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
double max_inc = inc.array().abs().maxCoeff();
if (max_inc < 1e-4) converged = true;
backup();
// apply increment to poses
for (auto& kv : frame_poses) {
int idx = aom.abs_order_map.at(kv.first).first;
kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
}
// apply increment to states
for (auto& kv : frame_states) {
int idx = aom.abs_order_map.at(kv.first).first;
kv.second.applyInc(-inc.segment<POSE_VEL_BIAS_SIZE>(idx));
}
// Update points
tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
auto update_points_func = [&](const tbb::blocked_range<size_t>& r) {
for (size_t i = r.begin(); i != r.end(); ++i) {
const auto& rld = rld_vec[i];
updatePoints(aom, rld, inc);
}
};
tbb::parallel_for(keys_range, update_points_func);
double after_update_marg_prior_error = 0;
double after_update_vision_error = 0, after_update_imu_error = 0,
after_bg_error = 0, after_ba_error = 0;
computeError(after_update_vision_error);
computeImuError(aom, after_update_imu_error, after_bg_error,
after_ba_error, frame_states, imu_meas,
gyro_bias_weight, accel_bias_weight, g);
computeMargPriorError(after_update_marg_prior_error);
double after_error_total =
after_update_vision_error + after_update_imu_error +
after_update_marg_prior_error + after_bg_error + after_ba_error;
double f_diff = (error_total - after_error_total);
double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB());
// std::cout << "f_diff " << f_diff << " l_diff " << l_diff <<
// std::endl;
double step_quality = f_diff / l_diff;
if (step_quality < 0) {
if (config.vio_debug)
std::cout << "\t[REJECTED] lambda:" << lambda
<< " step_quality: " << step_quality
<< " max_inc: " << max_inc
<< " Error: " << after_error_total << std::endl;
lambda = std::min(max_lambda, lambda_vee * lambda);
lambda_vee *= 2;
restore();
} else {
if (config.vio_debug)
std::cout << "\t[ACCEPTED] lambda:" << lambda
<< " step_quality: " << step_quality
<< " max_inc: " << max_inc
<< " Error: " << after_error_total << std::endl;
lambda = std::max(
min_lambda,
lambda *
std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
lambda_vee = 2;
step = true;
}
max_iter--;
}
if (config.vio_debug && converged) {
std::cout << "[CONVERGED]" << std::endl;
}
} else { // Use Gauss-Newton
Eigen::VectorXd Hdiag_lambda = Hdiag * min_lambda;
for (int i = 0; i < Hdiag_lambda.size(); i++) for (int i = 0; i < Hdiag_lambda.size(); i++)
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda); Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
@ -964,8 +1053,6 @@ void KeypointVioEstimator::optimize() {
double max_inc = inc.array().abs().maxCoeff(); double max_inc = inc.array().abs().maxCoeff();
if (max_inc < 1e-4) converged = true; if (max_inc < 1e-4) converged = true;
backup();
// apply increment to poses // apply increment to poses
for (auto& kv : frame_poses) { for (auto& kv : frame_poses) {
int idx = aom.abs_order_map.at(kv.first).first; int idx = aom.abs_order_map.at(kv.first).first;
@ -987,56 +1074,6 @@ void KeypointVioEstimator::optimize() {
} }
}; };
tbb::parallel_for(keys_range, update_points_func); tbb::parallel_for(keys_range, update_points_func);
double after_update_marg_prior_error = 0;
double after_update_vision_error = 0, after_update_imu_error = 0,
after_bg_error = 0, after_ba_error = 0;
computeError(after_update_vision_error);
computeImuError(aom, after_update_imu_error, after_bg_error,
after_ba_error, frame_states, imu_meas,
gyro_bias_weight, accel_bias_weight, g);
computeMargPriorError(after_update_marg_prior_error);
double after_error_total =
after_update_vision_error + after_update_imu_error +
after_update_marg_prior_error + after_bg_error + after_ba_error;
double f_diff = (error_total - after_error_total);
double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB());
std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
double step_quality = f_diff / l_diff;
if (step_quality < 0) {
std::cout << "\t[REJECTED] lambda:" << lambda
<< " step_quality: " << step_quality
<< " max_inc: " << max_inc
<< " Error: " << after_error_total << std::endl;
lambda = std::min(max_lambda, lambda_vee * lambda);
lambda_vee *= 2;
restore();
} else {
std::cout << "\t[ACCEPTED] lambda:" << lambda
<< " step_quality: " << step_quality
<< " max_inc: " << max_inc
<< " Error: " << after_error_total << std::endl;
lambda = std::max(
min_lambda,
lambda *
std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
lambda_vee = 2;
step = true;
}
max_iter--;
}
if (converged) {
std::cout << "[CONVERGED]" << std::endl;
} }
if (config.vio_debug) { if (config.vio_debug) {
@ -1099,7 +1136,7 @@ void KeypointVioEstimator::optimize() {
if (config.vio_debug) { if (config.vio_debug) {
std::cout << "=================================" << std::endl; std::cout << "=================================" << std::endl;
} }
} } // namespace basalt
void KeypointVioEstimator::computeProjections( void KeypointVioEstimator::computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& data) const { std::vector<Eigen::vector<Eigen::Vector4d>>& data) const {