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

small fix

This commit is contained in:
Vladyslav Usenko 2019-07-23 15:47:14 +02:00
parent 2f18e1d333
commit 725b965bb3
2 changed files with 24 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/basalt/vi_estimator/nfr_mapper.h
View File

@ -168,7 +168,7 @@ class NfrMapper : public BundleAdjustmentBase {
void processMargData(basalt::MargData& m); void processMargData(basalt::MargData& m);
void extractNonlinearFactors(basalt::MargData& m); bool extractNonlinearFactors(basalt::MargData& m);
void optimize(int num_iterations = 10); void optimize(int num_iterations = 10);

21
src/vi_estimator/nfr_mapper.cpp
View File

@ -53,6 +53,10 @@ NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
} }
void NfrMapper::addMargData(MargData::Ptr& data) { void NfrMapper::addMargData(MargData::Ptr& data) {
processMargData(*data);
bool valid = extractNonlinearFactors(*data);
if (valid) {
for (const auto& kv : data->frame_poses) { for (const auto& kv : data->frame_poses) {
PoseStateWithLin p(kv.second.getT_ns(), kv.second.getPose()); PoseStateWithLin p(kv.second.getT_ns(), kv.second.getPose());
@ -66,9 +70,7 @@ void NfrMapper::addMargData(MargData::Ptr& data) {
frame_poses[kv.first] = p; frame_poses[kv.first] = p;
} }
} }
}
processMargData(*data);
extractNonlinearFactors(*data);
} }
void NfrMapper::processMargData(MargData& m) { void NfrMapper::processMargData(MargData& m) {
@ -138,13 +140,14 @@ void NfrMapper::processMargData(MargData& m) {
} }
} }
void NfrMapper::extractNonlinearFactors(MargData& m) { bool NfrMapper::extractNonlinearFactors(MargData& m) {
size_t asize = m.aom.total_size; size_t asize = m.aom.total_size;
// std::cout << "asize " << asize << std::endl; // std::cout << "asize " << asize << std::endl;
Eigen::MatrixXd cov_old; Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qr(m.abs_H);
cov_old.setIdentity(asize, asize); if (qr.rank() != m.abs_H.cols()) return false;
m.abs_H.ldlt().solveInPlace(cov_old);
Eigen::MatrixXd cov_old = qr.solve(Eigen::MatrixXd::Identity(asize, asize));
int64_t kf_id = *m.kfs_to_marg.cbegin(); int64_t kf_id = *m.kfs_to_marg.cbegin();
int kf_start_idx = m.aom.abs_order_map.at(kf_id).first; int kf_start_idx = m.aom.abs_order_map.at(kf_id).first;
@ -216,6 +219,8 @@ void NfrMapper::extractNonlinearFactors(MargData& m) {
rel_pose_factors.emplace_back(rpf); rel_pose_factors.emplace_back(rpf);
} }
return true;
} }
void NfrMapper::optimize(int num_iterations) { void NfrMapper::optimize(int num_iterations) {
@ -365,8 +370,10 @@ void NfrMapper::computeRollPitch(double& roll_pitch_error) {
void NfrMapper::detect_keypoints() { void NfrMapper::detect_keypoints() {
std::vector<int64_t> keys; std::vector<int64_t> keys;
for (const auto& kv : img_data) { for (const auto& kv : img_data) {
if (frame_poses.count(kv.first) > 0) {
keys.emplace_back(kv.first); keys.emplace_back(kv.first);
} }
}
auto t1 = std::chrono::high_resolution_clock::now(); auto t1 = std::chrono::high_resolution_clock::now();