parallel version of compute error
This commit is contained in:
parent
29ed498aa0
commit
43c9914592
|
@ -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;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue