Revision history [back]

In the first 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?

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?