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;
}
