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