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

1
src/vi_estimator/keypoint_vio.cpp
View File

@ -626,6 +626,7 @@ void KeypointVioEstimator::marginalize(
m->frame_states = frame_states; m->frame_states = frame_states;
m->kfs_all = kf_ids_all; m->kfs_all = kf_ids_all;
m->kfs_to_marg = kfs_to_marg; m->kfs_to_marg = kfs_to_marg;
m->use_imu = true;
for (int64_t t : m->kfs_all) { for (int64_t t : m->kfs_all) {
m->opt_flow_res.emplace_back(prev_opt_flow_res.at(t)); 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->frame_states = frame_states;
m->kfs_all = kf_ids_all; m->kfs_all = kf_ids_all;
m->kfs_to_marg = kfs_to_marg; m->kfs_to_marg = kfs_to_marg;
m->use_imu = false;
for (int64_t t : m->kfs_all) { for (int64_t t : m->kfs_all) {
m->opt_flow_res.emplace_back(prev_opt_flow_res.at(t)); 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; // << std::endl;
} }
Eigen::MatrixXd marg_H_new; if (!idx_to_marg.empty()) {
Eigen::VectorXd marg_b_new; Eigen::MatrixXd marg_H_new;
BundleAdjustmentBase::marginalizeHelper(m.abs_H, m.abs_b, idx_to_keep, Eigen::VectorXd marg_b_new;
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 " // std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "
// << marg_H_new.cols() << std::endl; // << marg_H_new.cols() << std::endl;
m.abs_H = marg_H_new; m.abs_H = marg_H_new;
m.abs_b = marg_b_new; m.abs_b = marg_b_new;
m.aom = aom_new; m.aom = aom_new;
}
BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols())); 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(); 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) { for (int64_t other_id : m.kfs_all) {