improve LM
This commit is contained in:
parent
d20c9012e9
commit
84212bc314
|
@ -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();
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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,13 +350,13 @@ 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;
|
||||
|
@ -408,15 +409,17 @@ class SplineOptimization {
|
|||
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::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;
|
||||
|
@ -441,11 +444,20 @@ class SplineOptimization {
|
|||
tbb::parallel_reduce(accel_range, eopt);
|
||||
tbb::parallel_reduce(gyro_range, eopt);
|
||||
|
||||
if (eopt.error > lopt.error) {
|
||||
double f_diff = (lopt.error - eopt.error);
|
||||
double l_diff = 0.5 * inc_full.dot(inc_full * 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;
|
||||
|
||||
spline = spline_backup;
|
||||
*calib = calib_backup;
|
||||
|
@ -454,10 +466,15 @@ class SplineOptimization {
|
|||
|
||||
} 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;
|
||||
|
@ -467,7 +484,12 @@ class SplineOptimization {
|
|||
}
|
||||
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;
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue