diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index 92c4e22..2b823bb 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -37,6 +37,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + namespace basalt { Sophus::SE3d BundleAdjustmentBase::computeRelPose(const Sophus::SE3d& T_w_i_h, @@ -129,101 +131,119 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom, } void BundleAdjustmentBase::computeError( - double& error, + double& error_all, std::map>>* outliers, double outlier_threshold) const { - error = 0; + error_all = 0; + std::vector obs_tcid_vec; for (const auto& kv : lmdb.getObservations()) { - 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); - } - } + obs_tcid_vec.emplace_back(kv.first); } + + 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( @@ -465,8 +485,9 @@ void BundleAdjustmentBase::filterOutliers(double outlier_threshold, std::map>> outliers; computeError(error, &outliers, outlier_threshold); - // std::cout << "============================================" << std::endl; - // std::cout << "Num landmarks: " << lmdb.numLandmarks() << " with outliners + // std::cout << "============================================" << + // std::endl; std::cout << "Num landmarks: " << lmdb.numLandmarks() << " + // with outliners // " // << outliers.size() << std::endl; @@ -498,7 +519,8 @@ void BundleAdjustmentBase::filterOutliers(double outlier_threshold, } } - // std::cout << "============================================" << std::endl; + // std::cout << "============================================" << + // std::endl; } void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H, @@ -552,7 +574,8 @@ void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H, // H_mm_inv = abs_H.bottomRightCorner(marg_size, marg_size) // .fullPivHouseholderQr() - // .solve(Eigen::MatrixXd::Identity(marg_size, marg_size)); + // .solve(Eigen::MatrixXd::Identity(marg_size, + // marg_size)); abs_H.topRightCorner(keep_size, marg_size) *= H_mm_inv;