diff --git a/.gitignore b/.gitignore index fb932d1..2346430 100644 --- a/.gitignore +++ b/.gitignore @@ -12,7 +12,7 @@ ba_log.ubjson __pycache__ /*.cereal /rootba_config.toml - +.vscode # generic: .DS_Store *~ diff --git a/src/rootba/bal/bal_bundle_adjustment_helper.cpp b/src/rootba/bal/bal_bundle_adjustment_helper.cpp index a2e9818..19aa032 100644 --- a/src/rootba/bal/bal_bundle_adjustment_helper.cpp +++ b/src/rootba/bal/bal_bundle_adjustment_helper.cpp @@ -112,7 +112,7 @@ template bool BalBundleAdjustmentHelper::linearize_point( const Vec2& obs, const Vec3& lm_p_w, const SE3& T_c_w, const basalt::BalCamera& intr, const bool ignore_validity_check, - VecR& res, MatRP* d_res_d_xi, MatRI* d_res_d_i, MatRL* d_res_d_l) { + VecR& res, MatRP* d_res_d_xi, MatRI* d_res_d_i, MatRL* d_res_d_l, bool fix_cam) { Mat4 T_c_w_mat = T_c_w.matrix(); Vec4 p_c_3d = T_c_w_mat * lm_p_w.homogeneous(); @@ -133,12 +133,16 @@ bool BalBundleAdjustmentHelper::linearize_point( } if (d_res_d_xi) { - Mat d_point_d_xi; - d_point_d_xi.template topLeftCorner<3, 3>() = Mat3::Identity(); - d_point_d_xi.template topRightCorner<3, 3>() = - -SO3::hat(p_c_3d.template head<3>()); - d_point_d_xi.row(3).setZero(); - *d_res_d_xi = d_res_d_p * d_point_d_xi; + if(!fix_cam){ + Mat d_point_d_xi; + d_point_d_xi.template topLeftCorner<3, 3>() = Mat3::Identity(); + d_point_d_xi.template topRightCorner<3, 3>() = + -SO3::hat(p_c_3d.template head<3>()); + d_point_d_xi.row(3).setZero(); + *d_res_d_xi = d_res_d_p * d_point_d_xi; + }else{ + d_res_d_xi->setZero(); + } } if (d_res_d_l) { diff --git a/src/rootba/bal/bal_bundle_adjustment_helper.hpp b/src/rootba/bal/bal_bundle_adjustment_helper.hpp index 6409ab7..fa098ed 100644 --- a/src/rootba/bal/bal_bundle_adjustment_helper.hpp +++ b/src/rootba/bal/bal_bundle_adjustment_helper.hpp @@ -82,7 +82,8 @@ class BalBundleAdjustmentHelper { bool ignore_validity_check, VecR& res, MatRP* d_res_d_xi = nullptr, MatRI* d_res_d_i = nullptr, - MatRL* d_res_d_l = nullptr); + MatRL* d_res_d_l = nullptr, + bool fix_cam = false); }; } // namespace rootba diff --git a/src/rootba/bal/bal_problem.cpp b/src/rootba/bal/bal_problem.cpp index c6bb230..99bcc83 100644 --- a/src/rootba/bal/bal_problem.cpp +++ b/src/rootba/bal/bal_problem.cpp @@ -184,6 +184,49 @@ BalProblem::BalProblem(const std::string& path) { load_bal(path); } +template +void BalProblem::init_problem(int num_cams, int num_lms){ + // clear memory and re-allocate + if (cameras_.capacity() > unsigned_cast(num_cams)) { + decltype(cameras_)().swap(cameras_); + } + if (landmarks_.capacity() > unsigned_cast(num_lms)) { + decltype(landmarks_)().swap(landmarks_); + } + cameras_.resize(num_cams); + landmarks_.resize(num_lms); + forceStopFlag_ = nullptr; +} + +template +void BalProblem::add_cam(int cam_idx, Eigen::Quaterniond &R_cw, Eigen::Vector3d &t_cw, Eigen::Vector4d &intr){ + auto& cam = cameras_.at(cam_idx); + + cam.T_c_w.so3() = SO3(R_cw.cast()); + cam.T_c_w.translation() = t_cw.cast(); + cam.intrinsics = CameraModel(); + cam.intrinsics.setPinholeIntrinsic(intr.cast()); +} + +template +void BalProblem::add_landmark(int lm_idx, const Eigen::Vector3d &p_w){ + landmarks_.at(lm_idx).p_w = p_w.cast(); +} + +template +void BalProblem::set_observation(int cam_idx, int lm_idx, const Eigen::Vector2d &pixel_obs){ + auto [obs, inserted] = landmarks_.at(lm_idx).obs.try_emplace(cam_idx); + CHECK(inserted) << "Repeated observation!"; + + obs->second.pos = pixel_obs.cast(); +} + +template +void BalProblem::fix_cam(int cam_idx){ + auto& cam = cameras_.at(cam_idx); + cam.setFixed(true); +} + template void BalProblem::load_bal(const std::string& path) { FILE* fptr = std::fopen(path.c_str(), "r"); @@ -213,6 +256,7 @@ void BalProblem::load_bal(const std::string& path) { cameras_.resize(num_cams); landmarks_.resize(num_lms); + LOG(INFO) << "cam size:" << num_cams << " num_lms size:" << num_lms << std::endl; // parse observations for (int i = 0; i < num_obs; ++i) { int cam_idx; @@ -238,11 +282,11 @@ void BalProblem::load_bal(const std::string& path) { // we don't have the "minus" like in the original Snavely model. // invert y axis - obs->second.pos.y() = -obs->second.pos.y(); + // obs->second.pos.y() = -obs->second.pos.y(); } // invert y and z axis (same as rotation around x by 180; self-inverse) - const SO3 axis_inversion = SO3(Vec3(1, -1, -1).asDiagonal()); + // const SO3 axis_inversion = SO3(Vec3(1, -1, -1).asDiagonal()); // parse camera parameters for (int i = 0; i < num_cams; ++i) { @@ -252,9 +296,12 @@ void BalProblem::load_bal(const std::string& path) { params = paramsd.cast(); auto& cam = cameras_.at(i); - cam.T_c_w.so3() = axis_inversion * SO3::exp(params.template head<3>()); - cam.T_c_w.translation() = axis_inversion * params.template segment<3>(3); + cam.T_c_w.so3() = SO3::exp(params.template head<3>()); + cam.T_c_w.translation() = params.template segment<3>(3); cam.intrinsics = CameraModel(params.template tail<3>()); + Eigen::Vector4d intrinsics(718.856, 718.856, 607.1928, 185.2157); + + cam.intrinsics.setPinholeIntrinsic(intrinsics.cast()); } // parse landmark parameters diff --git a/src/rootba/bal/bal_problem.hpp b/src/rootba/bal/bal_problem.hpp index c2f6671..05fbae1 100644 --- a/src/rootba/bal/bal_problem.hpp +++ b/src/rootba/bal/bal_problem.hpp @@ -94,6 +94,13 @@ class BalProblem { intrinsics = CameraModel(p.template tail<3>()); } + inline void setFixed(bool fixed = true){ + fixed_=fixed; + } + bool isFixed() const{ + return fixed_; + } + void apply_inc_pose(const Vec6& inc) { inc_pose(inc, T_c_w); } inline static void inc_pose(const Vec6& inc, SE3& T_c_w) { @@ -128,6 +135,7 @@ class BalProblem { } private: + bool fixed_ = false; SE3 T_c_w_backup_; CameraModel intrinsics_backup_; }; @@ -161,6 +169,16 @@ class BalProblem { BalProblem() = default; explicit BalProblem(const std::string& path); + void init_problem(int num_cams, int num_lms); + + void add_cam(int cam_idx, Eigen::Quaterniond &R_cw, Eigen::Vector3d &t_cw, Eigen::Vector4d &intr); + void add_landmark(int lm_idx, const Eigen::Vector3d &p_w); + void set_observation(int cam_idx, int lm_idx, const Eigen::Vector2d &pixel_obs); + void fix_cam(int cam_idx); + + void set_force_stop_flag(bool *flag) { forceStopFlag_ = flag; } + bool terminate() {return forceStopFlag_ ? (*forceStopFlag_) : false; } + void load_bal(const std::string& path); void load_bundler(const std::string& path); @@ -227,6 +245,7 @@ class BalProblem { Cameras cameras_; Landmarks landmarks_; + bool *forceStopFlag_; /// quiet means no INFO level log output bool quiet_ = false; }; diff --git a/src/rootba/bal/bal_residual_options.hpp b/src/rootba/bal/bal_residual_options.hpp index 8242a2d..1d8c33e 100644 --- a/src/rootba/bal/bal_residual_options.hpp +++ b/src/rootba/bal/bal_residual_options.hpp @@ -50,14 +50,14 @@ struct BalResidualOptions : public VisitableOptions { BEGIN_VISITABLES(BalResidualOptions); VISITABLE_META(RobustNorm, robust_norm, - init(RobustNorm::NONE) + init(RobustNorm::HUBER) .help("which robust norm to use. NONE: squared norm, " "HUBER: Huber norm.")); // ceres uses value of 1.0 in bundle_adjuster example VISITABLE_META( double, huber_parameter, - init(1).range(0, 10).help("huber parameter for robust norm in pixels")); + init(sqrt(5.991)).range(0, 10).help("huber parameter for robust norm in pixels")); END_VISITABLES; }; diff --git a/src/rootba/bal/solver_options.hpp b/src/rootba/bal/solver_options.hpp index 9e92532..1410cc8 100644 --- a/src/rootba/bal/solver_options.hpp +++ b/src/rootba/bal/solver_options.hpp @@ -84,7 +84,7 @@ struct SolverOptions : public VisitableOptions { // solver type VISITABLE_META(SolverType, solver_type, - init(SolverType::SQUARE_ROOT) + init(SolverType::SCHUR_COMPLEMENT) .help("Solver type; 'SQUARE_ROOT' for square root BA, " "'SCHUR_COMPLEMENT' for classical Schur complement " "BA, 'CERES' for a Ceres-based implementation.")); @@ -92,7 +92,7 @@ struct SolverOptions : public VisitableOptions { // verbosity VISITABLE_META( int, verbosity_level, - init(2).range(0, 2).help("Output verbosity level. 0: silent, 1: brief " + init(0).range(0, 2).help("Output verbosity level. 0: silent, 1: brief " "report (one line), 2: full report")); // debug output @@ -120,7 +120,7 @@ struct SolverOptions : public VisitableOptions { // optimized cost in lm (for ceres we can only do Error and ErrorValid) VISITABLE_META( OptimizedCost, optimized_cost, - init(OptimizedCost::ERROR) + init(OptimizedCost::ERROR_VALID) .help("Which cost to consider for the 'cost decreased?' check in " "Levenberg-Marqardt. ERROR considers all residuals, " "ERROR_VALID ignores residuals with negative z, and " diff --git a/src/rootba/ceres/CMakeLists.txt b/src/rootba/ceres/CMakeLists.txt index 38e7ffd..339ad94 100644 --- a/src/rootba/ceres/CMakeLists.txt +++ b/src/rootba/ceres/CMakeLists.txt @@ -19,6 +19,8 @@ target_sources(rootba_ceres # include order: ceres first; ensure ceres is found first in the specified location, not in /usr/local (on macOS with homebrew) target_link_libraries(rootba_ceres PUBLIC Ceres::ceres) +# add_subdirectory(g2o) +# target_link_libraries(rootba_ceres PUBLIC g2o) target_link_libraries(rootba_ceres PUBLIC diff --git a/src/rootba/ceres/bal_bundle_adjustment.cpp b/src/rootba/ceres/bal_bundle_adjustment.cpp index 8dfe82a..9269154 100644 --- a/src/rootba/ceres/bal_bundle_adjustment.cpp +++ b/src/rootba/ceres/bal_bundle_adjustment.cpp @@ -44,6 +44,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rootba/ceres/bal_residuals.hpp" #include "rootba/ceres/option_utils.hpp" +// #include "g2o/g2o/core/block_solver.h" +// #include "g2o/g2o/core/optimization_algorithm_levenberg.h" +// #include "g2o/g2o/solvers/linear_solver_eigen.h" +// #include "g2o/g2o/types/types_six_dof_expmap.h" +// #include "g2o/g2o/core/robust_kernel_impl.h" +// #include "g2o/g2o/solvers/linear_solver_dense.h" +// #include "g2o/g2o/types/types_seven_dof_expmap.h" + namespace rootba { void bundle_adjust_ceres(BalProblem& bal_problem, @@ -123,4 +131,86 @@ void bundle_adjust_ceres(BalProblem& bal_problem, } } + +// void bundle_adjust_g2o(BalProblem& bal_problem, +// const SolverOptions& solver_options, BaLog* log) { +// // options +// const bool projection_validitity_check = +// ceres_use_projection_validity_check(solver_options); + +// // Setup optimizer +// g2o::SparseOptimizer optimizer; +// g2o::BlockSolver_6_3::LinearSolverType * linearSolver; + +// linearSolver = new g2o::LinearSolverEigen(); + +// g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + +// g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); +// optimizer.setAlgorithm(solver); +// optimizer.setVerbose(true); + +// const rootba::BalProblem::Cameras &cams = bal_problem.cameras(); +// const rootba::BalProblem::Landmarks &landmarks = bal_problem.landmarks(); + +// for (int i = 0; i < cams.size(); ++i) { +// const rootba::BalProblem::Camera &cam = cams.at(i); +// g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); +// vSE3->setEstimate(g2o::SE3Quat(cam.T_c_w.so3().matrix(),cam.T_c_w.translation())); +// vSE3->setId(i); +// vSE3->setFixed(i==0); +// optimizer.addVertex(vSE3); +// } + +// for (int i = 0; i < landmarks.size(); ++i) { +// const rootba::BalProblem::Landmark &lm = landmarks.at(i); + +// g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); +// vPoint->setEstimate(lm.p_w); +// int id = cams.size()+i; +// vPoint->setId(id); +// vPoint->setMarginalized(true); +// optimizer.addVertex(vPoint); + +// for (const auto& [cam_idx, obs] : lm.obs) { +// g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ(); + +// e->setVertex(0, dynamic_cast(optimizer.vertex(id))); +// e->setVertex(1, dynamic_cast(optimizer.vertex(cam_idx))); +// e->setMeasurement(obs.pos); +// e->setInformation(Eigen::Matrix2d::Identity()); + +// g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; +// e->setRobustKernel(rk); +// rk->setDelta(sqrt(5.991)); + +// e->fx = 718.856; +// e->fy = 718.856; +// e->cx = 607.1928; +// e->cy = 185.2157; +// optimizer.addEdge(e); +// } +// } + +// optimizer.initializeOptimization(); +// optimizer.optimize(100); +// using SO3 = Sophus::SO3; + +// //Keyframes +// // for (int i = 0; i < cams.size(); ++i) { +// // g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(i)); +// // g2o::SE3Quat SE3quat = vSE3->estimate(); + +// // rootba::BalProblem::Camera &cam = bal_problem.cameras().at(i); +// // cam.T_c_w.so3() = SO3(SE3quat.rotation()); + +// // cam.T_c_w.translation() = SE3quat.translation(); +// // } + +// // //Points +// // for (int i = 0; i < landmarks.size(); ++i) { +// // g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(i+cams.size())); +// // bal_problem.landmarks().at(i).p_w = vPoint->estimate(); +// // } +// } } // namespace rootba diff --git a/src/rootba/ceres/bal_bundle_adjustment.hpp b/src/rootba/ceres/bal_bundle_adjustment.hpp index 1c84891..94d455b 100644 --- a/src/rootba/ceres/bal_bundle_adjustment.hpp +++ b/src/rootba/ceres/bal_bundle_adjustment.hpp @@ -44,4 +44,8 @@ void bundle_adjust_ceres(BalProblem& bal_problem, const SolverOptions& solver_options, BaLog* log = nullptr); +void bundle_adjust_g2o(BalProblem& bal_problem, + const SolverOptions& solver_options, + BaLog* log = nullptr); + } diff --git a/src/rootba/qr/impl/landmark_block_base.ipp b/src/rootba/qr/impl/landmark_block_base.ipp index 8543a6f..68591b1 100644 --- a/src/rootba/qr/impl/landmark_block_base.ipp +++ b/src/rootba/qr/impl/landmark_block_base.ipp @@ -113,7 +113,7 @@ void LandmarkBlockBase::linearize_landmark( Vec2 res; const bool valid = BalBundleAdjustmentHelper::linearize_point( obs.pos, lm_ptr_->p_w, cam.T_c_w, cam.intrinsics, true, res, &Jp, &Ji, - &Jl); + &Jl, cam.isFixed()); if (!options_.use_valid_projections_only || valid) { numerically_valid = numerically_valid && Jl.array().isFinite().all() && diff --git a/src/rootba/solver/bal_bundle_adjustment.cpp b/src/rootba/solver/bal_bundle_adjustment.cpp index d3f83fe..497546a 100644 --- a/src/rootba/solver/bal_bundle_adjustment.cpp +++ b/src/rootba/solver/bal_bundle_adjustment.cpp @@ -281,7 +281,7 @@ void optimize_lm_ours(BalProblem& bal_problem, bool terminated = false; - for (int it = 0; it <= max_lm_iter && !terminated;) { + for (int it = 0; it <= max_lm_iter && !terminated && !bal_problem.terminate();) { IterationSummary it_summary; it_summary.iteration = it; linearizor->start_iteration(&it_summary); @@ -293,9 +293,9 @@ void optimize_lm_ours(BalProblem& bal_problem, ResidualInfo ri; linearizor->compute_error(ri); - std::cout << "Iteration {}, {}\n"_format( - it, error_summary_oneline( - ri, solver_options.use_projection_validity_check())); + // std::cout << "Iteration {}, {}\n"_format( + // it, error_summary_oneline( + // ri, solver_options.use_projection_validity_check())); CHECK(ri.is_numerically_valid) << "did not expect numerical failure during linearization"; @@ -316,15 +316,15 @@ void optimize_lm_ours(BalProblem& bal_problem, linearizor->linearize(); - std::cout << "\t[INFO] Stage 1 time {:.3f}s.\n"_format( - it_summary.stage1_time_in_seconds); + // std::cout << "\t[INFO] Stage 1 time {:.3f}s.\n"_format( + // it_summary.stage1_time_in_seconds); // Don't limit inner lm iterations (to be in line with ceres) constexpr int MAX_INNER_IT = std::numeric_limits::max(); - for (int j = 0; j < MAX_INNER_IT && it <= max_lm_iter && !terminated; j++) { + for (int j = 0; j < MAX_INNER_IT && it <= max_lm_iter && !terminated && !bal_problem.terminate(); j++) { if (j > 0) { - std::cout << "Iteration {}, backtracking\n"_format(it); + // std::cout << "Iteration {}, backtracking\n"_format(it); it_summary = IterationSummary(); it_summary.iteration = it; @@ -336,15 +336,15 @@ void optimize_lm_ours(BalProblem& bal_problem, // dampen and solve linear system VecX inc = linearizor->solve(lambda); - std::cout << "\t[INFO] Stage 2 time {:.3f}s.\n"_format( - it_summary.stage2_time_in_seconds); + // std::cout << "\t[INFO] Stage 2 time {:.3f}s.\n"_format( + // it_summary.stage2_time_in_seconds); - std::cout << "\t[CG] Summary: {} Time {:.3f}s. Time per iteration " - "{:.3f}s\n"_format( - it_summary.linear_solver_message, - it_summary.solve_reduced_system_time_in_seconds, - it_summary.solve_reduced_system_time_in_seconds / - it_summary.linear_solver_iterations); + // std::cout << "\t[CG] Summary: {} Time {:.3f}s. Time per iteration " + // "{:.3f}s\n"_format( + // it_summary.linear_solver_message, + // it_summary.solve_reduced_system_time_in_seconds, + // it_summary.solve_reduced_system_time_in_seconds / + // it_summary.linear_solver_iterations); // TODO: cleanly abort linear solver on numerical issue @@ -359,14 +359,14 @@ void optimize_lm_ours(BalProblem& bal_problem, std::string reason = it_summary.step_is_valid ? "Reject" : "Invalid"; - std::cout - << "\t[{}] {}, lambda: {:.1e}, cg_iter: {}, it_time: " - "{:.3f}s, total_time: {:.3f}s\n" - ""_format( - reason, - "Numeric issues when computing increment (contains NaNs)", - lambda, it_summary.linear_solver_iterations, iteration_time, - cumulative_time); + // std::cout + // << "\t[{}] {}, lambda: {:.1e}, cg_iter: {}, it_time: " + // "{:.3f}s, total_time: {:.3f}s\n" + // ""_format( + // reason, + // "Numeric issues when computing increment (contains NaNs)", + // lambda, it_summary.linear_solver_iterations, iteration_time, + // cumulative_time); lambda = lambda_vee * lambda; lambda_vee *= vee_factor; @@ -402,9 +402,9 @@ void optimize_lm_ours(BalProblem& bal_problem, if (!ri2.is_numerically_valid) { it_summary.step_is_valid = false; it_summary.step_is_successful = false; - std::cout << "\t[EVAL] failed to evaluate cost: {}"_format( - error_summary_oneline( - ri2, solver_options.use_projection_validity_check())); + // std::cout << "\t[EVAL] failed to evaluate cost: {}"_format( + // error_summary_oneline( + // ri2, solver_options.use_projection_validity_check())); } else { // compute "ri - ri2", depending on 'optimized_cost' config Scalar f_diff( @@ -421,10 +421,10 @@ void optimize_lm_ours(BalProblem& bal_problem, Scalar step_quality = f_diff / l_diff; - std::cout << "\t[EVAL] f_diff {:.4e} l_diff {:.4e} " - "step_quality {:.4e} ri1 {:.4e} ri2 {:.4e}\n" - ""_format(f_diff, l_diff, step_quality, ri.valid.error, - ri2.valid.error); + // std::cout << "\t[EVAL] f_diff {:.4e} l_diff {:.4e} " + // "step_quality {:.4e} ri1 {:.4e} ri2 {:.4e}\n" + // ""_format(f_diff, l_diff, step_quality, ri.valid.error, + // ri2.valid.error); it_summary.relative_decrease = step_quality; @@ -440,12 +440,12 @@ void optimize_lm_ours(BalProblem& bal_problem, const double iteration_time = timer_iteration.elapsed(); const double cumulative_time = timer_total.elapsed(); - std::cout << "\t[Success] {}, lambda: {:.1e}, cg_iter: {}, it_time: " - "{:.3f}s, total_time: {:.3f}s\n" - ""_format(format_new_error_info( - ri2, solver_options.optimized_cost), - lambda, it_summary.linear_solver_iterations, - iteration_time, cumulative_time); + // std::cout << "\t[Success] {}, lambda: {:.1e}, cg_iter: {}, it_time: " + // "{:.3f}s, total_time: {:.3f}s\n" + // ""_format(format_new_error_info( + // ri2, solver_options.optimized_cost), + // lambda, it_summary.linear_solver_iterations, + // iteration_time, cumulative_time); lambda *= Scalar(std::max( 1.0 / 3, 1 - std::pow(2 * it_summary.relative_decrease - 1, 3))); @@ -476,13 +476,13 @@ void optimize_lm_ours(BalProblem& bal_problem, std::string reason = it_summary.step_is_valid ? "Reject" : "Invalid"; - std::cout << "\t[{}] {}, lambda: {:.1e}, cg_iter: {}, it_time: " - "{:.3f}s, total_time: {:.3f}s\n" - ""_format(reason, - format_new_error_info( - ri2, solver_options.optimized_cost), - lambda, it_summary.linear_solver_iterations, - iteration_time, cumulative_time); + // std::cout << "\t[{}] {}, lambda: {:.1e}, cg_iter: {}, it_time: " + // "{:.3f}s, total_time: {:.3f}s\n" + // ""_format(reason, + // format_new_error_info( + // ri2, solver_options.optimized_cost), + // lambda, it_summary.linear_solver_iterations, + // iteration_time, cumulative_time); lambda = lambda_vee * lambda; lambda_vee *= vee_factor; @@ -522,10 +522,12 @@ void optimize_lm_ours(BalProblem& bal_problem, finish_solve(summary, solver_options); - std::cout << "Final Cost: {}\n"_format(error_summary_oneline( - summary.final_cost, solver_options.use_projection_validity_check())); - std::cout << "{}: {}\n"_format( - magic_enum::enum_name(summary.termination_type), summary.message); + // std::cout << "Final Cost: {}\n"_format(error_summary_oneline( + // summary.final_cost, solver_options.use_projection_validity_check())); + // std::cout << "{}: {}\n"_format( + // magic_enum::enum_name(summary.termination_type), summary.message); + const double opt_time = timer_total.elapsed(); + std::cout << "total time is:" << opt_time*1000 << std::endl; std::cout.flush(); } @@ -544,7 +546,7 @@ void bundle_adjust_manual(BalProblem& bal_problem, solver_summary->total_time_in_seconds; } - LOG(INFO) << solver_summary->message; + // LOG(INFO) << solver_summary->message; // TODO: Print summary similar to ceres: oneline, or full }