In the first streaming camera frame (let it be frame A) I have found the location of a marker and performed solvePNP, so I can find the 3D pose of the marker. That works fine. For the next camera frames I want to find the new 3D marker pose using optical flow.
I have gone through the process of detecting salient features for frame A and B, and computed the fundamental matrix and the essential matrix. But how do I use these projections to find the new 3D pose of the marker? Here is the code so far:
calcOpticalFlowPyrLK( frameA, frameB, cornersA, cornersB, features_found, feature_errors ,
Size( win_size, win_size ), 5,
cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3 ), 0 );
Mat F = findFundamentalMat(cornersA, cornersB, FM_RANSAC, 0.1, 0.99);
//-- Step 5: calculate Essential Matrix
double data[] = { camera_matrix.at(0,0), camera_matrix.at(0,1), camera_matrix.at(0,2),
camera_matrix.at(1,0), camera_matrix.at(1,1), camera_matrix.at(1,2),
camera_matrix.at(2,0), camera_matrix.at(2,1), camera_matrix.at(2,2)};//Camera Matrix
Mat K(3, 3, CV_64F, data);
Mat_ E = K.t() * F * K; //according to HZ (9.12)
//-- Step 6: calculate Rotation Matrix and Translation Vector
Matx34d P;
Matx34d P1;
//decompose E to P' , HZ (9.19)
SVD svd(E,SVD::MODIFY_A);
Mat svd_u = svd.u;
Mat svd_vt = svd.vt;
Mat svd_w = svd.w;
Matx33d W(0,-1,0,1,0,0,0,0,1);//HZ 9.13
Mat_ R = svd_u * Mat(W) * svd_vt; //HZ 9.19
Mat_ t = svd_u.col(2); //u3
if (!CheckCoherentRotation(R)) {
cout << "resulting rotation is not coherent\n";
P1 = 0;
}
P1 = Matx34d(R(0,0),R(0,1),R(0,2),t(0),
R(1,0),R(1,1),R(1,2),t(1),
R(2,0),R(2,1),R(2,2),t(2));
I tried to put the new P1 matrix values in the new 3D pose matrix, but that gave me weird results.
How can I get the 3D pose of the next frame based on optical flow?
↧