void Est(CvMat* state, CvMat* estimation, void *param) { // State: cam1, cam2, cam3, cam4, ..., X1(x,y,z), X2, X3, ... // Estimation: (u11,v11), (u) // For every image observation (camera)... for(int i = 0; i < n_images; ++i) { // Get camera from state Pose p; p.SetQuaternion(&(state->data.db[i*7+3])); double tra[3]; double rodr[3]; CvMat mat_translation_vector = cvMat(3, 1, CV_64F, tra); CvMat mat_rotation_vector = cvMat(3, 1, CV_64F, rodr); memcpy(tra, &(state->data.db[i*7]), 3*sizeof(double)); p.GetRodriques(&mat_rotation_vector); // For every point in marker field int n_points = n_markers*4; for(int j = 0; j < n_points; ++j) { int index = n_images*7 + 3*j; double object_points[3] = {state->data.db[index+0], state->data.db[index+1], state->data.db[index+2]}; CvMat mat_object_points; cvInitMatHeader(&mat_object_points, 1, 1, CV_64FC3, object_points); double proj[2]={0}; CvMat mat_proj = cvMat(1, 1, CV_64FC2, proj); cvProjectPoints2(&mat_object_points, &mat_rotation_vector, &mat_translation_vector, &(camera->calib_K), &(camera->calib_D), &mat_proj); index = i*n_points*2 + j*2; estimation->data.db[index+0] = proj[0]; estimation->data.db[index+1] = proj[1]; } } }