| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
|
|
| #include <string.h> |
| #include <iostream> |
| #include <algorithm> |
| #include <time.h> |
| #include <iomanip> |
| #include <fstream> |
| #include <string> |
|
|
| using std::cout; |
| using std::ofstream; |
| using std::string; |
|
|
| #ifndef _WIN32 |
| #include <sys/time.h> |
| #endif |
|
|
| #include "ConfigBA.h" |
|
|
| #ifdef _MSC_VER |
| #define strcpy strcpy_s |
| #define sprintf sprintf_s |
| #endif |
|
|
| namespace pba { |
|
|
| ConfigBA::ConfigBA() { |
| __lm_max_iteration = 50; |
| __lm_initial_damp = 1e-3f; |
| __lm_minimum_damp = 1e-10f; |
| __lm_maximum_damp = 1e+5f; |
| __lm_delta_threshold = 1e-6f; |
| __lm_gradient_threshold = 1e-10f; |
| __lm_mse_threshold = 0.25f; |
| __lm_use_diagonal_damp = true; |
| __lm_check_gradient = false; |
| __lm_damping_auto_switch = 0; |
| __bundle_time_budget = 0; |
| __bundle_mode_next = 0; |
| __bundle_current_mode = 0; |
|
|
| |
| __cg_max_iteration = 100; |
| __cg_min_iteration = 10; |
| __cg_recalculate_freq = 0; |
| __cg_norm_threshold = 0.1f; |
| __cg_norm_guard = 1.0f; |
| __pba_experimental = 0; |
| __cg_schur_complement = 0; |
|
|
| |
| __fixed_intrinsics = false; |
| __use_radial_distortion = 0; |
| __reset_initial_distortion = false; |
|
|
| |
| __verbose_level = 2; |
| __verbose_cg_iteration = false; |
| __verbose_function_time = false; |
| __verbose_allocation = false; |
| __verbose_sse = false; |
| __save_gradient_norm = false; |
| __stat_filename = NULL; |
| __matlab_format_stat = true; |
|
|
| |
| __jc_store_transpose = true; |
| __jc_store_original = true; |
| __no_jacobian_store = false; |
|
|
| __focal_normalize = true; |
| __depth_normalize = true; |
| __depth_degeneracy_fix = true; |
| __jacobian_normalize = true; |
| __data_normalize_median = 0.5f; |
| __depth_check_epsilon = 0.01f; |
|
|
| |
| __multiply_jx_usenoj = true; |
|
|
| |
| __accurate_gain_ratio = true; |
| |
| __cpu_data_precision = 0; |
| __current_device = -1; |
| __selected_device = -1; |
| __memory_usage = 0; |
| __current_iteration = 0; |
| __num_cpu_thread_all = 0; |
|
|
| |
| __debug_pba = false; |
| __profile_pba = 0; |
| __cpu_thread_profile = false; |
| __warmup_device = false; |
|
|
| |
| __driver_output = NULL; |
|
|
| |
| ResetBundleStatistics(); |
| } |
|
|
| void ConfigBA::ResetBundleStatistics() { |
| __abort_flag = false; |
| __num_lm_success = 0; |
| __num_lm_iteration = 0; |
| __num_cg_iteration = 0; |
| __num_projection_eval = 0; |
| __num_jacobian_eval = 0; |
| __num_camera_modified = 0; |
| __num_point_behind = 0; |
| __initial_mse = 0; |
| __final_mse = 0; |
| __final_mse_x = 0; |
| __focal_scaling = 1.0f; |
| __depth_scaling = 1.0f; |
| __pba_return_code = 0; |
| __current_iteration = 0; |
| __warmup_device = false; |
| __bundle_current_mode = __bundle_mode_next; |
| for (int i = 0; i < NUM_TIMER; ++i) __timer_record[i] = 0; |
| __bundle_records.resize(0); |
| if (__num_cpu_thread_all) { |
| std::cout << "WARNING: set all thread number to " << __num_cpu_thread_all |
| << '\n'; |
| for (int i = 0; i < NUM_FUNC; ++i) |
| __num_cpu_thread[i] = __num_cpu_thread_all; |
| } |
| } |
|
|
| void ConfigBA::ResetTemporarySetting() { |
| __reset_initial_distortion = false; |
| __bundle_time_budget = 0; |
| __bundle_mode_next = 0; |
| __bundle_current_mode = 0; |
| __stat_filename = NULL; |
| if (__lm_damping_auto_switch > 0 && !__lm_use_diagonal_damp) |
| __lm_use_diagonal_damp = true; |
| } |
|
|
| void ConfigBA::SaveBundleStatistics(int ncam, int npt, int nproj) { |
| if (__profile_pba) return; |
| if (__stat_filename && __bundle_records.size() > 0) { |
| char filenamebuf[1024]; |
| char* ret = strchr(__stat_filename, '\r'); |
| if (ret) ret[0] = 0; |
| char* dot = strrchr(__stat_filename, '.'); |
| if (dot && strchr(dot, '/') == NULL && strchr(dot, '\\') == NULL) |
| strcpy(filenamebuf, __stat_filename); |
| |
| else |
| sprintf(filenamebuf, "%s%s%s%s%s%s%s%s%s.%s", __stat_filename, |
| __cpu_data_precision == 0 ? "_gpu" : "_cpu", |
| __cpu_data_precision == sizeof(double) ? "d" : "", |
| __cg_schur_complement ? "_schur" : "\0", |
| __lm_use_diagonal_damp |
| ? "\0" |
| : (__lm_damping_auto_switch > 0 ? "_ad" : "_id"), |
| __use_radial_distortion == -1 |
| ? "_md" |
| : (__use_radial_distortion ? "_pd" : "\0"), |
| __jacobian_normalize ? "\0" : "_nojn", |
| __focal_normalize || __depth_normalize ? "\0" : "_nodn", |
| __depth_degeneracy_fix ? "\0" : "_nodf", |
| __matlab_format_stat ? "m" : "log"); |
|
|
| |
| ofstream out(filenamebuf); |
| out << std::left; |
|
|
| float overhead = |
| (BundleTimerGet(TIMER_OVERALL) - BundleTimerGet(TIMER_OPTIMIZATION)); |
| if (__matlab_format_stat) |
| out << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n" |
| << "ncam = " << ncam << "; npt = " << npt << "; nproj = " << nproj |
| << ";\n" |
| << "%% overhead = " << overhead << ";\n" |
| << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n" |
| << "%% " << std::setw(10) << __num_lm_iteration |
| << "\t linear systems solved;\n" |
| << "%% " << std::setw(10) << __num_cg_iteration |
| << "\t conjugated gradient steps;\n" |
| << "%% " << std::setw(10) << BundleTimerGet(TIMER_OVERALL) |
| << "\t seconds used overall;\n" |
| << "%% " << std::setw(10) << BundleTimerGet(TIMER_PREPROCESSING) |
| << "\t seconds on pre-processing;\n" |
| << "%% " << std::setw(10) |
| << BundleTimerGet(TIMER_GPU_UPLOAD) + |
| BundleTimerGet(TIMER_GPU_ALLOCATION) |
| << "\t seconds on upload;\n" |
| << "%% " << std::setw(10) << BundleTimerGet(TIMER_OPTIMIZATION) |
| << "\t seconds on optimization;\n" |
| << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n" |
| << (__cpu_data_precision == 0 ? "gpustat" : "cpustat") |
| << (__cpu_data_precision == sizeof(double) ? "_db" : "") |
| << (__jacobian_normalize ? "" : "_nojn") |
| << (__depth_degeneracy_fix ? "" : "_nodf") |
| << (__cg_schur_complement ? "_schur" : "") << " = [\n"; |
|
|
| for (size_t i = 0; i < __bundle_records.size(); ++i) |
| out << std::setw((i % 7 > 2) ? ((i % 7 > 4 && !__save_gradient_norm && |
| !__lm_check_gradient) |
| ? 0 |
| : 12) |
| : 5) |
| << (__bundle_records[i] + (i == 1 ? overhead : 0)) |
| << (i % 7 == 6 ? '\n' : '\t'); |
|
|
| if (__matlab_format_stat) out << "];\n\n"; |
|
|
| if (__verbose_level) |
| std::cout << "\n---------------------------------------\n" << filenamebuf; |
| } |
| } |
|
|
| #define REPORT_FUNCTION_TIME(FID) \ |
| std::setw(5) << (((int)(BundleTimerGet(FID) * 100 + 50)) * 0.01) << "(" \ |
| << std::setw(2) \ |
| << 0.1f * ((int)(1000 * BundleTimerGet(FID) / \ |
| BundleTimerGet(TIMER_OPTIMIZATION))) \ |
| << "%)" |
|
|
| void ConfigBA::PrintBundleStatistics() { |
| if (__profile_pba) return; |
|
|
| if (__verbose_level) |
| std::cout << "\n---------------------------------------\n" << std::setw(10) |
| << __num_lm_success << "\t successful iterations;\n" |
| << std::setw(10) << __num_lm_iteration |
| << "\t linear systems solved;\n" << std::setw(10) |
| << __num_cg_iteration << "\t conjugated gradient steps;\n" |
| << std::setw(10) << BundleTimerGet(TIMER_OVERALL) |
| << "\t seconds used overall;\n" << std::setw(10) |
| << BundleTimerGet(TIMER_GPU_ALLOCATION) |
| << "\t seconds on allocation;\n" << std::setw(10) |
| << BundleTimerGet(TIMER_PREPROCESSING) |
| << "\t seconds on pre-processing;\n" << std::setw(10) |
| << BundleTimerGet(TIMER_GPU_UPLOAD) << "\t seconds on upload;\n" |
| << std::setw(10) << BundleTimerGet(TIMER_OPTIMIZATION) |
| << "\t seconds on optimization;\n"; |
| if (__verbose_level && __cpu_data_precision) |
| std::cout << REPORT_FUNCTION_TIME(TIMER_FUNCTION_JJ) |
| << "\t seconds on jacobians;\n" |
| << REPORT_FUNCTION_TIME(TIMER_FUNCTION_PJ) |
| << "\t seconds on projections;\n" |
| << REPORT_FUNCTION_TIME(TIMER_FUNCTION_JX) |
| << "\t seconds on JX;\n" |
| << REPORT_FUNCTION_TIME(TIMER_FUNCTION_JTE) |
| << "\t seconds on JtE;\n" |
| << REPORT_FUNCTION_TIME(TIMER_FUNCTION_BC) |
| << "\t seconds to compute preconditioner;\n" |
| << REPORT_FUNCTION_TIME(TIMER_FUNCTION_MP) |
| << "\t seconds to apply preconditioner;\n" |
| << REPORT_FUNCTION_TIME(TIMER_FUNCTION_UP) |
| << "\t seconds to update parameters;\n"; |
| if (__verbose_level) |
| std::cout << "---------------------------------------\n" |
| << "mse = " << __initial_mse << " -> " << __final_mse << "" |
| << " (" << __final_mse_x |
| << (__use_radial_distortion == -1 ? 'D' : 'U') << ")\n" |
| << "---------------------------------------\n"; |
| } |
|
|
| double ConfigBA::MyClock() { |
| #ifdef _WIN32 |
| return clock() / double(CLOCKS_PER_SEC); |
| #else |
| static int started = 0; |
| static struct timeval tstart; |
| if (started == 0) { |
| gettimeofday(&tstart, NULL); |
| started = 1; |
| return 0; |
| } else { |
| struct timeval now; |
| gettimeofday(&now, NULL); |
| return ((now.tv_usec - tstart.tv_usec) / 1000000.0 + |
| (now.tv_sec - tstart.tv_sec)); |
| } |
| #endif |
| } |
|
|
| void ConfigBA::BundleTimerStart(int timer) { |
| __timer_record[timer] = MyClock(); |
| } |
|
|
| void ConfigBA::BundleTimerSwitch(int timer) { |
| __timer_record[timer] = MyClock() - __timer_record[timer]; |
| } |
|
|
| void ConfigBA::BundleTimerSwap(int timer1, int timer2) { |
| BundleTimerSwitch(timer1); |
| BundleTimerSwitch(timer2); |
| } |
|
|
| float ConfigBA::BundleTimerGet(int timer) { |
| return float(__timer_record[timer]); |
| } |
|
|
| float ConfigBA::BundleTimerGetNow(int timer) { |
| return 0.01f * ((int)(100 * (MyClock() - __timer_record[timer]))); |
| } |
|
|
| bool ConfigBA::IsTimeBudgetAvailable() { |
| if (__bundle_time_budget <= 0) return true; |
| return BundleTimerGetNow(TIMER_OVERALL) < __bundle_time_budget; |
| } |
|
|
| void ConfigBA::SaveBundleRecord(int iter, float res, float damping, float gn, |
| float gi) { |
| __bundle_records.push_back(float(iter)); |
| __bundle_records.push_back(BundleTimerGetNow()); |
| __bundle_records.push_back(float(__num_cg_iteration)); |
| __bundle_records.push_back(res); |
| __bundle_records.push_back(damping); |
| __bundle_records.push_back(gn); |
| __bundle_records.push_back(gi); |
| } |
|
|
| void ConfigBA::ParseParam(int argc, char** argv) { |
| #define CHAR1_TO_INT(x) ((x >= 'A' && x <= 'Z') ? x + 32 : x) |
| #define CHAR2_TO_INT(str, i) \ |
| (str[i] ? CHAR1_TO_INT(str[i]) + (CHAR1_TO_INT(str[i + 1]) << 8) : 0) |
| #define CHAR3_TO_INT(str, i) \ |
| (str[i] ? CHAR1_TO_INT(str[i]) + (CHAR2_TO_INT(str, i + 1) << 8) : 0) |
| #define STRING_TO_INT(str) (CHAR1_TO_INT(str[0]) + (CHAR3_TO_INT(str, 1) << 8)) |
|
|
| #ifdef _MSC_VER |
| |
| #define MAKEINT1(a) (#@ a) |
| #define sscanf sscanf_s |
| #else |
| #define mychar0 '0' |
| #define mychar1 '1' |
| #define mychar2 '2' |
| #define mychar3 '3' |
| #define mychara 'a' |
| #define mycharb 'b' |
| #define mycharc 'c' |
| #define mychard 'd' |
| #define mychare 'e' |
| #define mycharf 'f' |
| #define mycharg 'g' |
| #define mycharh 'h' |
| #define mychari 'i' |
| #define mycharj 'j' |
| #define mychark 'k' |
| #define mycharl 'l' |
| #define mycharm 'm' |
| #define mycharn 'n' |
| #define mycharo 'o' |
| #define mycharp 'p' |
| #define mycharq 'q' |
| #define mycharr 'r' |
| #define mychars 's' |
| #define mychart 't' |
| #define mycharu 'u' |
| #define mycharv 'v' |
| #define mycharw 'w' |
| #define mycharx 'x' |
| #define mychary 'y' |
| #define mycharz 'z' |
| #define MAKEINT1(a) (mychar##a) |
| #endif |
| #define MAKEINT2(a, b) (MAKEINT1(a) + (MAKEINT1(b) << 8)) |
| #define MAKEINT3(a, b, c) (MAKEINT1(a) + (MAKEINT2(b, c) << 8)) |
| #define MAKEINT4(a, b, c, d) (MAKEINT1(a) + (MAKEINT3(b, c, d) << 8)) |
|
|
| char *arg, *param, *opt; |
| int opti, argi; |
| float argf; |
| for (int i = 0; i < argc; i++) { |
| arg = argv[i]; |
| if (arg == NULL || arg[0] != '-' || !arg[1]) continue; |
| opt = arg + 1; |
| opti = STRING_TO_INT(opt); |
| param = argv[i + 1]; |
|
|
| |
| switch (opti) { |
| case MAKEINT3(l, m, i): |
| if (i + 1 < argc && sscanf(param, "%d", &argi) && argi > 0) |
| __lm_max_iteration = argi; |
| break; |
| case MAKEINT3(l, m, d): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf >= 0) |
| __lm_delta_threshold = argf; |
| break; |
| case MAKEINT3(l, m, e): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf >= 0) |
| __lm_mse_threshold = argf; |
| break; |
| case MAKEINT3(l, m, g): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf > 0) |
| __lm_gradient_threshold = argf; |
| break; |
| case MAKEINT4(d, a, m, p): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf > 0) |
| __lm_initial_damp = argf; |
| break; |
| case MAKEINT4(d, m, i, n): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf > 0) |
| __lm_minimum_damp = argf; |
| break; |
| case MAKEINT4(d, m, a, x): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf > 0) |
| __lm_maximum_damp = argf; |
| break; |
| case MAKEINT3(c, g, i): |
| if (i + 1 < argc && sscanf(param, "%d", &argi) && argi > 0) |
| __cg_max_iteration = argi; |
| break; |
| case MAKEINT4(c, g, i, m): |
| if (i + 1 < argc && sscanf(param, "%d", &argi) && argi > 0) |
| __cg_min_iteration = argi; |
| break; |
| case MAKEINT3(c, g, n): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf > 0) |
| __cg_norm_threshold = argf; |
| break; |
| case MAKEINT3(c, g, g): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf > 0) |
| __cg_norm_guard = argf; |
| break; |
| case MAKEINT4(c, g, r, f): |
| if (i + 1 < argc && sscanf(param, "%d", &argi) && argi > 0) |
| __cg_recalculate_freq = argi; |
| break; |
| case MAKEINT1(v): |
| if (i + 1 < argc && sscanf(param, "%d", &argi) && argi >= 0) |
| __verbose_level = argi; |
| break; |
| case MAKEINT4(d, e, v, i): |
| if (i + 1 < argc && sscanf(param, "%d", &argi) && argi >= 0) |
| __selected_device = argi; |
| break; |
| case MAKEINT4(b, u, d, g): |
| if (i + 1 < argc && sscanf(param, "%d", &argi) && argi >= 0) |
| __bundle_time_budget = argi; |
| break; |
| case MAKEINT3(e, x, p): |
| if (i + 1 < argc && sscanf(param, "%d", &argi) && argi >= 0) |
| __pba_experimental = argi; |
| break; |
| case MAKEINT4(t, n, u, m): |
| if (i + 1 < argc && sscanf(param, "%d", &argi) && argi > 0) |
| __num_cpu_thread_all = argi; |
| break; |
| case MAKEINT4(p, r, o, f): |
| __profile_pba = (i + 1 < argc && sscanf(param, "%d", &argi)) |
| ? std::max(10, argi) |
| : 100; |
| break; |
| case MAKEINT4(t, p, r, o): |
| __cpu_thread_profile = true; |
| break; |
| case MAKEINT4(c, a, l, i): |
| __fixed_intrinsics = true; |
| break; |
| case MAKEINT4(s, c, h, u): |
| case MAKEINT4(s, s, o, r): |
| __cg_schur_complement = true; |
| break; |
| case MAKEINT2(m, d): |
| case MAKEINT4(r, a, d, i): |
| __use_radial_distortion = -1; |
| break; |
| case MAKEINT2(p, d): |
| __use_radial_distortion = 1; |
| break; |
| case MAKEINT3(r, 0, 0): |
| __reset_initial_distortion = true; |
| break; |
| case MAKEINT4(v, a, r, i): |
| __fixed_intrinsics = false; |
| break; |
| case MAKEINT4(n, a, c, c): |
| __accurate_gain_ratio = false; |
| break; |
| case MAKEINT4(v, c, g, i): |
| __verbose_cg_iteration = true; |
| break; |
| case MAKEINT4(v, f, u, n): |
| __verbose_function_time = true; |
| break; |
| case MAKEINT4(v, a, l, l): |
| __verbose_allocation = true; |
| break; |
| case MAKEINT4(v, s, s, e): |
| __verbose_sse = true; |
| break; |
| case MAKEINT4(s, v, g, n): |
| __save_gradient_norm = true; |
| break; |
| case MAKEINT2(i, d): |
| __lm_use_diagonal_damp = false; |
| break; |
| case MAKEINT3(d, a, s): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf > 0) |
| __lm_damping_auto_switch = std::max(argf, 0.1f); |
| else |
| __lm_damping_auto_switch = 2.0f; |
| break; |
| case MAKEINT4(c, h, k, g): |
| __lm_check_gradient = true; |
| break; |
| case MAKEINT4(n, o, j, n): |
| __jacobian_normalize = false; |
| break; |
| case MAKEINT2(n, j): |
| __no_jacobian_store = true; |
| case MAKEINT3(n, j, c): |
| __jc_store_transpose = false; |
| __jc_store_original = false; |
| break; |
| case MAKEINT4(n, j, c, o): |
| __jc_store_original = false; |
| break; |
| case MAKEINT4(n, j, c, t): |
| __jc_store_transpose = false; |
| break; |
| case MAKEINT3(j, x, j): |
| __multiply_jx_usenoj = false; |
| break; |
| case MAKEINT4(j, x, n, j): |
| __multiply_jx_usenoj = true; |
| break; |
| case MAKEINT4(n, o, d, n): |
| __depth_normalize = false; |
| __focal_normalize = false; |
| break; |
| case MAKEINT4(n, o, d, f): |
| __depth_degeneracy_fix = false; |
| break; |
| case MAKEINT4(n, o, r, m): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf > 0) |
| __data_normalize_median = argf; |
| break; |
| case MAKEINT3(d, c, e): |
| if (i + 1 < argc && sscanf(param, "%f", &argf) && argf > 0 && |
| argf <= 0.01) |
| __depth_check_epsilon = argf; |
| break; |
| case MAKEINT4(d, e, b, u): |
| __debug_pba = true; |
| break; |
| case MAKEINT4(e, v, a, l): |
| __lm_max_iteration = 100; |
| __warmup_device = true; |
| case MAKEINT4(s, t, a, t): |
| __stat_filename = (i + 1 < argc && param[0] != '-') ? param : NULL; |
| break; |
| case MAKEINT3(o, u, t): |
| __driver_output = (i + 1 < argc && param[0] != '-') ? param : NULL; |
| break; |
| case MAKEINT4(w, a, r, m): |
| __warmup_device = true; |
| break; |
| case MAKEINT4(m, o, t, i): |
| __bundle_mode_next = 1; |
| break; |
| case MAKEINT4(s, t, r, u): |
| __bundle_mode_next = 2; |
| break; |
| } |
| } |
| } |
|
|
| } |
|
|
