small changes to mapping

This commit is contained in:
Vladyslav Usenko 2019-08-23 14:02:55 +02:00
parent d4da96119f
commit 402228d52e
4 changed files with 18 additions and 10 deletions

View File

@ -274,6 +274,7 @@ struct MargData {
Eigen::map<int64_t, PoseStateWithLin> frame_poses;
std::set<int64_t> kfs_all;
std::set<int64_t> kfs_to_marg;
bool use_imu;
std::vector<OpticalFlowResult::Ptr> opt_flow_res;
};
@ -316,6 +317,7 @@ void serialize(Archive& ar, basalt::MargData& m) {
ar(m.frame_states);
ar(m.kfs_all);
ar(m.kfs_to_marg);
ar(m.use_imu);
}
} // namespace cereal

View File

@ -626,6 +626,7 @@ void KeypointVioEstimator::marginalize(
m->frame_states = frame_states;
m->kfs_all = kf_ids_all;
m->kfs_to_marg = kfs_to_marg;
m->use_imu = true;
for (int64_t t : m->kfs_all) {
m->opt_flow_res.emplace_back(prev_opt_flow_res.at(t));

View File

@ -537,6 +537,7 @@ void KeypointVoEstimator::marginalize(
m->frame_states = frame_states;
m->kfs_all = kf_ids_all;
m->kfs_to_marg = kfs_to_marg;
m->use_imu = false;
for (int64_t t : m->kfs_all) {
m->opt_flow_res.emplace_back(prev_opt_flow_res.at(t));

View File

@ -122,10 +122,11 @@ void NfrMapper::processMargData(MargData& m) {
// << std::endl;
}
if (!idx_to_marg.empty()) {
Eigen::MatrixXd marg_H_new;
Eigen::VectorXd marg_b_new;
BundleAdjustmentBase::marginalizeHelper(m.abs_H, m.abs_b, idx_to_keep,
idx_to_marg, marg_H_new, marg_b_new);
BundleAdjustmentBase::marginalizeHelper(
m.abs_H, m.abs_b, idx_to_keep, idx_to_marg, marg_H_new, marg_b_new);
// std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "
// << marg_H_new.cols() << std::endl;
@ -133,6 +134,7 @@ void NfrMapper::processMargData(MargData& m) {
m.abs_H = marg_H_new;
m.abs_b = marg_b_new;
m.aom = aom_new;
}
BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols()));
@ -193,8 +195,10 @@ bool NfrMapper::extractNonlinearFactors(MargData& m) {
rpf.cov_inv.setIdentity();
}
if (m.use_imu) {
roll_pitch_factors.emplace_back(rpf);
}
}
for (int64_t other_id : m.kfs_all) {
if (m.frame_poses.count(other_id) == 0 || other_id == kf_id) {