int calib_gimbal_params_load(calib_gimbal_params_t &data,
const std::string &camchain_file,
const std::string &joint_file) {
// Parse camchain file
// if (this->camchain.load(3, camchain_file) != 0) {
// LOG_ERROR("Failed to load camchain file [%s]!", camchain_file.c_str());
// return -1;
// }
// Load joint angles file
matx_t joint_data;
if (csv2mat(joint_file, false, joint_data) != 0) {
LOG_ERROR("Failed to load joint angles file [%s]!", joint_file.c_str());
return -1;
}
data.nb_measurements = joint_data.rows();
// Initialize optimization params
// data.tau_s = vec2array(data.camchain.tau_s);
// data.tau_d = vec2array(data.camchain.tau_d);
// data.w1 = vec2array(data.camchain.w1);
// data.w2 = vec2array(data.camchain.w2);
data.theta1_offset = (double *) malloc(sizeof(double) * 1);
data.theta2_offset = (double *) malloc(sizeof(double) * 1);
// *data.theta1_offset = data.camchain.theta1_offset;
// *data.theta2_offset = data.camchain.theta2_offset;
data.Lambda1 = vec2array(joint_data.col(0));
data.Lambda2 = vec2array(joint_data.col(1));
return 0;
}
array ADFun<Base>::Hessian(array& x, array& w)
{ vec<Base> x_vec(x);
vec<Base> w_vec(w);
vec<Base> result = f_.Hessian(x_vec, w_vec);
// Kludge: return a vector which is reshaped by cppad.py
return vec2array(result);
}
array ADFun<Base>::Reverse(int p, array& w)
{ size_t p_sz(p);
vec<Base> w_vec(w);
vec<Base> dw_vec = f_.Reverse(p_sz, w_vec);
size_t n = f_.Domain();
vec<Base> result(n);
for(size_t j = 0; j < n; j++)
result[j] = dw_vec[j*p + p - 1];
return vec2array(result);
}
array ADFun<Base>::Jacobian(array& x)
{ vec<Base> x_vec(x);
vec<Base> result = f_.Jacobian(x_vec);
// Kludge: return a vector which is reshaped by cppad.py
return vec2array(result);
}
array ADFun<Base>::Forward(int p, array& xp)
{ size_t p_sz(p);
vec<Base> xp_vec(xp);
vec<Base> result = f_.Forward(p_sz, xp_vec);
return vec2array(result);
}
bn::ndarray test_vec_double(bn::ndarray const &y) {
vec v = array2vec(y);
return vec2array(v);
}
