diff --git a/include/basalt/optimization/accumulator.h b/include/basalt/optimization/accumulator.h index eee7c45..d619846 100644 --- a/include/basalt/optimization/accumulator.h +++ b/include/basalt/optimization/accumulator.h @@ -129,65 +129,6 @@ class DenseAccumulator { VectorX b; }; -template -class SparseAccumulator { - public: - typedef Eigen::Matrix VectorX; - typedef Eigen::Triplet T; - typedef Eigen::SparseMatrix SparseMatrix; - - template - inline void addH(int si, int sj, const Eigen::MatrixBase& 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 - inline void addB(int i, const Eigen::MatrixBase& data) { - b.template segment(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 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& 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 triplets; - VectorX b; -}; - template 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(); diff --git a/include/basalt/optimization/poses_optimize.h b/include/basalt/optimization/poses_optimize.h index 53ba09f..849c43f 100644 --- a/include/basalt/optimization/poses_optimize.h +++ b/include/basalt/optimization/poses_optimize.h @@ -60,7 +60,8 @@ class PosesOptimization { typename Eigen::vector::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 &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 timestam_to_pose_backup = timestam_to_pose; Calibration 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 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>::CalibCommonData ccd; - Scalar lambda, min_lambda, max_lambda; + Scalar lambda, min_lambda, max_lambda, lambda_vee; size_t problem_size; diff --git a/include/basalt/optimization/spline_optimize.h b/include/basalt/optimization/spline_optimize.h index b567998..8af3b35 100644 --- a/include/basalt/optimization/spline_optimize.h +++ b/include/basalt/optimization/spline_optimize.h @@ -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 pose_range(pose_measurements.begin(), - pose_measurements.end()); - tbb::blocked_range april_range( - aprilgrid_corners_measurements.begin(), - aprilgrid_corners_measurements.end()); + tbb::blocked_range pose_range(pose_measurements.begin(), + pose_measurements.end()); + tbb::blocked_range april_range( + aprilgrid_corners_measurements.begin(), + aprilgrid_corners_measurements.end()); - tbb::blocked_range mocap_pose_range( - mocap_measurements.begin(), mocap_measurements.end()); + tbb::blocked_range mocap_pose_range( + mocap_measurements.begin(), mocap_measurements.end()); - tbb::blocked_range accel_range(accel_measurements.begin(), - accel_measurements.end()); + tbb::blocked_range accel_range(accel_measurements.begin(), + accel_measurements.end()); - tbb::blocked_range gyro_range(gyro_measurements.begin(), - gyro_measurements.end()); + tbb::blocked_range 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 calib_backup = *calib; + MocapCalibration 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 calib_backup = *calib; - MocapCalibration 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::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; diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index b7f13c3..84ce73e 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -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; } diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp index 6e5f960..0eb08c3 100644 --- a/src/calibration/cam_imu_calib.cpp +++ b/src/calibration/cam_imu_calib.cpp @@ -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; }