bool Date::parseISO_8601(std::string str0) { // ISO 8601 Format: yyyy-mm-ddThh:mm:ss double year, month, day, hour; // defaults: year = 0.; month = 1.; day = 1.; hour=0.; Split x_str0(str0, " T", true); Split x_d(x_str0[0],"-"); if( x_d.size() > 2 ) { year = hdhC::string2Double( x_d[0] ); month = hdhC::string2Double( x_d[1] ); day = hdhC::string2Double( x_d[2] ); } else if( x_d.size() > 1 ) { year = hdhC::string2Double( x_d[0] ); month = hdhC::string2Double( x_d[1] ); } else year = hdhC::string2Double( x_d[0] ); if( x_str0.size() > 1 ) { Split x_t(x_str0[1], ':') ; if( x_t.size() > 2 ) { hour = hdhC::string2Double( x_t[0] ); hour += hdhC::string2Double( x_t[1] ) / 60.; hour += hdhC::string2Double( x_t[2] ) / 3600.; } else if( x_t.size() > 1 ) { hour = hdhC::string2Double( x_t[0] ); hour += hdhC::string2Double( x_t[1] ) / 60.; } else hour = hdhC::string2Double( x_t[0] ); } if( x_str0.size() == 3 ) { // local time zone Split x_t(x_str0[2], ':') ; if( x_t.size() > 1 ) { hour += hdhC::string2Double(x_t[0]) ; hour += hdhC::string2Double(x_t[1]) / 60. ; // minutes } else hour += hdhC::string2Double(x_t[0]) ; } jul = date2Julian( year, month, day, 0., 0., 0.); jul += hour/24.; isDateSet=true; return false ; }
int calib_gimbal_load(calib_gimbal_t &calib, const std::string &data_dir) { // Load calibration data calib.data_dir = data_dir; if (calib_gimbal_data_load(calib.data, data_dir) != 0) { LOG_ERROR("Failed to load calibration data [%s]!", data_dir.c_str()); return -1; } // Load optimization params const std::string config_file = data_dir + "/camchain.yaml"; const std::string joint_file = data_dir + "/joint.csv"; if (calib_gimbal_params_load(calib.params, config_file, joint_file) != 0) { LOG_ERROR("Failed to load optimization params!"); return -1; } // Setup optimization problem // const mat3_t K_s = calib.params.camchain.cam[0].K(); // const mat3_t K_d = calib.params.camchain.cam[2].K(); // const vec4_t D_s = calib.params.camchain.cam[0].D(); // const vec4_t D_d = calib.params.camchain.cam[2].D(); const mat3_t K_s; const mat3_t K_d; const vec4_t D_s; const vec4_t D_d; const double theta1_offset = *calib.params.theta1_offset; const double theta2_offset = *calib.params.theta2_offset; // const std::string dmodel_s = calib.params.camchain.cam[0].distortion_model; // const std::string dmodel_d = calib.params.camchain.cam[2].distortion_model; const std::string dmodel_s; const std::string dmodel_d; // Form residual blocks for (int i = 0; i < calib.data.nb_measurements; i++) { for (int j = 0; j < calib.data.P_s[i].rows(); j++) { const vec3_t P_s = calib.data.P_s[i].row(j); const vec3_t P_d = calib.data.P_d[i].row(j); // Undistort pixel measurements from static camera const vec2_t p_s = calib.data.Q_s[i].row(j); cv::Point2f pt_s(p_s(0), p_s(1)); // pt_s = calib.params.camchain.cam[0].undistortPoint(pt_s); vec3_t x_s{pt_s.x, pt_s.y, 1.0}; // x_s = calib.params.camchain.cam[0].K() * x_s; const vec2_t Q_s{x_s(0), x_s(1)}; // Undistort pixel measurements from dynamic camera const vec2_t p_d = calib.data.Q_d[i].row(j); cv::Point2f pt_d(p_d(0), p_d(1)); // pt_d = calib.params.camchain.cam[2].undistortPoint(pt_d); vec3_t x_d{pt_d.x, pt_d.y, 1.0}; // x_d = calib.params.camchain.cam[2].K() * x_d; const vec2_t Q_d{x_d(0), x_d(1)}; // auto residual = // new GimbalCalibResidual(P_s, P_d, // Q_s, Q_d, // K_s, K_d, // D_s, D_d, // theta1_offset, theta2_offset); // Build cost function // auto cost_func = // new ceres::AutoDiffCostFunction<GimbalCalibResidual, // Residual // type // 4, // Size of residual // 6, // Size of: tau_s // 6, // Size of: tau_d // 3, // Size of: w1 // 3, // Size of: w2 // 1, // Size of: Lambda1 // 1 // Size of: Lambda2 // >(residual); // Add residual block to problem // calib.problem.AddResidualBlock(cost_func, // Cost function // NULL, // Loss function // calib.params.tau_s, // calib.params.tau_d, // calib.params.w1, // calib.params.w2, // &calib.params.Lambda1[i], // &calib.params.Lambda2[i]); // calib.problem.SetParameterBlockConstant(&calib.params.Lambda1[i]); // calib.problem.SetParameterBlockConstant(&calib.params.Lambda2[i]); } } calib.problem.SetParameterBlockConstant(calib.params.w2); return 0; }