void sba_pinhole_model(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) { const sba_model_data_t<double> *sba_data = static_cast< sba_model_data_t<double>* >(adata); if(!sba_data) return; const cv::Mat &camera_matrix = sba_data->camera_matrix; const int num_cols = idxij->nc; double *p_pose = p; double *p_points = p+num_cols*num_params_per_cam; double rotation_quat[quaternion_size]; double total_rot_quat[quaternion_size]; for(int j=0; j<num_cols; ++j) { // get j-th camera parameters double *p_quat = p_pose + j*num_params_per_cam; double *p_transl = p_quat + 3; // rotation vector part has 3 elements // get rotation estimation vec2quat(p_quat, rotation_quat); const double *p_rotation_guess = &(sba_data->initial_rotations[j*quaternion_size]); multiply_quaternion(rotation_quat, p_rotation_guess, total_rot_quat); // find number of nonzero hx_ij, i=0...n-1 int num_nonzero = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); for(int i=0; i<num_nonzero; ++i) { double *p_point = p_points + rcsubs[i]*num_params_per_point; double *p_measurement = hx + idxij->val[rcidxs[i]]*num_params_per_measuremnt; // p_measurement = hx_ij quaternion_pinhole_model<double>(p_point, total_rot_quat, p_transl, camera_matrix, p_measurement); } } }
void quat_rot_vec(VEC *q1, VEC *v1){ VEC *quat_of_vec, *qmulv, *qinv, *fin_rot_vec; //all are quaternions quat_of_vec = v_get(4); v_zero(quat_of_vec); qmulv = v_get(4); v_zero(qmulv); qinv = v_get(4); v_zero(qinv); fin_rot_vec = v_get(4); v_zero(fin_rot_vec); vec2quat(v1, quat_of_vec); quat_inv(q1, qinv); quat_mul(qinv, quat_of_vec, qmulv); quat_mul(qmulv, q1, fin_rot_vec); quat2vec(fin_rot_vec, v1); v_free(quat_of_vec); v_free(qmulv); v_free(qinv); v_free(fin_rot_vec); }