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

improve LM

This commit is contained in:
Vladyslav Usenko 2019-07-09 16:50:08 +02:00
parent d20c9012e9
commit 84212bc314
5 changed files with 153 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -129,65 +129,6 @@ class DenseAccumulator {
VectorX b;
};
template <typename Scalar = double>
class SparseAccumulator {
public:
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Triplet<Scalar> T;
typedef Eigen::SparseMatrix<Scalar> SparseMatrix;
template <int ROWS, int COLS, typename Derived>
inline void addH(int si, int sj, const Eigen::MatrixBase<Derived>& data) {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, ROWS, COLS);
for (int i = 0; i < ROWS; i++) {
for (int j = 0; j < COLS; j++) {
triplets.emplace_back(si + i, sj + j, data(i, j));
}
}
}
template <int ROWS, typename Derived>
inline void addB(int i, const Eigen::MatrixBase<Derived>& data) {
b.template segment<ROWS>(i) += data;
}
inline VectorX solve() const {
SparseMatrix sm(b.rows(), b.rows());
auto triplets_copy = triplets;
for (int i = 0; i < b.rows(); i++) {
triplets_copy.emplace_back(i, i, 0.000001);
}
sm.setFromTriplets(triplets_copy.begin(), triplets_copy.end());
// Eigen::IOFormat CleanFmt(2);
// std::cerr << "sm\n" << sm.toDense().format(CleanFmt) << std::endl;
Eigen::SimplicialLDLT<SparseMatrix> chol(sm);
return chol.solve(-b);
// return sm.toDense().ldlt().solve(-b);
}
inline void reset(int opt_size) {
triplets.clear();
b.setZero(opt_size);
}
inline void join(const SparseAccumulator<Scalar>& other) {
triplets.reserve(triplets.size() + other.triplets.size());
triplets.insert(triplets.end(), other.triplets.begin(),
other.triplets.end());
b += other.b;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
std::vector<T> triplets;
VectorX b;
};
template <typename Scalar = double>
class SparseHashAccumulator {
public:
@ -243,6 +184,8 @@ class SparseHashAccumulator {
inline VectorX Hdiagonal() const { return smm.diagonal(); }
inline VectorX& getB() { return b; }
inline VectorX solve(const VectorX* diagonal) const {
auto t2 = std::chrono::high_resolution_clock::now();

38
include/basalt/optimization/poses_optimize.h
View File

@ -60,7 +60,8 @@ class PosesOptimization {
typename Eigen::vector<AprilgridCornersData>::const_iterator;
public:
PosesOptimization() : lambda(1e-6), min_lambda(1e-12), max_lambda(10) {}
PosesOptimization()
: lambda(1e-6), min_lambda(1e-12), max_lambda(10), lambda_vee(2) {}
bool initializeIntrinsics(
size_t cam_id, const Eigen::vector<Eigen::Vector2d> &corners,
@ -126,7 +127,8 @@ class PosesOptimization {
bool calibInitialized() const { return calib != nullptr; }
bool initialized() const { return true; }
void optimize(bool opt_intrinsics, double huber_thresh, double &error,
// Returns true when converged
bool optimize(bool opt_intrinsics, double huber_thresh, double &error,
int &num_points, double &reprojection_error) {
error = 0;
num_points = 0;
@ -152,10 +154,11 @@ class PosesOptimization {
lopt.accum.setup_solver();
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
bool converged = false;
bool step = false;
int max_iter = 10;
while (!step && max_iter > 0) {
while (!step && max_iter > 0 && !converged) {
Eigen::unordered_map<int64_t, Sophus::SE3d> timestam_to_pose_backup =
timestam_to_pose;
Calibration<Scalar> calib_backup = *calib;
@ -165,6 +168,7 @@ class PosesOptimization {
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
Eigen::VectorXd inc = -lopt.accum.solve(&Hdiag_lambda);
if (inc.array().abs().maxCoeff() < 1e-10) converged = true;
for (auto &kv : timestam_to_pose) {
kv.second *=
@ -184,20 +188,34 @@ class PosesOptimization {
ComputeErrorPosesOpt<double> eopt(problem_size, timestam_to_pose, ccd);
tbb::parallel_reduce(april_range, eopt);
if (eopt.error > lopt.error) {
double f_diff = (lopt.error - eopt.error);
double l_diff = 0.5 * inc.dot(inc * lambda - lopt.accum.getB());
// std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
double step_quality = f_diff / l_diff;
if (step_quality < 0) {
std::cout << "\t[REJECTED] lambda:" << lambda
<< " step_quality: " << step_quality
<< " Error: " << eopt.error << " num points "
<< eopt.num_points << std::endl;
lambda = std::min(max_lambda, 2 * lambda);
lambda = std::min(max_lambda, lambda_vee * lambda);
lambda_vee *= 2;
timestam_to_pose = timestam_to_pose_backup;
*calib = calib_backup;
} else {
std::cout << "\t[ACCEPTED] lambda:" << lambda
<< " step_quality: " << step_quality
<< " Error: " << eopt.error << " num points "
<< eopt.num_points << std::endl;
lambda = std::max(min_lambda, lambda / 2);
lambda = std::max(
min_lambda,
lambda *
std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
lambda_vee = 2;
error = eopt.error;
num_points = eopt.num_points;
@ -207,6 +225,12 @@ class PosesOptimization {
}
max_iter--;
}
if (converged) {
std::cout << "[CONVERGED]" << std::endl;
}
return converged;
}
void recompute_size() {
@ -288,7 +312,7 @@ class PosesOptimization {
typename LinearizePosesOpt<
Scalar, SparseHashAccumulator<Scalar>>::CalibCommonData ccd;
Scalar lambda, min_lambda, max_lambda;
Scalar lambda, min_lambda, max_lambda, lambda_vee;
size_t problem_size;

200
include/basalt/optimization/spline_optimize.h
View File

@ -90,8 +90,9 @@ class SplineOptimization {
SplineOptimization(int64_t dt_ns = 1e7)
: pose_var(1e-4),
lambda(1e-12),
min_lambda(1e-12),
min_lambda(1e-18),
max_lambda(10),
lambda_vee(2),
spline(dt_ns),
dt_ns(dt_ns) {
pose_var_inv = 1.0 / pose_var;
@ -349,125 +350,146 @@ class SplineOptimization {
// << std::endl;
}
void optimize(bool use_intr, bool use_poses, bool use_april_corners,
// Returns true when converged
bool optimize(bool use_intr, bool use_poses, bool use_april_corners,
bool opt_cam_time_offset, bool opt_imu_scale, bool use_mocap,
double huber_thresh, double& error, int& num_points,
double& reprojection_error) {
// std::cerr << "optimize num_knots " << num_knots << std::endl;
for (int i = 0; i < 1; i++) {
ccd.opt_intrinsics = use_intr;
ccd.opt_cam_time_offset = opt_cam_time_offset;
ccd.opt_imu_scale = opt_imu_scale;
ccd.huber_thresh = huber_thresh;
ccd.opt_intrinsics = use_intr;
ccd.opt_cam_time_offset = opt_cam_time_offset;
ccd.opt_imu_scale = opt_imu_scale;
ccd.huber_thresh = huber_thresh;
LinearizeT lopt(opt_size, &spline, ccd);
LinearizeT lopt(opt_size, &spline, ccd);
// auto t1 = std::chrono::high_resolution_clock::now();
// auto t1 = std::chrono::high_resolution_clock::now();
tbb::blocked_range<PoseDataIter> pose_range(pose_measurements.begin(),
pose_measurements.end());
tbb::blocked_range<AprilgridCornersDataIter> april_range(
aprilgrid_corners_measurements.begin(),
aprilgrid_corners_measurements.end());
tbb::blocked_range<PoseDataIter> pose_range(pose_measurements.begin(),
pose_measurements.end());
tbb::blocked_range<AprilgridCornersDataIter> april_range(
aprilgrid_corners_measurements.begin(),
aprilgrid_corners_measurements.end());
tbb::blocked_range<MocapPoseDataIter> mocap_pose_range(
mocap_measurements.begin(), mocap_measurements.end());
tbb::blocked_range<MocapPoseDataIter> mocap_pose_range(
mocap_measurements.begin(), mocap_measurements.end());
tbb::blocked_range<AccelDataIter> accel_range(accel_measurements.begin(),
accel_measurements.end());
tbb::blocked_range<AccelDataIter> accel_range(accel_measurements.begin(),
accel_measurements.end());
tbb::blocked_range<GyroDataIter> gyro_range(gyro_measurements.begin(),
gyro_measurements.end());
tbb::blocked_range<GyroDataIter> gyro_range(gyro_measurements.begin(),
gyro_measurements.end());
if (use_poses) {
tbb::parallel_reduce(pose_range, lopt);
// lopt(pose_range);
}
if (use_april_corners) {
tbb::parallel_reduce(april_range, lopt);
// lopt(april_range);
}
if (use_mocap) {
tbb::parallel_reduce(mocap_pose_range, lopt);
// lopt(mocap_pose_range);
}
tbb::parallel_reduce(accel_range, lopt);
tbb::parallel_reduce(gyro_range, lopt);
error = lopt.error;
num_points = lopt.num_points;
reprojection_error = lopt.reprojection_error;
std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
<< lopt.num_points << std::endl;
lopt.accum.setup_solver();
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
bool converged = false;
bool step = false;
int max_iter = 10;
while (!step && max_iter > 0 && !converged) {
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);
if (inc_full.array().abs().maxCoeff() < 1e-10) converged = true;
Calibration<Scalar> calib_backup = *calib;
MocapCalibration<Scalar> mocap_calib_backup = *mocap_calib;
SplineT spline_backup = spline;
Vector3 g_backup = g;
applyInc(inc_full, offset_cam_intrinsics);
ComputeErrorSplineOpt eopt(opt_size, &spline, ccd);
if (use_poses) {
tbb::parallel_reduce(pose_range, lopt);
// lopt(pose_range);
tbb::parallel_reduce(pose_range, eopt);
}
if (use_april_corners) {
tbb::parallel_reduce(april_range, lopt);
// lopt(april_range);
tbb::parallel_reduce(april_range, eopt);
}
if (use_mocap) {
tbb::parallel_reduce(mocap_pose_range, lopt);
// lopt(mocap_pose_range);
tbb::parallel_reduce(mocap_pose_range, eopt);
}
tbb::parallel_reduce(accel_range, lopt);
tbb::parallel_reduce(gyro_range, lopt);
tbb::parallel_reduce(accel_range, eopt);
tbb::parallel_reduce(gyro_range, eopt);
error = lopt.error;
num_points = lopt.num_points;
reprojection_error = lopt.reprojection_error;
double f_diff = (lopt.error - eopt.error);
double l_diff = 0.5 * inc_full.dot(inc_full * lambda - lopt.accum.getB());
std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
<< lopt.num_points << std::endl;
std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
lopt.accum.setup_solver();
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
double step_quality = f_diff / l_diff;
bool step = false;
int max_iter = 10;
if (step_quality < 0) {
std::cout << "\t[REJECTED] lambda:" << lambda
<< " step_quality: " << step_quality
<< " Error: " << eopt.error << " num points "
<< eopt.num_points << std::endl;
lambda = std::min(max_lambda, lambda_vee * lambda);
lambda_vee *= 2;
while (!step && max_iter > 0) {
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);
spline = spline_backup;
*calib = calib_backup;
*mocap_calib = mocap_calib_backup;
g = g_backup;
VectorX inc_full = -lopt.accum.solve(&Hdiag_lambda);
} else {
std::cout << "\t[ACCEPTED] lambda:" << lambda
<< " step_quality: " << step_quality
<< " Error: " << eopt.error << " num points "
<< eopt.num_points << std::endl;
Calibration<Scalar> calib_backup = *calib;
MocapCalibration<Scalar> mocap_calib_backup = *mocap_calib;
SplineT spline_backup = spline;
Vector3 g_backup = g;
lambda = std::max(
min_lambda,
lambda *
std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
lambda_vee = 2;
applyInc(inc_full, offset_cam_intrinsics);
error = eopt.error;
num_points = eopt.num_points;
reprojection_error = eopt.reprojection_error;
ComputeErrorSplineOpt eopt(opt_size, &spline, ccd);
if (use_poses) {
tbb::parallel_reduce(pose_range, eopt);
}
if (use_april_corners) {
tbb::parallel_reduce(april_range, eopt);
}
if (use_mocap) {
tbb::parallel_reduce(mocap_pose_range, eopt);
}
tbb::parallel_reduce(accel_range, eopt);
tbb::parallel_reduce(gyro_range, eopt);
if (eopt.error > lopt.error) {
std::cout << "\t[REJECTED] lambda:" << lambda
<< " Error: " << eopt.error << " num points "
<< eopt.num_points << std::endl;
lambda = std::min(max_lambda, 2 * lambda);
spline = spline_backup;
*calib = calib_backup;
*mocap_calib = mocap_calib_backup;
g = g_backup;
} else {
std::cout << "\t[ACCEPTED] lambda:" << lambda
<< " Error: " << eopt.error << " num points "
<< eopt.num_points << std::endl;
lambda = std::max(min_lambda, lambda / 2);
error = eopt.error;
num_points = eopt.num_points;
reprojection_error = eopt.reprojection_error;
step = true;
}
max_iter--;
step = true;
}
max_iter--;
}
if (converged) {
std::cout << "[CONVERGED]" << std::endl;
}
return converged;
}
typename Calibration<Scalar>::Ptr calib;
@ -529,7 +551,7 @@ class SplineOptimization {
1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE + 1];
}
Scalar lambda, min_lambda, max_lambda;
Scalar lambda, min_lambda, max_lambda, lambda_vee;
int64_t min_time_us, max_time_us;

6
src/calibration/cam_calib.cpp
View File

@ -731,8 +731,8 @@ void CamCalib::optimizeWithParam(bool print_info,
auto start = std::chrono::high_resolution_clock::now();
calib_opt->optimize(opt_intr, huber_thresh, error, num_points,
reprojection_error);
bool converged = calib_opt->optimize(opt_intr, huber_thresh, error,
num_points, reprojection_error);
auto finish = std::chrono::high_resolution_clock::now();
@ -768,6 +768,8 @@ void CamCalib::optimizeWithParam(bool print_info,
.count()
<< "ms." << std::endl;
if (converged) std::cout << "Optimization Converged !!" << std::endl;
std::cout << "==================================" << std::endl;
}

8
src/calibration/cam_imu_calib.cpp
View File

@ -681,9 +681,9 @@ void CamImuCalib::optimizeWithParam(bool print_info,
auto start = std::chrono::high_resolution_clock::now();
calib_opt->optimize(opt_intr, opt_poses, opt_corners, opt_cam_time_offset,
opt_imu_scale, opt_mocap, huber_thresh, error,
num_points, reprojection_error);
bool converged = calib_opt->optimize(
opt_intr, opt_poses, opt_corners, opt_cam_time_offset, opt_imu_scale,
opt_mocap, huber_thresh, error, num_points, reprojection_error);
auto finish = std::chrono::high_resolution_clock::now();
@ -752,6 +752,8 @@ void CamImuCalib::optimizeWithParam(bool print_info,
.count()
<< "ms." << std::endl;
if (converged) std::cout << "Optimization Converged !!" << std::endl;
std::cout << "==================================" << std::endl;
}