809 lines
29 KiB
C++
809 lines
29 KiB
C++
/**
|
|
BSD 3-Clause License
|
|
|
|
This file is part of the Basalt project.
|
|
https://gitlab.com/VladyslavUsenko/basalt.git
|
|
|
|
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
|
|
* Redistributions of source code must retain the above copyright notice, this
|
|
list of conditions and the following disclaimer.
|
|
|
|
* Redistributions in binary form must reproduce the above copyright notice,
|
|
this list of conditions and the following disclaimer in the documentation
|
|
and/or other materials provided with the distribution.
|
|
|
|
* Neither the name of the copyright holder nor the names of its
|
|
contributors may be used to endorse or promote products derived from
|
|
this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
|
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
|
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include <basalt/vi_estimator/sc_ba_base.h>
|
|
|
|
#include <tbb/parallel_for.h>
|
|
|
|
#include <basalt/imu/preintegration.h>
|
|
#include <basalt/utils/ba_utils.h>
|
|
|
|
namespace basalt {
|
|
|
|
template <class Scalar_>
|
|
void ScBundleAdjustmentBase<Scalar_>::updatePoints(
|
|
const AbsOrderMap& aom, const RelLinData& rld, const VecX& inc,
|
|
LandmarkDatabase<Scalar>& lmdb, Scalar* l_diff) {
|
|
// We want to compute the model cost change. The model fuction is
|
|
//
|
|
// L(inc) = 0.5 r^T r + inc^T b + 0.5 inc^T H inc,
|
|
//
|
|
// and thus the expected decrease in cost for the computed increment is
|
|
//
|
|
// l_diff = L(0) - L(inc)
|
|
// = - inc^T b - 0.5 inc^T H inc.
|
|
//
|
|
// Here we have
|
|
//
|
|
// | incp | | bp | | Hpp Hpl |
|
|
// inc = | | b = | | H = | |
|
|
// | incl |, | bl |, | Hlp Hll |,
|
|
//
|
|
// and thus
|
|
//
|
|
// l_diff = - incp^T bp - incl^T bl -
|
|
// 0.5 incp^T Hpp incp - incp^T Hpl incl - 0.5 incl^T Hll incl
|
|
// = - sum_{p} (incp^T (bp + 0.5 Hpp incp)) -
|
|
// sum_{l} (incl^T (bl + 0.5 Hll incl +
|
|
// sum_{obs} (Hpl^T incp))),
|
|
//
|
|
// where sum_{p} sums over all (relative) poses, sum_{l} over all landmarks,
|
|
// and sum_{obs} over all observations of each landmark.
|
|
//
|
|
// Note: l_diff acts like an accumulator; we just add w/o initializing to 0.
|
|
|
|
VecX rel_inc;
|
|
rel_inc.setZero(rld.order.size() * POSE_SIZE);
|
|
for (size_t i = 0; i < rld.order.size(); i++) {
|
|
const TimeCamId& tcid_h = rld.order[i].first;
|
|
const TimeCamId& tcid_t = rld.order[i].second;
|
|
|
|
if (tcid_h.frame_id != tcid_t.frame_id) {
|
|
int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
|
|
int abs_t_idx = aom.abs_order_map.at(tcid_t.frame_id).first;
|
|
|
|
Eigen::Matrix<Scalar, POSE_SIZE, 1> inc_p =
|
|
rld.d_rel_d_h[i] * inc.template segment<POSE_SIZE>(abs_h_idx) +
|
|
rld.d_rel_d_t[i] * inc.template segment<POSE_SIZE>(abs_t_idx);
|
|
|
|
rel_inc.template segment<POSE_SIZE>(i * POSE_SIZE) = inc_p;
|
|
|
|
if (l_diff) {
|
|
// Note: inc_p is still negated b/c we solve H(-x) = b
|
|
const FrameRelLinData& frld = rld.Hpppl[i];
|
|
*l_diff -= -inc_p.dot((frld.bp - Scalar(0.5) * (frld.Hpp * inc_p)));
|
|
}
|
|
}
|
|
}
|
|
|
|
for (const auto& kv : rld.lm_to_obs) {
|
|
int lm_idx = kv.first;
|
|
const auto& lm_obs = kv.second;
|
|
|
|
Vec3 H_l_p_x;
|
|
H_l_p_x.setZero();
|
|
|
|
for (size_t k = 0; k < lm_obs.size(); k++) {
|
|
int rel_idx = lm_obs[k].first;
|
|
const FrameRelLinData& frld = rld.Hpppl.at(rel_idx);
|
|
const Eigen::Matrix<Scalar, POSE_SIZE, 3>& H_p_l_other =
|
|
frld.Hpl.at(lm_obs[k].second);
|
|
|
|
// Note: pose increment is still negated b/c we solve "H (-inc) = b"
|
|
H_l_p_x += H_p_l_other.transpose() *
|
|
rel_inc.template segment<POSE_SIZE>(rel_idx * POSE_SIZE);
|
|
}
|
|
|
|
// final negation of inc_l b/c we solve "H (-inc) = b"
|
|
Vec3 inc_l = -(rld.Hllinv.at(lm_idx) * (rld.bl.at(lm_idx) - H_l_p_x));
|
|
|
|
Keypoint<Scalar>& kpt = lmdb.getLandmark(lm_idx);
|
|
kpt.direction += inc_l.template head<2>();
|
|
kpt.inv_dist += inc_l[2];
|
|
|
|
kpt.inv_dist = std::max(Scalar(0), kpt.inv_dist);
|
|
|
|
if (l_diff) {
|
|
// Note: rel_inc and thus H_l_p_x are still negated b/c we solve H(-x) = b
|
|
*l_diff -= inc_l.transpose() *
|
|
(rld.bl.at(lm_idx) +
|
|
Scalar(0.5) * (rld.Hll.at(lm_idx) * inc_l) - H_l_p_x);
|
|
}
|
|
}
|
|
}
|
|
|
|
template <class Scalar_>
|
|
void ScBundleAdjustmentBase<Scalar_>::updatePointsAbs(
|
|
const AbsOrderMap& aom, const AbsLinData& ald, const VecX& inc,
|
|
LandmarkDatabase<Scalar>& lmdb, Scalar* l_diff) {
|
|
// We want to compute the model cost change. The model fuction is
|
|
//
|
|
// L(inc) = 0.5 r^T r + inc^T b + 0.5 inc^T H inc,
|
|
//
|
|
// and thus the expected decrease in cost for the computed increment is
|
|
//
|
|
// l_diff = L(0) - L(inc)
|
|
// = - inc^T b - 0.5 inc^T H inc.
|
|
//
|
|
// Here we have
|
|
//
|
|
// | incp | | bp | | Hpp Hpl |
|
|
// inc = | | b = | | H = | |
|
|
// | incl |, | bl |, | Hlp Hll |,
|
|
//
|
|
// and thus
|
|
//
|
|
// l_diff = - incp^T bp - incl^T bl -
|
|
// 0.5 incp^T Hpp incp - incp^T Hpl incl - 0.5 incl^T Hll incl
|
|
// = - sum_{p} (incp^T (bp + 0.5 Hpp incp)) -
|
|
// sum_{l} (incl^T (bl + 0.5 Hll incl +
|
|
// sum_{obs} (Hpl^T incp))),
|
|
//
|
|
// where sum_{p} sums over all (relative) poses, sum_{l} over all landmarks,
|
|
// and sum_{obs} over all observations of each landmark.
|
|
//
|
|
// Note: l_diff acts like an accumulator; we just add w/o initializing to 0.
|
|
|
|
if (l_diff) {
|
|
for (size_t i = 0; i < ald.order.size(); i++) {
|
|
const TimeCamId& tcid_h = ald.order[i].first;
|
|
const TimeCamId& tcid_t = ald.order[i].second;
|
|
|
|
int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
|
|
int abs_t_idx = aom.abs_order_map.at(tcid_t.frame_id).first;
|
|
|
|
auto inc_h = inc.template segment<POSE_SIZE>(abs_h_idx);
|
|
auto inc_t = inc.template segment<POSE_SIZE>(abs_t_idx);
|
|
|
|
// Note: inc_p is still negated b/c we solve H(-x) = b
|
|
const FrameAbsLinData& fald = ald.Hpppl[i];
|
|
*l_diff -= -inc_h.dot((fald.bph - Scalar(0.5) * (fald.Hphph * inc_h)) -
|
|
fald.Hphpt * inc_t) -
|
|
inc_t.dot((fald.bpt - Scalar(0.5) * (fald.Hptpt * inc_t)));
|
|
}
|
|
}
|
|
|
|
for (const auto& kv : ald.lm_to_obs) {
|
|
int lm_idx = kv.first;
|
|
const auto& lm_obs = kv.second;
|
|
|
|
Vec3 H_l_p_x;
|
|
H_l_p_x.setZero();
|
|
|
|
for (size_t k = 0; k < lm_obs.size(); k++) {
|
|
int rel_idx = lm_obs[k].first;
|
|
const TimeCamId& tcid_h = ald.order.at(rel_idx).first;
|
|
const TimeCamId& tcid_t = ald.order.at(rel_idx).second;
|
|
|
|
int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
|
|
int abs_t_idx = aom.abs_order_map.at(tcid_t.frame_id).first;
|
|
|
|
auto inc_h = inc.template segment<POSE_SIZE>(abs_h_idx);
|
|
auto inc_t = inc.template segment<POSE_SIZE>(abs_t_idx);
|
|
|
|
const FrameAbsLinData& fald = ald.Hpppl.at(rel_idx);
|
|
const Eigen::Matrix<Scalar, POSE_SIZE, 3>& H_ph_l_other =
|
|
fald.Hphl.at(lm_obs[k].second);
|
|
const Eigen::Matrix<Scalar, POSE_SIZE, 3>& H_pt_l_other =
|
|
fald.Hptl.at(lm_obs[k].second);
|
|
|
|
// Note: pose increment is still negated b/c we solve "H (-inc) = b"
|
|
H_l_p_x += H_ph_l_other.transpose() * inc_h;
|
|
H_l_p_x += H_pt_l_other.transpose() * inc_t;
|
|
}
|
|
|
|
// final negation of inc_l b/c we solve "H (-inc) = b"
|
|
Vec3 inc_l = -(ald.Hllinv.at(lm_idx) * (ald.bl.at(lm_idx) - H_l_p_x));
|
|
|
|
Keypoint<Scalar>& kpt = lmdb.getLandmark(lm_idx);
|
|
kpt.direction += inc_l.template head<2>();
|
|
kpt.inv_dist += inc_l[2];
|
|
|
|
kpt.inv_dist = std::max(Scalar(0), kpt.inv_dist);
|
|
|
|
if (l_diff) {
|
|
// Note: rel_inc and thus H_l_p_x are still negated b/c we solve H(-x) = b
|
|
*l_diff -= inc_l.transpose() *
|
|
(ald.bl.at(lm_idx) +
|
|
Scalar(0.5) * (ald.Hll.at(lm_idx) * inc_l) - H_l_p_x);
|
|
}
|
|
}
|
|
}
|
|
|
|
template <class Scalar_>
|
|
void ScBundleAdjustmentBase<Scalar_>::linearizeHelperStatic(
|
|
Eigen::aligned_vector<RelLinData>& rld_vec,
|
|
const std::unordered_map<
|
|
TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
|
|
const BundleAdjustmentBase<Scalar>* ba_base, Scalar& error) {
|
|
error = 0;
|
|
|
|
rld_vec.clear();
|
|
|
|
std::vector<TimeCamId> obs_tcid_vec;
|
|
for (const auto& kv : obs_to_lin) {
|
|
obs_tcid_vec.emplace_back(kv.first);
|
|
rld_vec.emplace_back(ba_base->lmdb.numLandmarks(), kv.second.size());
|
|
}
|
|
|
|
tbb::parallel_for(
|
|
tbb::blocked_range<size_t>(0, obs_tcid_vec.size()),
|
|
[&](const tbb::blocked_range<size_t>& range) {
|
|
for (size_t r = range.begin(); r != range.end(); ++r) {
|
|
auto kv = obs_to_lin.find(obs_tcid_vec[r]);
|
|
|
|
RelLinData& rld = rld_vec[r];
|
|
|
|
rld.error = Scalar(0);
|
|
|
|
const TimeCamId& tcid_h = kv->first;
|
|
|
|
for (const auto& obs_kv : kv->second) {
|
|
const TimeCamId& tcid_t = obs_kv.first;
|
|
rld.order.emplace_back(std::make_pair(tcid_h, tcid_t));
|
|
|
|
Mat4 T_t_h;
|
|
|
|
if (tcid_h != tcid_t) {
|
|
// target and host are not the same
|
|
PoseStateWithLin state_h =
|
|
ba_base->getPoseStateWithLin(tcid_h.frame_id);
|
|
PoseStateWithLin state_t =
|
|
ba_base->getPoseStateWithLin(tcid_t.frame_id);
|
|
|
|
Mat6 d_rel_d_h, d_rel_d_t;
|
|
|
|
SE3 T_t_h_sophus = computeRelPose(
|
|
state_h.getPoseLin(), ba_base->calib.T_i_c[tcid_h.cam_id],
|
|
state_t.getPoseLin(), ba_base->calib.T_i_c[tcid_t.cam_id],
|
|
&d_rel_d_h, &d_rel_d_t);
|
|
|
|
rld.d_rel_d_h.emplace_back(d_rel_d_h);
|
|
rld.d_rel_d_t.emplace_back(d_rel_d_t);
|
|
|
|
if (state_h.isLinearized() || state_t.isLinearized()) {
|
|
T_t_h_sophus = computeRelPose(
|
|
state_h.getPose(), ba_base->calib.T_i_c[tcid_h.cam_id],
|
|
state_t.getPose(), ba_base->calib.T_i_c[tcid_t.cam_id]);
|
|
}
|
|
|
|
T_t_h = T_t_h_sophus.matrix();
|
|
} else {
|
|
T_t_h.setIdentity();
|
|
rld.d_rel_d_h.emplace_back(Mat6::Zero());
|
|
rld.d_rel_d_t.emplace_back(Mat6::Zero());
|
|
}
|
|
|
|
FrameRelLinData frld;
|
|
|
|
std::visit(
|
|
[&](const auto& cam) {
|
|
for (KeypointId kpt_id : obs_kv.second) {
|
|
const Keypoint<Scalar>& kpt_pos =
|
|
ba_base->lmdb.getLandmark(kpt_id);
|
|
const Vec2& kpt_obs = kpt_pos.obs.at(tcid_t);
|
|
|
|
Vec2 res;
|
|
Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi;
|
|
Eigen::Matrix<Scalar, 2, 3> d_res_d_p;
|
|
|
|
bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam,
|
|
res, &d_res_d_xi, &d_res_d_p);
|
|
|
|
if (valid) {
|
|
Scalar e = res.norm();
|
|
Scalar huber_weight = e < ba_base->huber_thresh
|
|
? Scalar(1.0)
|
|
: ba_base->huber_thresh / e;
|
|
Scalar obs_weight = huber_weight / (ba_base->obs_std_dev *
|
|
ba_base->obs_std_dev);
|
|
|
|
rld.error += Scalar(0.5) * (2 - huber_weight) *
|
|
obs_weight * res.transpose() * res;
|
|
|
|
if (rld.Hll.count(kpt_id) == 0) {
|
|
rld.Hll[kpt_id].setZero();
|
|
rld.bl[kpt_id].setZero();
|
|
}
|
|
|
|
rld.Hll[kpt_id] +=
|
|
obs_weight * d_res_d_p.transpose() * d_res_d_p;
|
|
rld.bl[kpt_id] +=
|
|
obs_weight * d_res_d_p.transpose() * res;
|
|
|
|
frld.Hpp +=
|
|
obs_weight * d_res_d_xi.transpose() * d_res_d_xi;
|
|
frld.bp += obs_weight * d_res_d_xi.transpose() * res;
|
|
frld.Hpl.emplace_back(obs_weight *
|
|
d_res_d_xi.transpose() * d_res_d_p);
|
|
frld.lm_id.emplace_back(kpt_id);
|
|
|
|
rld.lm_to_obs[kpt_id].emplace_back(rld.Hpppl.size(),
|
|
frld.lm_id.size() - 1);
|
|
}
|
|
}
|
|
},
|
|
ba_base->calib.intrinsics[tcid_t.cam_id].variant);
|
|
|
|
rld.Hpppl.emplace_back(frld);
|
|
}
|
|
|
|
rld.invert_keypoint_hessians();
|
|
}
|
|
});
|
|
|
|
for (const auto& rld : rld_vec) error += rld.error;
|
|
}
|
|
|
|
template <class Scalar_>
|
|
void ScBundleAdjustmentBase<Scalar_>::linearizeHelperAbsStatic(
|
|
Eigen::aligned_vector<AbsLinData>& ald_vec,
|
|
const std::unordered_map<
|
|
TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>& obs_to_lin,
|
|
const BundleAdjustmentBase<Scalar>* ba_base, Scalar& error) {
|
|
error = 0;
|
|
|
|
ald_vec.clear();
|
|
|
|
std::vector<TimeCamId> obs_tcid_vec;
|
|
for (const auto& kv : obs_to_lin) {
|
|
obs_tcid_vec.emplace_back(kv.first);
|
|
ald_vec.emplace_back(ba_base->lmdb.numLandmarks(), kv.second.size());
|
|
}
|
|
|
|
tbb::parallel_for(
|
|
tbb::blocked_range<size_t>(0, obs_tcid_vec.size()),
|
|
[&](const tbb::blocked_range<size_t>& range) {
|
|
for (size_t r = range.begin(); r != range.end(); ++r) {
|
|
auto kv = obs_to_lin.find(obs_tcid_vec[r]);
|
|
|
|
AbsLinData& ald = ald_vec[r];
|
|
|
|
ald.error = Scalar(0);
|
|
|
|
const TimeCamId& tcid_h = kv->first;
|
|
|
|
for (const auto& obs_kv : kv->second) {
|
|
const TimeCamId& tcid_t = obs_kv.first;
|
|
ald.order.emplace_back(std::make_pair(tcid_h, tcid_t));
|
|
|
|
Mat4 T_t_h;
|
|
Mat6 d_rel_d_h, d_rel_d_t;
|
|
|
|
if (tcid_h != tcid_t) {
|
|
// target and host are not the same
|
|
PoseStateWithLin state_h =
|
|
ba_base->getPoseStateWithLin(tcid_h.frame_id);
|
|
PoseStateWithLin state_t =
|
|
ba_base->getPoseStateWithLin(tcid_t.frame_id);
|
|
|
|
SE3 T_t_h_sophus = computeRelPose(
|
|
state_h.getPoseLin(), ba_base->calib.T_i_c[tcid_h.cam_id],
|
|
state_t.getPoseLin(), ba_base->calib.T_i_c[tcid_t.cam_id],
|
|
&d_rel_d_h, &d_rel_d_t);
|
|
|
|
if (state_h.isLinearized() || state_t.isLinearized()) {
|
|
T_t_h_sophus = computeRelPose(
|
|
state_h.getPose(), ba_base->calib.T_i_c[tcid_h.cam_id],
|
|
state_t.getPose(), ba_base->calib.T_i_c[tcid_t.cam_id]);
|
|
}
|
|
|
|
T_t_h = T_t_h_sophus.matrix();
|
|
} else {
|
|
T_t_h.setIdentity();
|
|
d_rel_d_h.setZero();
|
|
d_rel_d_t.setZero();
|
|
}
|
|
|
|
FrameAbsLinData fald;
|
|
|
|
std::visit(
|
|
[&](const auto& cam) {
|
|
for (KeypointId kpt_id : obs_kv.second) {
|
|
const Keypoint<Scalar>& kpt_pos =
|
|
ba_base->lmdb.getLandmark(kpt_id);
|
|
const Vec2& kpt_obs = kpt_pos.obs.at(tcid_t);
|
|
|
|
Vec2 res;
|
|
Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi_h,
|
|
d_res_d_xi_t;
|
|
Eigen::Matrix<Scalar, 2, 3> d_res_d_p;
|
|
|
|
bool valid;
|
|
{
|
|
Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi;
|
|
|
|
valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res,
|
|
&d_res_d_xi, &d_res_d_p);
|
|
|
|
d_res_d_xi_h = d_res_d_xi * d_rel_d_h;
|
|
d_res_d_xi_t = d_res_d_xi * d_rel_d_t;
|
|
}
|
|
|
|
if (valid) {
|
|
Scalar e = res.norm();
|
|
Scalar huber_weight = e < ba_base->huber_thresh
|
|
? Scalar(1.0)
|
|
: ba_base->huber_thresh / e;
|
|
Scalar obs_weight = huber_weight / (ba_base->obs_std_dev *
|
|
ba_base->obs_std_dev);
|
|
|
|
ald.error += Scalar(0.5) * (2 - huber_weight) *
|
|
obs_weight * res.transpose() * res;
|
|
|
|
if (ald.Hll.count(kpt_id) == 0) {
|
|
ald.Hll[kpt_id].setZero();
|
|
ald.bl[kpt_id].setZero();
|
|
}
|
|
|
|
ald.Hll[kpt_id] +=
|
|
obs_weight * d_res_d_p.transpose() * d_res_d_p;
|
|
ald.bl[kpt_id] +=
|
|
obs_weight * d_res_d_p.transpose() * res;
|
|
|
|
fald.Hphph +=
|
|
obs_weight * d_res_d_xi_h.transpose() * d_res_d_xi_h;
|
|
fald.Hptpt +=
|
|
obs_weight * d_res_d_xi_t.transpose() * d_res_d_xi_t;
|
|
fald.Hphpt +=
|
|
obs_weight * d_res_d_xi_h.transpose() * d_res_d_xi_t;
|
|
|
|
fald.bph += obs_weight * d_res_d_xi_h.transpose() * res;
|
|
fald.bpt += obs_weight * d_res_d_xi_t.transpose() * res;
|
|
|
|
fald.Hphl.emplace_back(
|
|
obs_weight * d_res_d_xi_h.transpose() * d_res_d_p);
|
|
fald.Hptl.emplace_back(
|
|
obs_weight * d_res_d_xi_t.transpose() * d_res_d_p);
|
|
|
|
fald.lm_id.emplace_back(kpt_id);
|
|
|
|
ald.lm_to_obs[kpt_id].emplace_back(ald.Hpppl.size(),
|
|
fald.lm_id.size() - 1);
|
|
}
|
|
}
|
|
},
|
|
ba_base->calib.intrinsics[tcid_t.cam_id].variant);
|
|
|
|
ald.Hpppl.emplace_back(fald);
|
|
}
|
|
|
|
ald.invert_keypoint_hessians();
|
|
}
|
|
});
|
|
|
|
for (const auto& rld : ald_vec) error += rld.error;
|
|
}
|
|
|
|
template <class Scalar_>
|
|
void ScBundleAdjustmentBase<Scalar_>::linearizeRel(const RelLinData& rld,
|
|
MatX& H, VecX& b) {
|
|
// std::cout << "linearizeRel: KF " << frame_states.size() << " obs "
|
|
// << obs.size() << std::endl;
|
|
|
|
// Do schur complement
|
|
size_t msize = rld.order.size();
|
|
H.setZero(POSE_SIZE * msize, POSE_SIZE * msize);
|
|
b.setZero(POSE_SIZE * msize);
|
|
|
|
for (size_t i = 0; i < rld.order.size(); i++) {
|
|
const FrameRelLinData& frld = rld.Hpppl.at(i);
|
|
|
|
H.template block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i, POSE_SIZE * i) +=
|
|
frld.Hpp;
|
|
b.template segment<POSE_SIZE>(POSE_SIZE * i) += frld.bp;
|
|
|
|
for (size_t j = 0; j < frld.lm_id.size(); j++) {
|
|
Eigen::Matrix<Scalar, POSE_SIZE, 3> H_pl_H_ll_inv;
|
|
int lm_id = frld.lm_id[j];
|
|
|
|
H_pl_H_ll_inv = frld.Hpl[j] * rld.Hllinv.at(lm_id);
|
|
b.template segment<POSE_SIZE>(POSE_SIZE * i) -=
|
|
H_pl_H_ll_inv * rld.bl.at(lm_id);
|
|
|
|
const auto& other_obs = rld.lm_to_obs.at(lm_id);
|
|
for (size_t k = 0; k < other_obs.size(); k++) {
|
|
const FrameRelLinData& frld_other = rld.Hpppl.at(other_obs[k].first);
|
|
int other_i = other_obs[k].first;
|
|
|
|
Eigen::Matrix<Scalar, 3, POSE_SIZE> H_l_p_other =
|
|
frld_other.Hpl[other_obs[k].second].transpose();
|
|
|
|
H.template block<POSE_SIZE, POSE_SIZE>(
|
|
POSE_SIZE * i, POSE_SIZE * other_i) -= H_pl_H_ll_inv * H_l_p_other;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
template <class Scalar_>
|
|
Eigen::VectorXd ScBundleAdjustmentBase<Scalar_>::checkNullspace(
|
|
const MatX& H, const VecX& b, const AbsOrderMap& order,
|
|
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>&
|
|
frame_states,
|
|
const Eigen::aligned_map<int64_t, PoseStateWithLin<Scalar>>& frame_poses,
|
|
bool verbose) {
|
|
using Vec3d = Eigen::Vector3d;
|
|
using VecXd = Eigen::VectorXd;
|
|
using Mat3d = Eigen::Matrix3d;
|
|
using MatXd = Eigen::MatrixXd;
|
|
using SO3d = Sophus::SO3d;
|
|
|
|
BASALT_ASSERT(size_t(H.cols()) == order.total_size);
|
|
size_t marg_size = order.total_size;
|
|
|
|
// Idea: We construct increments that we know should lie in the null-space of
|
|
// the prior from marginalized observations (except for the initial pose prior
|
|
// we set at initialization) --> shift global translations (x,y,z separately),
|
|
// or global rotations (r,p,y separately); for VIO only yaw rotation shift is
|
|
// in nullspace. Compared to a random increment, we expect them to stay small
|
|
// (at initial level of the initial pose prior). If they increase over time,
|
|
// we accumulate spurious information on unobservable degrees of freedom.
|
|
//
|
|
// Poses are cam-to-world and we use left-increment in optimization, so
|
|
// perturbations are in world frame. Hence translational increments are
|
|
// independent. For rotational increments we also need to rotate translations
|
|
// and velocities, both of which are expressed in world frame. For
|
|
// translations, we move the center of rotation to the camera center centroid
|
|
// for better numerics.
|
|
|
|
VecXd inc_x, inc_y, inc_z, inc_roll, inc_pitch, inc_yaw;
|
|
inc_x.setZero(marg_size);
|
|
inc_y.setZero(marg_size);
|
|
inc_z.setZero(marg_size);
|
|
inc_roll.setZero(marg_size);
|
|
inc_pitch.setZero(marg_size);
|
|
inc_yaw.setZero(marg_size);
|
|
|
|
int num_trans = 0;
|
|
Vec3d mean_trans;
|
|
mean_trans.setZero();
|
|
|
|
// Compute mean translation
|
|
for (const auto& kv : order.abs_order_map) {
|
|
Vec3d trans;
|
|
if (kv.second.second == POSE_SIZE) {
|
|
mean_trans += frame_poses.at(kv.first)
|
|
.getPoseLin()
|
|
.translation()
|
|
.template cast<double>();
|
|
num_trans++;
|
|
} else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
|
|
mean_trans += frame_states.at(kv.first)
|
|
.getStateLin()
|
|
.T_w_i.translation()
|
|
.template cast<double>();
|
|
num_trans++;
|
|
} else {
|
|
std::cerr << "Unknown size of the state: " << kv.second.second
|
|
<< std::endl;
|
|
std::abort();
|
|
}
|
|
}
|
|
mean_trans /= num_trans;
|
|
|
|
double eps = 0.01;
|
|
|
|
// Compute nullspace increments
|
|
for (const auto& kv : order.abs_order_map) {
|
|
inc_x(kv.second.first + 0) = eps;
|
|
inc_y(kv.second.first + 1) = eps;
|
|
inc_z(kv.second.first + 2) = eps;
|
|
inc_roll(kv.second.first + 3) = eps;
|
|
inc_pitch(kv.second.first + 4) = eps;
|
|
inc_yaw(kv.second.first + 5) = eps;
|
|
|
|
Vec3d trans;
|
|
if (kv.second.second == POSE_SIZE) {
|
|
trans = frame_poses.at(kv.first)
|
|
.getPoseLin()
|
|
.translation()
|
|
.template cast<double>();
|
|
} else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
|
|
trans = frame_states.at(kv.first)
|
|
.getStateLin()
|
|
.T_w_i.translation()
|
|
.template cast<double>();
|
|
} else {
|
|
BASALT_ASSERT(false);
|
|
}
|
|
|
|
// combine rotation with global translation to make it rotation around
|
|
// translation centroid, not around origin (better numerics). Note that
|
|
// velocities are not affected by global translation.
|
|
trans -= mean_trans;
|
|
|
|
// Jacobian of translation w.r.t. the rotation increment (one column each
|
|
// for the 3 different increments)
|
|
Mat3d J = -SO3d::hat(trans);
|
|
J *= eps;
|
|
|
|
inc_roll.template segment<3>(kv.second.first) = J.col(0);
|
|
inc_pitch.template segment<3>(kv.second.first) = J.col(1);
|
|
inc_yaw.template segment<3>(kv.second.first) = J.col(2);
|
|
|
|
if (kv.second.second == POSE_VEL_BIAS_SIZE) {
|
|
Vec3d vel = frame_states.at(kv.first)
|
|
.getStateLin()
|
|
.vel_w_i.template cast<double>();
|
|
|
|
// Jacobian of velocity w.r.t. the rotation increment (one column each
|
|
// for the 3 different increments)
|
|
Mat3d J_vel = -SO3d::hat(vel);
|
|
J_vel *= eps;
|
|
|
|
inc_roll.template segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(0);
|
|
inc_pitch.template segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(1);
|
|
inc_yaw.template segment<3>(kv.second.first + POSE_SIZE) = J_vel.col(2);
|
|
}
|
|
}
|
|
|
|
inc_x.normalize();
|
|
inc_y.normalize();
|
|
inc_z.normalize();
|
|
inc_roll.normalize();
|
|
inc_pitch.normalize();
|
|
inc_yaw.normalize();
|
|
|
|
// std::cout << "inc_x " << inc_x.transpose() << std::endl;
|
|
// std::cout << "inc_y " << inc_y.transpose() << std::endl;
|
|
// std::cout << "inc_z " << inc_z.transpose() << std::endl;
|
|
// std::cout << "inc_yaw " << inc_yaw.transpose() << std::endl;
|
|
|
|
VecXd inc_random;
|
|
inc_random.setRandom(marg_size);
|
|
inc_random.normalize();
|
|
|
|
MatXd H_d = H.template cast<double>();
|
|
VecXd b_d = b.template cast<double>();
|
|
|
|
VecXd xHx(7);
|
|
VecXd xb(7);
|
|
|
|
xHx[0] = inc_x.transpose() * H_d * inc_x;
|
|
xHx[1] = inc_y.transpose() * H_d * inc_y;
|
|
xHx[2] = inc_z.transpose() * H_d * inc_z;
|
|
xHx[3] = inc_roll.transpose() * H_d * inc_roll;
|
|
xHx[4] = inc_pitch.transpose() * H_d * inc_pitch;
|
|
xHx[5] = inc_yaw.transpose() * H_d * inc_yaw;
|
|
xHx[6] = inc_random.transpose() * H_d * inc_random;
|
|
|
|
// b == J^T r, so the increments should also lie in left-nullspace of b
|
|
xb[0] = inc_x.transpose() * b_d;
|
|
xb[1] = inc_y.transpose() * b_d;
|
|
xb[2] = inc_z.transpose() * b_d;
|
|
xb[3] = inc_roll.transpose() * b_d;
|
|
xb[4] = inc_pitch.transpose() * b_d;
|
|
xb[5] = inc_yaw.transpose() * b_d;
|
|
xb[6] = inc_random.transpose() * b_d;
|
|
|
|
if (verbose) {
|
|
std::cout << "nullspace x_trans: " << xHx[0] << " + " << xb[0] << std::endl;
|
|
std::cout << "nullspace y_trans: " << xHx[1] << " + " << xb[1] << std::endl;
|
|
std::cout << "nullspace z_trans: " << xHx[2] << " + " << xb[2] << std::endl;
|
|
std::cout << "nullspace roll : " << xHx[3] << " + " << xb[3] << std::endl;
|
|
std::cout << "nullspace pitch : " << xHx[4] << " + " << xb[4] << std::endl;
|
|
std::cout << "nullspace yaw : " << xHx[5] << " + " << xb[5] << std::endl;
|
|
std::cout << "nullspace random : " << xHx[6] << " + " << xb[6] << std::endl;
|
|
}
|
|
|
|
return xHx + xb;
|
|
}
|
|
|
|
template <class Scalar_>
|
|
Eigen::VectorXd ScBundleAdjustmentBase<Scalar_>::checkEigenvalues(
|
|
const MatX& H, bool verbose) {
|
|
// For EV, we use SelfAdjointEigenSolver to avoid getting (numerically)
|
|
// complex eigenvalues.
|
|
|
|
// We do this computation in double precision to avoid any inaccuracies that
|
|
// come from the squaring.
|
|
|
|
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(
|
|
H.template cast<double>());
|
|
if (eigensolver.info() != Eigen::Success) {
|
|
BASALT_LOG_FATAL("eigen solver failed");
|
|
}
|
|
|
|
if (verbose) {
|
|
std::cout << "EV:\n" << eigensolver.eigenvalues() << std::endl;
|
|
}
|
|
|
|
return eigensolver.eigenvalues();
|
|
}
|
|
|
|
template <class Scalar_>
|
|
void ScBundleAdjustmentBase<Scalar_>::computeImuError(
|
|
const AbsOrderMap& aom, Scalar& imu_error, Scalar& bg_error,
|
|
Scalar& ba_error,
|
|
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<Scalar>>& states,
|
|
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<Scalar>>&
|
|
imu_meas,
|
|
const Vec3& gyro_bias_weight, const Vec3& accel_bias_weight,
|
|
const Vec3& g) {
|
|
imu_error = 0;
|
|
bg_error = 0;
|
|
ba_error = 0;
|
|
for (const auto& kv : imu_meas) {
|
|
if (kv.second.get_dt_ns() != 0) {
|
|
int64_t start_t = kv.second.get_start_t_ns();
|
|
int64_t end_t = kv.second.get_start_t_ns() + kv.second.get_dt_ns();
|
|
|
|
if (aom.abs_order_map.count(start_t) == 0 ||
|
|
aom.abs_order_map.count(end_t) == 0)
|
|
continue;
|
|
|
|
PoseVelBiasStateWithLin<Scalar> start_state = states.at(start_t);
|
|
PoseVelBiasStateWithLin<Scalar> end_state = states.at(end_t);
|
|
|
|
const typename PoseVelState<Scalar>::VecN res = kv.second.residual(
|
|
start_state.getState(), g, end_state.getState(),
|
|
start_state.getState().bias_gyro, start_state.getState().bias_accel);
|
|
|
|
// std::cout << "res: (" << start_t << "," << end_t << ") "
|
|
// << res.transpose() << std::endl;
|
|
|
|
// std::cerr << "cov_inv:\n" << kv.second.get_cov_inv() <<
|
|
// std::endl;
|
|
|
|
imu_error +=
|
|
Scalar(0.5) * res.transpose() * kv.second.get_cov_inv() * res;
|
|
|
|
Scalar dt = kv.second.get_dt_ns() * Scalar(1e-9);
|
|
{
|
|
Vec3 gyro_bias_weight_dt = gyro_bias_weight / dt;
|
|
Vec3 res_bg =
|
|
start_state.getState().bias_gyro - end_state.getState().bias_gyro;
|
|
|
|
bg_error += Scalar(0.5) * res_bg.transpose() *
|
|
gyro_bias_weight_dt.asDiagonal() * res_bg;
|
|
}
|
|
|
|
{
|
|
Vec3 accel_bias_weight_dt = accel_bias_weight / dt;
|
|
Vec3 res_ba =
|
|
start_state.getState().bias_accel - end_state.getState().bias_accel;
|
|
|
|
ba_error += Scalar(0.5) * res_ba.transpose() *
|
|
accel_bias_weight_dt.asDiagonal() * res_ba;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// //////////////////////////////////////////////////////////////////
|
|
// instatiate templates
|
|
|
|
// Note: double specialization is unconditional, b/c NfrMapper depends on it.
|
|
//#ifdef BASALT_INSTANTIATIONS_DOUBLE
|
|
template class ScBundleAdjustmentBase<double>;
|
|
//#endif
|
|
|
|
#ifdef BASALT_INSTANTIATIONS_FLOAT
|
|
template class ScBundleAdjustmentBase<float>;
|
|
#endif
|
|
|
|
} // namespace basalt
|