ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/basalt/utils/imu_types.h
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

1
src/vi_estimator/keypoint_vio.cpp
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));

1
src/vi_estimator/keypoint_vo.cpp
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));

24
src/vi_estimator/nfr_mapper.cpp
View File

@ -122,17 +122,19 @@ void NfrMapper::processMargData(MargData& m) {
// << std::endl;
}
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);
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);
// std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "
// << marg_H_new.cols() << std::endl;
// std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "
// << marg_H_new.cols() << std::endl;
m.abs_H = marg_H_new;
m.abs_b = marg_b_new;
m.aom = aom_new;
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,7 +195,9 @@ bool NfrMapper::extractNonlinearFactors(MargData& m) {
rpf.cov_inv.setIdentity();
}
roll_pitch_factors.emplace_back(rpf);
if (m.use_imu) {
roll_pitch_factors.emplace_back(rpf);
}
}
for (int64_t other_id : m.kfs_all) {