diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index 2b823bb..f5d8f33 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -131,119 +131,101 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom, } void BundleAdjustmentBase::computeError( - double& error_all, + double& error, std::map>>* outliers, double outlier_threshold) const { - error_all = 0; + error = 0; - std::vector obs_tcid_vec; for (const auto& kv : lmdb.getObservations()) { - obs_tcid_vec.emplace_back(kv.first); + const TimeCamId& tcid_h = kv.first; + + for (const auto& obs_kv : kv.second) { + const TimeCamId& tcid_t = obs_kv.first; + + if (tcid_h != tcid_t) { + PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id); + PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id); + + Sophus::SE3d T_t_h_sophus = + computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id], + state_t.getPose(), calib.T_i_c[tcid_t.cam_id]); + + Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); + + Eigen::Vector2d res; + + bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res); + + if (valid) { + double e = res.norm(); + + if (outliers && e > outlier_threshold) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, e); + } + + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + error += + (2 - huber_weight) * obs_weight * res.transpose() * res; + } else { + if (outliers) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1); + } + } + } + }, + calib.intrinsics[tcid_t.cam_id].variant); + + } else { + // target and host are the same + // residual does not depend on the pose + // it just depends on the point + + std::visit( + [&](const auto& cam) { + for (size_t i = 0; i < obs_kv.second.size(); i++) { + const KeypointObservation& kpt_obs = obs_kv.second[i]; + const KeypointPosition& kpt_pos = + lmdb.getLandmark(kpt_obs.kpt_id); + + Eigen::Vector2d res; + + bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res); + if (valid) { + double e = res.norm(); + + if (outliers && e > outlier_threshold) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); + } + + double huber_weight = + e < huber_thresh ? 1.0 : huber_thresh / e; + double obs_weight = + huber_weight / (obs_std_dev * obs_std_dev); + + error += + (2 - huber_weight) * obs_weight * res.transpose() * res; + } else { + if (outliers) { + (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); + } + } + } + }, + calib.intrinsics[tcid_t.cam_id].variant); + } + } } - - std::mutex res_mutex; - - tbb::parallel_for( - tbb::blocked_range(0, obs_tcid_vec.size()), - [&](const tbb::blocked_range& range) { - double error = 0; - - for (size_t r = range.begin(); r != range.end(); ++r) { - auto kv = lmdb.getObservations().find(obs_tcid_vec[r]); - const TimeCamId& tcid_h = kv->first; - - for (const auto& obs_kv : kv->second) { - const TimeCamId& tcid_t = obs_kv.first; - - if (tcid_h != tcid_t) { - PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id); - PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id); - - Sophus::SE3d T_t_h_sophus = - computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id], - state_t.getPose(), calib.T_i_c[tcid_t.cam_id]); - - Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix(); - - std::visit( - [&](const auto& cam) { - for (size_t i = 0; i < obs_kv.second.size(); i++) { - const KeypointObservation& kpt_obs = obs_kv.second[i]; - const KeypointPosition& kpt_pos = - lmdb.getLandmark(kpt_obs.kpt_id); - - Eigen::Vector2d res; - - bool valid = - linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res); - - if (valid) { - double e = res.norm(); - - if (outliers && e > outlier_threshold) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, e); - } - - double huber_weight = - e < huber_thresh ? 1.0 : huber_thresh / e; - double obs_weight = - huber_weight / (obs_std_dev * obs_std_dev); - - error += (2 - huber_weight) * obs_weight * - res.transpose() * res; - } else { - if (outliers) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1); - } - } - } - }, - calib.intrinsics[tcid_t.cam_id].variant); - - } else { - // target and host are the same - // residual does not depend on the pose - // it just depends on the point - - std::visit( - [&](const auto& cam) { - for (size_t i = 0; i < obs_kv.second.size(); i++) { - const KeypointObservation& kpt_obs = obs_kv.second[i]; - const KeypointPosition& kpt_pos = - lmdb.getLandmark(kpt_obs.kpt_id); - - Eigen::Vector2d res; - - bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res); - if (valid) { - double e = res.norm(); - - if (outliers && e > outlier_threshold) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); - } - - double huber_weight = - e < huber_thresh ? 1.0 : huber_thresh / e; - double obs_weight = - huber_weight / (obs_std_dev * obs_std_dev); - - error += (2 - huber_weight) * obs_weight * - res.transpose() * res; - } else { - if (outliers) { - (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); - } - } - } - }, - calib.intrinsics[tcid_t.cam_id].variant); - } - } - } - - std::scoped_lock l(res_mutex); - error_all += error; - }); } void BundleAdjustmentBase::linearizeHelper( diff --git a/src/vi_estimator/landmark_database.cpp b/src/vi_estimator/landmark_database.cpp index decdb35..b06b931 100644 --- a/src/vi_estimator/landmark_database.cpp +++ b/src/vi_estimator/landmark_database.cpp @@ -155,6 +155,8 @@ void LandmarkDatabase::removeLandmark(int lm_id) { host_to_kpts.at(it->second.kf_id).erase(lm_id); + std::set to_remove; + for (auto &kv : obs.at(it->second.kf_id)) { int idx = -1; for (size_t i = 0; i < kv.second.size(); ++i) { @@ -165,14 +167,22 @@ void LandmarkDatabase::removeLandmark(int lm_id) { } if (idx >= 0) { + BASALT_ASSERT(kv.second.size() > 0); + std::swap(kv.second[idx], kv.second[kv.second.size() - 1]); kv.second.resize(kv.second.size() - 1); num_observations--; kpts_num_obs.at(lm_id)--; + + if (kv.second.size() == 0) to_remove.insert(kv.first); } } + for (const auto &v : to_remove) { + obs.at(it->second.kf_id).erase(v); + } + BASALT_ASSERT_STREAM(kpts_num_obs.at(lm_id) == 0, kpts_num_obs.at(lm_id)); kpts_num_obs.erase(lm_id); kpts.erase(lm_id); @@ -183,6 +193,8 @@ void LandmarkDatabase::removeObservations(int lm_id, auto it = kpts.find(lm_id); BASALT_ASSERT(it != kpts.end()); + std::set to_remove; + for (auto &kv : obs.at(it->second.kf_id)) { if (outliers.count(kv.first) > 0) { int idx = -1; @@ -193,14 +205,21 @@ void LandmarkDatabase::removeObservations(int lm_id, } } BASALT_ASSERT(idx >= 0); + BASALT_ASSERT(kv.second.size() > 0); std::swap(kv.second[idx], kv.second[kv.second.size() - 1]); kv.second.resize(kv.second.size() - 1); num_observations--; kpts_num_obs.at(lm_id)--; + + if (kv.second.size() == 0) to_remove.insert(kv.first); } } + + for (const auto &v : to_remove) { + obs.at(it->second.kf_id).erase(v); + } } } // namespace basalt diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index 88bcbb0..949119d 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -47,7 +47,7 @@ NfrMapper::NfrMapper(const Calibration& calib, const VioConfig& config) : config(config), lambda(1e-10), min_lambda(1e-32), - max_lambda(100), + max_lambda(1000), lambda_vee(2) { this->calib = calib; this->obs_std_dev = config.mapper_obs_std_dev; @@ -288,7 +288,7 @@ void NfrMapper::optimize(int num_iterations) { lopt.accum.print_info = true; lopt.accum.setup_solver(); - Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); + const Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); bool converged = false; bool step = false;