6D pose estimation problem. How to estimate the rotation matrix?

asked 2014-01-13 21:36:26 -0500 This post is a wiki. Anyone with karma >75 is welcome to improve it.

I have been trying to estimate the 6D pose of a moving camera (in 6D) using opencv library. My steps are:

1. use goodFeaturesToTrack to get features.
2. calculate optical flow
3. track features
4. get fundamental matrix
5. get essential matrix
6. check the combination of R or t to determine the true R & t using triangulation (could find a better way to do this.. had to combine R & t and then later extract R & t)
7. Use the height information (from altitude sensor) to get scale factor and scale the translation
8. Update the transformation
9. Save the new feature set and image for next loop.

But the results are far from expectation. I verified that R is orthogonal. But cant figure out where I went wrong. My code is as follows:

void imageCb(const sensor_msgs::ImageConstPtr& image)
{
try
{
if(start_flag_)
{
cv_ptr_prev_ = cv_bridge::toCvCopy(image, enc::BGR8);
}
cv_ptr_new_ = cv_bridge::toCvCopy(image, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Conversion error: %s", e.what());
return;
}
if(start_flag_)
{
image_prev_.create(cv_ptr_prev_->image.rows, cv_ptr_prev_->image.cols , CV_8UC1);
image_new_.create(cv_ptr_new_->image.rows, cv_ptr_new_->image.cols , CV_8UC1);
}

cvtColor( cv_ptr_new_->image, image_new_, CV_BGR2GRAY);
cvtColor( cv_ptr_prev_->image, image_prev_, CV_BGR2GRAY);

Mat F, E;                //Fundamental and Essential Matrices
vector<Point2f> p1;      //common feature points in first image
vector<Point2f> p2;      //common feature points in second image
double f = 320;          //camera parameter focal length
double Cx = 320.5;       //Resolution
double Cy = 240.5;
Mat K = (Mat_<double>(3,3) << f, 0 , -Cx, 0, f, -Cy, 0, 0, 1); //camera internal matrix

Mat W = (Mat_<double>(3,3) << 0, -1, 0, 1, 0, 0, 0, 0 , 1);   //for SVD
Mat Z = (Mat_<double>(3,3) << 0, 1, 0, -1, 0, 0, 0, 0 , 1);  //for SVD

Mat w, u , vt; //SVD result of Essential Matrix
Mat tx, Ra, Rb; //Temporary checking for R and t
Mat t_unscaled, t_scaled, R;  //True R and t for the current step

//Lucas Kanade Optical Flow

vector<uchar> features_found;
if(!start_flag_)
{
calcOpticalFlowPyrLK(image_prev_, image_new_, features_prev_, features_new_, features_found, noArray(),Size(win_size_*4+1, win_size_*4+1), 5,TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20, 0.3));
for( int i = 0; i < (int)features_new_.size(); i++ )
{
circle(cv_ptr_prev_->image, features_new_[i],1,CV_RGB(255,255,255));
circle(cv_ptr_prev_->image, features_prev_[i],1,CV_RGB(0,0,0));
if( !features_found[i] )
continue;
else
{
line(cv_ptr_prev_->image, features_prev_[i], features_new_[i], Scalar(255,0,0), 1, CV_AA);
p1.push_back(Point2f(features_prev_[i].x-Cx, features_prev_[i].y-Cy));
p2.push_back(Point2f(features_new_[i].x-Cx, features_new_[i].y-Cy));
}
}
image_pub_.publish(cv_ptr_prev_->toImageMsg());          //publish image with features

//Estimation
vector<uchar> inliers;

if(p1.size()==p2.size())
{
F =  findFundamentalMat(p1, p2, CV_FM_RANSAC,1.0, 0.999, inliers);
//      cout<<"Number of inliers is::::::"<<sum(inliers)<<endl;
E = K.t()*F*K;
SVD::compute(E, w, u, vt);

tx = u * Z * u.t();     Ra = u * W * vt;        Rb = u * W.t() * vt;

if(determinant(Ra)<=0)        Ra = -Ra;
if(determinant(Rb)<=0)        Rb = -Rb;

t_unscaled = (Mat_<double>(1,3) << tx.at<double>(2,1), tx.at<double>(0,2), tx.at<double>(1,0) );

Mat tminus = -(t_unscaled.clone());

//Check which Ra or Rb or t

vector<Mat> R_vec ...
edit retag close merge delete