diff --git a/CMakeLists.txt b/CMakeLists.txt index e41e9a9..2b11d93 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -113,13 +113,18 @@ if(APPLE) # See: https://github.com/openMVG/openMVG/issues/1349#issuecomment-401492811 set(CMAKE_FIND_FRAMEWORK LAST) - # use brewed llvm's libc++ - # Note: the suffix "/../v1" for the include path is needed to work around a recent cmake issue: - # https://gitlab.kitware.com/cmake/cmake/issues/19251#note_571030 - include_directories("/usr/local/opt/llvm/include/c++/v1/../v1") - link_directories("/usr/local/opt/llvm/lib") - add_compile_options("-nostdinc++") - set(STD_CXX_FS c++fs) + if(CMAKE_SYSTEM_VERSION VERSION_LESS 19.0.0) + # use brewed llvm's libc++ + # Note: the suffix "/../v1" for the include path is needed to work around a recent cmake issue: + # https://gitlab.kitware.com/cmake/cmake/issues/19251#note_571030 + include_directories("/usr/local/opt/llvm/include/c++/v1/../v1") + link_directories("/usr/local/opt/llvm/lib") + add_compile_options("-nostdinc++") + set(STD_CXX_FS c++fs) + endif() + + + if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") message(STATUS "Detected macOS with non-Apple clang") diff --git a/include/basalt/optimization/accumulator.h b/include/basalt/optimization/accumulator.h index 56b23b4..420cc66 100644 --- a/include/basalt/optimization/accumulator.h +++ b/include/basalt/optimization/accumulator.h @@ -33,8 +33,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef BASALT_ACCUMULATOR_H -#define BASALT_ACCUMULATOR_H +#pragma once #include #include @@ -45,6 +44,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#if defined(BASALT_USE_CHOLMOD) + +#include + +template +using SparseLLT = Eigen::CholmodSupernodalLLT; + +#else + +template +using SparseLLT = Eigen::SimplicialLDLT; + +#endif + namespace basalt { template @@ -195,7 +208,7 @@ class SparseHashAccumulator { cg.compute(sm); res = cg.solve(b); } else { - Eigen::SimplicialLDLT chol(sm); + SparseLLT chol(sm); res = chol.solve(b); } @@ -261,5 +274,3 @@ class SparseHashAccumulator { }; } // namespace basalt - -#endif diff --git a/include/basalt/optimization/spline_optimize.h b/include/basalt/optimization/spline_optimize.h index 36c648c..53a62e3 100644 --- a/include/basalt/optimization/spline_optimize.h +++ b/include/basalt/optimization/spline_optimize.h @@ -87,10 +87,10 @@ class SplineOptimization { typedef Se3Spline SplineT; - SplineOptimization(int64_t dt_ns = 1e7) + SplineOptimization(int64_t dt_ns = 1e7, double init_lambda = 1e-12) : pose_var(1e-4), mocap_initialized(false), - lambda(1e-12), + lambda(init_lambda), min_lambda(1e-18), max_lambda(100), lambda_vee(2), @@ -367,7 +367,8 @@ class SplineOptimization { 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 stop_thresh, double& error, - int& num_points, double& reprojection_error) { + int& num_points, double& reprojection_error, + bool print_info = true) { // std::cerr << "optimize num_knots " << num_knots << std::endl; ccd.opt_intrinsics = use_intr; @@ -419,8 +420,9 @@ class SplineOptimization { num_points = lopt.num_points; reprojection_error = lopt.reprojection_error; - std::cout << "[LINEARIZE] Error: " << lopt.error << " num points " - << lopt.num_points << std::endl; + if (print_info) + std::cout << "[LINEARIZE] Error: " << lopt.error << " num points " + << lopt.num_points << std::endl; lopt.accum.setup_solver(); Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); @@ -473,10 +475,11 @@ class SplineOptimization { double step_quality = f_diff / l_diff; if (step_quality < 0) { - std::cout << "\t[REJECTED] lambda:" << lambda - << " step_quality: " << step_quality - << " max_inc: " << max_inc << " Error: " << eopt.error - << " num points " << eopt.num_points << std::endl; + if (print_info) + std::cout << "\t[REJECTED] lambda:" << lambda + << " step_quality: " << step_quality + << " max_inc: " << max_inc << " Error: " << eopt.error + << " num points " << eopt.num_points << std::endl; lambda = std::min(max_lambda, lambda_vee * lambda); lambda_vee *= 2; @@ -486,10 +489,11 @@ class SplineOptimization { g = g_backup; } else { - std::cout << "\t[ACCEPTED] lambda:" << lambda - << " step_quality: " << step_quality - << " max_inc: " << max_inc << " Error: " << eopt.error - << " num points " << eopt.num_points << std::endl; + if (print_info) + std::cout << "\t[ACCEPTED] lambda:" << lambda + << " step_quality: " << step_quality + << " max_inc: " << max_inc << " Error: " << eopt.error + << " num points " << eopt.num_points << std::endl; lambda = std::max( min_lambda, @@ -506,7 +510,7 @@ class SplineOptimization { max_iter--; } - if (converged) { + if (converged && print_info) { std::cout << "[CONVERGED]" << std::endl; }