ktyl/basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
basalt/src/linearization/linearization_abs_sc.cpp
Nikolaus Demmel 24325f2a06 ICCV'21 square root marginalization paper code release 
Major changes:

- New square-root implementation for optimization and
  marginalization, giving faster optimization and numerically
  more stable marginalization. The square root solver is the new
  default, but the Schur complement based implementation is still
  available. (Implements the ICCV'21 paper.)

- The odometry estimator is now fully templetized and you can run
  in float or double. Default is float, which works well with the
  new square-root implementation and gives best runtimes.

- Batch evaluation scripts and documentation to reproduce the
  ICCV'21 experiments.

Additional changes:

- New options in VIO to marginalize lost landmark right away and
  not only when the frame is marginalized (enabled by default).

- small bugfix for keypoint patch extraction bounds

- basalt_vio: more logging for batch evaluation

- basalt_vio: better handling of closing the GUI while estimator is still running

- basalt_vio: new command line argument to limit the number of frames processed

- basalt_vio: new command line argument to save ground truth trajectory

- added unit tests for square root marginalization

- update basalt-headers

- new submodules: gmt, nlohmann/json, magic_enum
2021-10-15 15:09:15 +02:00

433 lines
14 KiB
C++
Raw Blame History

/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git
Copyright (c) 2021, 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/linearization/linearization_abs_sc.hpp>
#include <tbb/blocked_range.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>
#include <basalt/utils/ba_utils.h>
#include <basalt/vi_estimator/sc_ba_base.h>
#include <basalt/linearization/imu_block.hpp>
#include <basalt/utils/cast_utils.hpp>
namespace basalt {
template <typename Scalar, int POSE_SIZE>
LinearizationAbsSC<Scalar, POSE_SIZE>::LinearizationAbsSC(
BundleAdjustmentBase<Scalar>* estimator, const AbsOrderMap& aom,
const Options& options, const MargLinData<Scalar>* marg_lin_data,
const ImuLinData<Scalar>* imu_lin_data,
const std::set<FrameId>* used_frames,
const std::unordered_set<KeypointId>* lost_landmarks,
int64_t last_state_to_marg)
: options_(options),
estimator(estimator),
lmdb_(estimator->lmdb),
calib(estimator->calib),
aom(aom),
used_frames(used_frames),
marg_lin_data(marg_lin_data),
imu_lin_data(imu_lin_data),
lost_landmarks(lost_landmarks),
last_state_to_marg(last_state_to_marg),
pose_damping_diagonal(0),
pose_damping_diagonal_sqrt(0) {
BASALT_ASSERT_STREAM(
options.lb_options.huber_parameter == estimator->huber_thresh,
"Huber threshold should be set to the same value");
BASALT_ASSERT_STREAM(options.lb_options.obs_std_dev == estimator->obs_std_dev,
"obs_std_dev should be set to the same value");
if (imu_lin_data) {
for (const auto& kv : imu_lin_data->imu_meas) {
imu_blocks.emplace_back(
new ImuBlock<Scalar>(kv.second, imu_lin_data, aom));
}
}
// std::cout << "num_rows_Q2r " << num_rows_Q2r << " num_poses " <<
// num_cameras
// << std::endl;
}
template <typename Scalar, int POSE_SIZE>
LinearizationAbsSC<Scalar, POSE_SIZE>::~LinearizationAbsSC() = default;
template <typename Scalar_, int POSE_SIZE_>
void LinearizationAbsSC<Scalar_, POSE_SIZE_>::log_problem_stats(
ExecutionStats& stats) const {
UNUSED(stats);
}
template <typename Scalar, int POSE_SIZE>
Scalar LinearizationAbsSC<Scalar, POSE_SIZE>::linearizeProblem(
bool* numerically_valid) {
// reset damping and scaling (might be set from previous iteration)
pose_damping_diagonal = 0;
pose_damping_diagonal_sqrt = 0;
marg_scaling = VecX();
std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>>
obs_to_lin;
if (used_frames) {
const auto& obs = lmdb_.getObservations();
// select all observations where the host frame is about to be
// marginalized
if (lost_landmarks) {
for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
if (used_frames->count(it->first.frame_id) > 0) {
for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
++it2) {
if (it2->first.frame_id <= last_state_to_marg)
obs_to_lin[it->first].emplace(*it2);
}
} else {
std::map<TimeCamId, std::set<KeypointId>> lost_obs_map;
for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
++it2) {
for (const auto& lm_id : it2->second) {
if (lost_landmarks->count(lm_id)) {
if (it2->first.frame_id <= last_state_to_marg)
lost_obs_map[it2->first].emplace(lm_id);
}
}
}
if (!lost_obs_map.empty()) {
obs_to_lin[it->first] = lost_obs_map;
}
}
}
} else {
for (auto it = obs.cbegin(); it != obs.cend(); ++it) {
if (used_frames->count(it->first.frame_id) > 0) {
for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
++it2) {
if (it2->first.frame_id <= last_state_to_marg)
obs_to_lin[it->first].emplace(*it2);
}
}
}
}
} else {
obs_to_lin = lmdb_.getObservations();
}
Scalar error;
ScBundleAdjustmentBase<Scalar>::linearizeHelperAbsStatic(ald_vec, obs_to_lin,
estimator, error);
// TODO: Fix the computation of numarically valid points
if (numerically_valid) *numerically_valid = true;
if (imu_lin_data) {
for (auto& imu_block : imu_blocks) {
error += imu_block->linearizeImu(estimator->frame_states);
}
}
if (marg_lin_data) {
Scalar marg_prior_error;
estimator->computeMargPriorError(*marg_lin_data, marg_prior_error);
error += marg_prior_error;
}
return error;
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::performQR() {}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::setPoseDamping(
const Scalar lambda) {
BASALT_ASSERT(lambda >= 0);
pose_damping_diagonal = lambda;
pose_damping_diagonal_sqrt = std::sqrt(lambda);
}
template <typename Scalar, int POSE_SIZE>
Scalar LinearizationAbsSC<Scalar, POSE_SIZE>::backSubstitute(
const VecX& pose_inc) {
BASALT_ASSERT(pose_inc.size() == signed_cast(aom.total_size));
// Update points
tbb::blocked_range<size_t> keys_range(0, ald_vec.size());
auto update_points_func = [&](const tbb::blocked_range<size_t>& r,
Scalar l_diff) {
for (size_t i = r.begin(); i != r.end(); ++i) {
const auto& ald = ald_vec[i];
ScBundleAdjustmentBase<Scalar>::updatePointsAbs(aom, ald, -pose_inc,
lmdb_, &l_diff);
}
return l_diff;
};
Scalar l_diff = tbb::parallel_reduce(keys_range, Scalar(0),
update_points_func, std::plus<Scalar>());
if (imu_lin_data) {
for (auto& imu_block : imu_blocks) {
imu_block->backSubstitute(pose_inc, l_diff);
}
}
if (marg_lin_data) {
size_t marg_size = marg_lin_data->H.cols();
VecX pose_inc_marg = pose_inc.head(marg_size);
l_diff += estimator->computeMargPriorModelCostChange(
*marg_lin_data, marg_scaling, pose_inc_marg);
}
return l_diff;
}
template <typename Scalar, int POSE_SIZE>
typename LinearizationAbsSC<Scalar, POSE_SIZE>::VecX
LinearizationAbsSC<Scalar, POSE_SIZE>::getJp_diag2() const {
// TODO: group relative by host frame
BASALT_ASSERT_STREAM(false, "Not implemented");
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::scaleJl_cols() {
BASALT_ASSERT_STREAM(false, "Not implemented");
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::scaleJp_cols(
const VecX& jacobian_scaling) {
UNUSED(jacobian_scaling);
BASALT_ASSERT_STREAM(false, "Not implemented");
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::setLandmarkDamping(Scalar lambda) {
UNUSED(lambda);
BASALT_ASSERT_STREAM(false, "Not implemented");
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r(
MatX& Q2Jp, VecX& Q2r) const {
MatX H;
VecX b;
get_dense_H_b(H, b);
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
VecX D_sqrt = ldlt.vectorD().array().max(0).sqrt().matrix();
// After LDLT, we have
// marg_H == P^T*L*D*L^T*P,
// so we compute the square root as
// marg_sqrt_H = sqrt(D)*L^T*P,
// such that
// marg_sqrt_H^T marg_sqrt_H == marg_H.
Q2Jp.setIdentity(H.rows(), H.cols());
Q2Jp = ldlt.transpositionsP() * Q2Jp;
Q2Jp = ldlt.matrixU() * Q2Jp; // U == L^T
Q2Jp = D_sqrt.asDiagonal() * Q2Jp;
// For the right hand side, we want
// marg_b == marg_sqrt_H^T * marg_sqrt_b
// so we compute
// marg_sqrt_b = (marg_sqrt_H^T)^-1 * marg_b
// = (P^T*L*sqrt(D))^-1 * marg_b
// = sqrt(D)^-1 * L^-1 * P * marg_b
Q2r = ldlt.transpositionsP() * b;
ldlt.matrixL().solveInPlace(Q2r);
// We already clamped negative values in D_sqrt to 0 above, but for values
// close to 0 we set b to 0.
for (int i = 0; i < Q2r.size(); ++i) {
if (D_sqrt(i) > std::sqrt(std::numeric_limits<Scalar>::min()))
Q2r(i) /= D_sqrt(i);
else
Q2r(i) = 0;
}
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_H_b(MatX& H,
VecX& b) const {
typename ScBundleAdjustmentBase<Scalar>::template LinearizeAbsReduce2<
DenseAccumulator<Scalar>>
lopt_abs(aom);
tbb::blocked_range<typename Eigen::aligned_vector<AbsLinData>::const_iterator>
range(ald_vec.cbegin(), ald_vec.cend());
tbb::parallel_reduce(range, lopt_abs);
// lopt_abs(range);
// Add imu
add_dense_H_b_imu(lopt_abs.accum.getH(), lopt_abs.accum.getB());
// Add damping
add_dense_H_b_pose_damping(lopt_abs.accum.getH());
// Add marginalization
add_dense_H_b_marg_prior(lopt_abs.accum.getH(), lopt_abs.accum.getB());
H = std::move(lopt_abs.accum.getH());
b = std::move(lopt_abs.accum.getB());
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_pose_damping(
MatX& Q2Jp, size_t start_idx) const {
UNUSED(Q2Jp);
UNUSED(start_idx);
BASALT_ASSERT_STREAM(false, "Not implemented");
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::get_dense_Q2Jp_Q2r_marg_prior(
MatX& Q2Jp, VecX& Q2r, size_t start_idx) const {
if (!marg_lin_data) return;
size_t marg_rows = marg_lin_data->H.rows();
size_t marg_cols = marg_lin_data->H.cols();
VecX delta;
estimator->computeDelta(marg_lin_data->order, delta);
if (marg_scaling.rows() > 0) {
Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) =
marg_lin_data->H * marg_scaling.asDiagonal();
} else {
Q2Jp.template block(start_idx, 0, marg_rows, marg_cols) = marg_lin_data->H;
}
Q2r.template segment(start_idx, marg_rows) =
marg_lin_data->H * delta + marg_lin_data->b;
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_pose_damping(
MatX& H) const {
if (hasPoseDamping()) {
H.diagonal().array() += pose_damping_diagonal;
}
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_marg_prior(
MatX& H, VecX& b) const {
if (!marg_lin_data) return;
BASALT_ASSERT(marg_scaling.rows() == 0);
Scalar marg_prior_error;
estimator->linearizeMargPrior(*marg_lin_data, aom, H, b, marg_prior_error);
// size_t marg_size = marg_lin_data->H.cols();
// VecX delta;
// estimator->computeDelta(marg_lin_data->order, delta);
// Scaling not supported ATM
// if (marg_scaling.rows() > 0) {
// H.topLeftCorner(marg_size, marg_size) +=
// marg_scaling.asDiagonal() * marg_lin_data->H.transpose() *
// marg_lin_data->H * marg_scaling.asDiagonal();
// b.head(marg_size) += marg_scaling.asDiagonal() *
// marg_lin_data->H.transpose() *
// (marg_lin_data->H * delta + marg_lin_data->b);
// } else {
// H.topLeftCorner(marg_size, marg_size) +=
// marg_lin_data->H.transpose() * marg_lin_data->H;
// b.head(marg_size) += marg_lin_data->H.transpose() *
// (marg_lin_data->H * delta + marg_lin_data->b);
// }
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(
DenseAccumulator<Scalar>& accum) const {
if (!imu_lin_data) return;
for (const auto& imu_block : imu_blocks) {
imu_block->add_dense_H_b(accum);
}
}
template <typename Scalar, int POSE_SIZE>
void LinearizationAbsSC<Scalar, POSE_SIZE>::add_dense_H_b_imu(MatX& H,
VecX& b) const {
if (!imu_lin_data) return;
// workaround: create an accumulator here, to avoid implementing the
// add_dense_H_b(H, b) overload in ImuBlock
DenseAccumulator<Scalar> accum;
accum.reset(b.size());
for (const auto& imu_block : imu_blocks) {
imu_block->add_dense_H_b(accum);
}
H += accum.getH();
b += accum.getB();
}
// //////////////////////////////////////////////////////////////////
// instatiate templates
#ifdef BASALT_INSTANTIATIONS_DOUBLE
// Scalar=double, POSE_SIZE=6
template class LinearizationAbsSC<double, 6>;
#endif
#ifdef BASALT_INSTANTIATIONS_FLOAT
// Scalar=float, POSE_SIZE=6
template class LinearizationAbsSC<float, 6>;
#endif
} // namespace basalt
Reference in New Issue View Git Blame Copy Permalink