Changes to LM

This commit is contained in:
Vladyslav Usenko 2019-06-27 15:56:55 +02:00
parent 7ecd04d9eb
commit b3e0a101d2
4 changed files with 41 additions and 30 deletions

View File

@ -220,24 +220,11 @@ class SparseHashAccumulator {
b.template segment<ROWS>(i) += data;
}
inline VectorX solve(Scalar alpha = 1e-6) const {
SparseMatrix sm(b.rows(), b.rows());
auto t1 = std::chrono::high_resolution_clock::now();
inline void setup_solver() {
std::vector<T> triplets;
triplets.reserve(hash_map.size() * 36 + b.rows());
if (alpha > 0)
for (int i = 0; i < b.rows(); i++) {
triplets.emplace_back(i, i, alpha);
}
for (const auto& kv : hash_map) {
// if (kv.first[2] != kv.second.rows()) std::cerr << "rows mismatch" <<
// std::endl;
// if (kv.first[3] != kv.second.cols()) std::cerr << "cols mismatch" <<
// std::endl;
for (int i = 0; i < kv.second.rows(); i++) {
for (int j = 0; j < kv.second.cols(); j++) {
triplets.emplace_back(kv.first[0] + i, kv.first[1] + j,
@ -246,14 +233,21 @@ class SparseHashAccumulator {
}
}
sm.setFromTriplets(triplets.begin(), triplets.end());
for (int i = 0; i < b.rows(); i++) {
triplets.emplace_back(i, i, std::numeric_limits<double>::min());
}
smm = SparseMatrix(b.rows(), b.rows());
smm.setFromTriplets(triplets.begin(), triplets.end());
}
inline VectorX Hdiagonal() const { return smm.diagonal(); }
inline VectorX solve(const VectorX* diagonal) const {
auto t2 = std::chrono::high_resolution_clock::now();
// sm.diagonal() *= 1.01;
// Eigen::IOFormat CleanFmt(2);
// std::cerr << "sm\n" << sm.toDense().format(CleanFmt) << std::endl;
SparseMatrix sm = smm;
if (diagonal) sm.diagonal() += *diagonal;
VectorX res;
@ -272,15 +266,12 @@ class SparseHashAccumulator {
auto t3 = std::chrono::high_resolution_clock::now();
auto elapsed1 =
std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
auto elapsed2 =
std::chrono::duration_cast<std::chrono::microseconds>(t3 - t2);
if (print_info) {
std::cout << "Forming matrix: " << elapsed1.count() * 1e-6
<< "s. Solving linear system: " << elapsed2.count() * 1e-6
<< "s." << std::endl;
std::cout << "Solving linear system: " << elapsed2.count() * 1e-6 << "s."
<< std::endl;
}
return res;
@ -330,6 +321,8 @@ class SparseHashAccumulator {
std::unordered_map<KeyT, MatrixX, KeyHash> hash_map;
VectorX b;
SparseMatrix smm;
};
} // namespace basalt

View File

@ -60,7 +60,7 @@ class PosesOptimization {
typename Eigen::vector<AprilgridCornersData>::const_iterator;
public:
PosesOptimization() : lambda(1e-6), min_lambda(1e-6), max_lambda(10) {}
PosesOptimization() : lambda(1e-12), min_lambda(1e-12), max_lambda(10) {}
bool initializeIntrinsics(
size_t cam_id, const Eigen::vector<Eigen::Vector2d> &corners,
@ -149,6 +149,9 @@ class PosesOptimization {
std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
<< lopt.num_points << std::endl;
lopt.accum.setup_solver();
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
bool step = false;
int max_iter = 10;
@ -157,7 +160,11 @@ class PosesOptimization {
timestam_to_pose;
Calibration<Scalar> calib_backup = *calib;
Eigen::VectorXd inc = -lopt.accum.solve(lambda);
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
for (int i = 0; i < Hdiag_lambda.size(); i++)
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
Eigen::VectorXd inc = -lopt.accum.solve(&Hdiag_lambda);
for (auto &kv : timestam_to_pose) {
kv.second *=

View File

@ -89,8 +89,8 @@ class SplineOptimization {
SplineOptimization(int64_t dt_ns = 1e7)
: pose_var(1e-4),
lambda(1),
min_lambda(1e-6),
lambda(1e-12),
min_lambda(1e-12),
max_lambda(10),
spline(dt_ns),
dt_ns(dt_ns) {
@ -405,11 +405,18 @@ class SplineOptimization {
std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
<< lopt.num_points << std::endl;
lopt.accum.setup_solver();
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
bool step = false;
int max_iter = 10;
while (!step && max_iter > 0) {
VectorX inc_full = -lopt.accum.solve(lambda);
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
for (int i = 0; i < Hdiag_lambda.size(); i++)
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
VectorX inc_full = -lopt.accum.solve(&Hdiag_lambda);
Calibration<Scalar> calib_backup = *calib;
MocapCalibration<Scalar> mocap_calib_backup = *mocap_calib;

View File

@ -270,7 +270,11 @@ void NfrMapper::optimize(int num_iterations) {
lopt.accum.iterative_solver = true;
lopt.accum.print_info = true;
const Eigen::VectorXd inc = lopt.accum.solve();
lopt.accum.setup_solver();
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
Hdiag.setConstant(Hdiag.size(), 1e-6);
const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag);
// apply increment to poses
for (auto& kv : frame_poses) {