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

Small changes to accumulator class

This commit is contained in:
Vladyslav Usenko 2019-07-19 16:28:42 +02:00
parent ec5db8aec0
commit 538d021ae1
2 changed files with 19 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
include/basalt/optimization/accumulator.h
View File

@ -79,26 +79,15 @@ class DenseAccumulator {
b.template segment<ROWS>(i) += data; b.template segment<ROWS>(i) += data;
} }
inline VectorX solve() const { return H.ldlt().solve(b); } // inline VectorX solve() const { return H.ldlt().solve(b); }
inline VectorX solve(const VectorX* diagonal) const {
inline VectorX solveWithPrior(const MatrixX& H_prior, const VectorX& b_prior, if (diagonal == nullptr) {
size_t pos) const { return H.ldlt().solve(b);
const int prior_size = b_prior.rows(); } else {
MatrixX HH = H;
BASALT_ASSERT_STREAM( HH.diagonal() += *diagonal;
H_prior.cols() == prior_size, return HH.ldlt().solve(b);
"H_prior.cols() " << H_prior.cols() << " prior_size " << prior_size); }
BASALT_ASSERT_STREAM(
H_prior.rows() == prior_size,
"H_prior.rows() " << H_prior.rows() << " prior_size " << prior_size);
MatrixX H_new = H;
VectorX b_new = b;
H_new.block(pos, pos, prior_size, prior_size) += H_prior;
b_new.segment(pos, prior_size) += b_prior;
return H_new.ldlt().solve(-b_new);
} }
inline void reset(int opt_size) { inline void reset(int opt_size) {
@ -117,6 +106,9 @@ class DenseAccumulator {
std::cerr << "b\n" << b.transpose().format(CleanFmt) << std::endl; std::cerr << "b\n" << b.transpose().format(CleanFmt) << std::endl;
} }
inline void setup_solver(){};
inline VectorX Hdiagonal() const { return H.diagonal(); }
inline const MatrixX& getH() const { return H; } inline const MatrixX& getH() const { return H; }
inline const VectorX& getB() const { return b; } inline const VectorX& getB() const { return b; }

9
src/vi_estimator/keypoint_vio.cpp
View File

@ -943,8 +943,13 @@ void KeypointVioEstimator::optimize() {
double error_total = double error_total =
rld_error + imu_error + marg_prior_error + ba_error + bg_error; rld_error + imu_error + marg_prior_error + ba_error + bg_error;
lopt.accum.getH().diagonal() *= 1.0001; lopt.accum.setup_solver();
const Eigen::VectorXd inc = lopt.accum.solve(); Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
Hdiag *= 1e-12;
for (int i = 0; i < Hdiag.size(); i++)
Hdiag[i] = std::max(Hdiag[i], 1e-12);
const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag);
// apply increment to poses // apply increment to poses
for (auto& kv : frame_poses) { for (auto& kv : frame_poses) {