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