Added LM to mapping
This commit is contained in:
parent
43c9914592
commit
3d6a4099cf
|
@ -205,5 +205,7 @@ class NfrMapper : public BundleAdjustmentBase {
|
||||||
std::shared_ptr<HashBow<256>> hash_bow_database;
|
std::shared_ptr<HashBow<256>> hash_bow_database;
|
||||||
|
|
||||||
VioConfig config;
|
VioConfig config;
|
||||||
|
|
||||||
|
double lambda, min_lambda, max_lambda, lambda_vee;
|
||||||
};
|
};
|
||||||
} // namespace basalt
|
} // namespace basalt
|
||||||
|
|
|
@ -44,7 +44,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
namespace basalt {
|
namespace basalt {
|
||||||
|
|
||||||
NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
|
NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
|
||||||
: config(config) {
|
: config(config),
|
||||||
|
lambda(1e-10),
|
||||||
|
min_lambda(1e-32),
|
||||||
|
max_lambda(100),
|
||||||
|
lambda_vee(2) {
|
||||||
this->calib = calib;
|
this->calib = calib;
|
||||||
this->obs_std_dev = config.mapper_obs_std_dev;
|
this->obs_std_dev = config.mapper_obs_std_dev;
|
||||||
this->huber_thresh = config.mapper_obs_huber_thresh;
|
this->huber_thresh = config.mapper_obs_huber_thresh;
|
||||||
|
@ -274,7 +278,7 @@ void NfrMapper::optimize(int num_iterations) {
|
||||||
|
|
||||||
double error_total = rld_error + lopt.rel_error + lopt.roll_pitch_error;
|
double error_total = rld_error + lopt.rel_error + lopt.roll_pitch_error;
|
||||||
|
|
||||||
std::cout << "iter " << iter
|
std::cout << "[LINEARIZE] iter " << iter
|
||||||
<< " before_update_error: vision: " << rld_error
|
<< " before_update_error: vision: " << rld_error
|
||||||
<< " rel_error: " << lopt.rel_error
|
<< " rel_error: " << lopt.rel_error
|
||||||
<< " roll_pitch_error: " << lopt.roll_pitch_error
|
<< " roll_pitch_error: " << lopt.roll_pitch_error
|
||||||
|
@ -285,60 +289,103 @@ void NfrMapper::optimize(int num_iterations) {
|
||||||
|
|
||||||
lopt.accum.setup_solver();
|
lopt.accum.setup_solver();
|
||||||
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
|
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
|
||||||
Hdiag.setConstant(Hdiag.size(), 1e-6);
|
|
||||||
|
|
||||||
const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag);
|
bool converged = false;
|
||||||
|
bool step = false;
|
||||||
|
int max_iter = 10;
|
||||||
|
|
||||||
// apply increment to poses
|
while (!step && max_iter > 0 && !converged) {
|
||||||
for (auto& kv : frame_poses) {
|
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
|
||||||
int idx = aom.abs_order_map.at(kv.first).first;
|
for (int i = 0; i < Hdiag_lambda.size(); i++)
|
||||||
BASALT_ASSERT(!kv.second.isLinearized());
|
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
|
||||||
kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
|
|
||||||
|
const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
|
||||||
|
double max_inc = inc.array().abs().maxCoeff();
|
||||||
|
if (max_inc < 1e-5) converged = true;
|
||||||
|
|
||||||
|
backup();
|
||||||
|
|
||||||
|
// apply increment to poses
|
||||||
|
for (auto& kv : frame_poses) {
|
||||||
|
int idx = aom.abs_order_map.at(kv.first).first;
|
||||||
|
BASALT_ASSERT(!kv.second.isLinearized());
|
||||||
|
kv.second.applyInc(-inc.segment<POSE_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_vision_error = 0;
|
||||||
|
double after_rel_error = 0;
|
||||||
|
double after_roll_pitch_error = 0;
|
||||||
|
|
||||||
|
computeError(after_update_vision_error);
|
||||||
|
computeRelPose(after_rel_error);
|
||||||
|
computeRollPitch(after_roll_pitch_error);
|
||||||
|
|
||||||
|
double after_error_total =
|
||||||
|
after_update_vision_error + after_rel_error + after_roll_pitch_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
|
||||||
|
<< " vision_error: " << after_update_vision_error
|
||||||
|
<< " rel_error: " << after_rel_error
|
||||||
|
<< " roll_pitch_error: " << after_roll_pitch_error
|
||||||
|
<< " total: " << 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
|
||||||
|
<< " vision_error: " << after_update_vision_error
|
||||||
|
<< " rel_error: " << after_rel_error
|
||||||
|
<< " roll_pitch_error: " << after_roll_pitch_error
|
||||||
|
<< " total: " << 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 (after_error_total > error_total) {
|
||||||
|
std::cout << "increased error after update!!!" << std::endl;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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_vision_error = 0;
|
|
||||||
double after_rel_error = 0;
|
|
||||||
double after_roll_pitch_error = 0;
|
|
||||||
|
|
||||||
computeError(after_update_vision_error);
|
|
||||||
computeRelPose(after_rel_error);
|
|
||||||
computeRollPitch(after_roll_pitch_error);
|
|
||||||
|
|
||||||
double after_error_total =
|
|
||||||
after_update_vision_error + after_rel_error + after_roll_pitch_error;
|
|
||||||
|
|
||||||
double error_diff = error_total - after_error_total;
|
|
||||||
|
|
||||||
auto t2 = std::chrono::high_resolution_clock::now();
|
auto t2 = std::chrono::high_resolution_clock::now();
|
||||||
|
|
||||||
auto elapsed =
|
auto elapsed =
|
||||||
std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
|
std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
|
||||||
|
|
||||||
std::cout << "iter " << iter
|
std::cout << "iter " << iter << " time : " << elapsed.count()
|
||||||
<< " after_update_error: vision: " << after_update_vision_error
|
<< "(us), num_states " << frame_states.size() << " num_poses "
|
||||||
<< " rel_error: " << after_rel_error
|
<< frame_poses.size() << std::endl;
|
||||||
<< " roll_pitch_error: " << after_roll_pitch_error
|
|
||||||
<< " total: " << after_error_total << " max_inc "
|
|
||||||
<< inc.array().abs().maxCoeff() << " error_diff " << error_diff
|
|
||||||
<< " time : " << elapsed.count() << "(us), num_states "
|
|
||||||
<< frame_states.size() << " num_poses " << frame_poses.size()
|
|
||||||
<< std::endl;
|
|
||||||
|
|
||||||
if (after_error_total > error_total) {
|
if (converged) break;
|
||||||
std::cout << "increased error after update!!!" << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (inc.array().abs().maxCoeff() < 1e-4) break;
|
|
||||||
|
|
||||||
// std::cerr << "LT\n" << LT << std::endl;
|
// std::cerr << "LT\n" << LT << std::endl;
|
||||||
// std::cerr << "z_p\n" << z_p.transpose() << std::endl;
|
// std::cerr << "z_p\n" << z_p.transpose() << std::endl;
|
||||||
|
@ -508,7 +555,8 @@ void NfrMapper::match_all() {
|
||||||
|
|
||||||
// std::cout << "Closest frames for " << tcid << ": ";
|
// std::cout << "Closest frames for " << tcid << ": ";
|
||||||
for (const auto& otcid_score : results) {
|
for (const auto& otcid_score : results) {
|
||||||
// std::cout << otcid_score.first << "(" << otcid_score.second << ") ";
|
// std::cout << otcid_score.first << "(" << otcid_score.second << ")
|
||||||
|
// ";
|
||||||
if (otcid_score.first.frame_id != tcid.frame_id &&
|
if (otcid_score.first.frame_id != tcid.frame_id &&
|
||||||
otcid_score.second > config.mapper_frames_to_match_threshold) {
|
otcid_score.second > config.mapper_frames_to_match_threshold) {
|
||||||
match_pair m;
|
match_pair m;
|
||||||
|
|
Loading…
Reference in New Issue