non-parallel compute_error
This commit is contained in:
Vladyslav Usenko
parent
3d6a4099cf
commit
be249f6354
|
|
@ -131,119 +131,101 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom,
|
|||
}
|
||||
|
||||
void BundleAdjustmentBase::computeError(
|
||||
double& error_all,
|
||||
double& error,
|
||||
std::map<int, std::vector<std::pair<TimeCamId, double>>>* outliers,
|
||||
double outlier_threshold) const {
|
||||
error_all = 0;
|
||||
error = 0;
|
||||
|
||||
std::vector<TimeCamId> 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<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;
|
||||
|
||||
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(
|
||||
|
|
|
|||
|
|
@ -155,6 +155,8 @@ void LandmarkDatabase::removeLandmark(int lm_id) {
|
|||
|
||||
host_to_kpts.at(it->second.kf_id).erase(lm_id);
|
||||
|
||||
std::set<TimeCamId> 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<TimeCamId> 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
|
||||
|
|
|
|||
|
|
@ -47,7 +47,7 @@ NfrMapper::NfrMapper(const Calibration<double>& 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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue