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