Get object location from optical flow
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<double>(0,0), camera_matrix.at<double>(0,1), camera_matrix.at<double>(0,2),
camera_matrix.at<double>(1,0), camera_matrix.at<double>(1,1), camera_matrix.at<double>(1,2),
camera_matrix.at<double>(2,0), camera_matrix.at<double>(2,1), camera_matrix.at<double>(2,2)};//Camera Matrix
Mat K(3, 3, CV_64F, data);
Mat_<double> 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_<double> R = svd_u * Mat(W) * svd_vt; //HZ 9.19
Mat_<double> 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?