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

parallel version of compute error

Browse Source
This commit is contained in:
Vladyslav Usenko 2019-08-08 14:56:51 +02:00
parent 29ed498aa0
commit 43c9914592
1 changed files with 117 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/vi_estimator/ba_base.cpp
View File

@ -37,6 +37,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <tbb/parallel_for.h> #include <tbb/parallel_for.h>
#include <mutex>
namespace basalt { namespace basalt {
Sophus::SE3d BundleAdjustmentBase::computeRelPose(const Sophus::SE3d& T_w_i_h, Sophus::SE3d BundleAdjustmentBase::computeRelPose(const Sophus::SE3d& T_w_i_h,
@ -129,15 +131,28 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom,
} }
void BundleAdjustmentBase::computeError( void BundleAdjustmentBase::computeError(
double& error, double& error_all,
std::map<int, std::vector<std::pair<TimeCamId, double>>>* outliers, std::map<int, std::vector<std::pair<TimeCamId, double>>>* outliers,
double outlier_threshold) const { double outlier_threshold) const {
error = 0; error_all = 0;
std::vector<TimeCamId> obs_tcid_vec;
for (const auto& kv : lmdb.getObservations()) { for (const auto& kv : lmdb.getObservations()) {
const TimeCamId& tcid_h = kv.first; obs_tcid_vec.emplace_back(kv.first);
}
for (const auto& obs_kv : kv.second) { std::mutex res_mutex;
tbb::parallel_for(
tbb::blocked_range<size_t>(0, obs_tcid_vec.size()),
[&](const tbb::blocked_range<size_t>& 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; const TimeCamId& tcid_t = obs_kv.first;
if (tcid_h != tcid_t) { if (tcid_h != tcid_t) {
@ -159,7 +174,8 @@ void BundleAdjustmentBase::computeError(
Eigen::Vector2d res; Eigen::Vector2d res;
bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res); bool valid =
linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res);
if (valid) { if (valid) {
double e = res.norm(); double e = res.norm();
@ -173,8 +189,8 @@ void BundleAdjustmentBase::computeError(
double obs_weight = double obs_weight =
huber_weight / (obs_std_dev * obs_std_dev); huber_weight / (obs_std_dev * obs_std_dev);
error += error += (2 - huber_weight) * obs_weight *
(2 - huber_weight) * obs_weight * res.transpose() * res; res.transpose() * res;
} else { } else {
if (outliers) { if (outliers) {
(*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1); (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1);
@ -211,8 +227,8 @@ void BundleAdjustmentBase::computeError(
double obs_weight = double obs_weight =
huber_weight / (obs_std_dev * obs_std_dev); huber_weight / (obs_std_dev * obs_std_dev);
error += error += (2 - huber_weight) * obs_weight *
(2 - huber_weight) * obs_weight * res.transpose() * res; res.transpose() * res;
} else { } else {
if (outliers) { if (outliers) {
(*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2); (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2);
@ -224,6 +240,10 @@ void BundleAdjustmentBase::computeError(
} }
} }
} }
std::scoped_lock l(res_mutex);
error_all += error;
});
} }
void BundleAdjustmentBase::linearizeHelper( void BundleAdjustmentBase::linearizeHelper(
@ -465,8 +485,9 @@ void BundleAdjustmentBase::filterOutliers(double outlier_threshold,
std::map<int, std::vector<std::pair<TimeCamId, double>>> outliers; std::map<int, std::vector<std::pair<TimeCamId, double>>> outliers;
computeError(error, &outliers, outlier_threshold); computeError(error, &outliers, outlier_threshold);
// std::cout << "============================================" << std::endl; // std::cout << "============================================" <<
// std::cout << "Num landmarks: " << lmdb.numLandmarks() << " with outliners // std::endl; std::cout << "Num landmarks: " << lmdb.numLandmarks() << "
// with outliners
// " // "
// << outliers.size() << std::endl; // << 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, 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) // H_mm_inv = abs_H.bottomRightCorner(marg_size, marg_size)
// .fullPivHouseholderQr() // .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; abs_H.topRightCorner(keep_size, marg_size) *= H_mm_inv;