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, t_vec;

        Mat t1 = (Ra.clone()).t();      t1.push_back(t_unscaled);           R_vec.push_back(t1.t());        
        Mat t2 = (Ra.clone()).t();      t2.push_back(tminus);       R_vec.push_back(t2.t());        
        Mat t3 = (Rb.clone()).t();      t3.push_back(t_unscaled);           R_vec.push_back(t3.t());        
        Mat t4 = (Rb.clone()).t();      t4.push_back(tminus);       R_vec.push_back(t4.t());

        //      Mat c1 = R_.clone();    c1 = c1.t();    c1.push_back(t_);    c1 = c1.t(); //for triangulation initial projection matrix
        Mat c1 = (Mat_<double>(3,4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0); 
        Mat out = Mat::zeros(4, p1.size(), CV_32F); //triangulated points

        int i = 0;  
        bool isOK;    //To check if all the points lie infront of the camera plane

        do
          {
        isOK = true;
        triangulatePoints(c1, R_vec.at(i), p1, p2, out);   //triangulate feature point in both images and check z value
        for(int j = 0; j< p1.size(); j++)
          {
            if (out.at<double>(2,j) < 0)
              {
            isOK = false;
            break;
              }
          }
        i++;

          }while(isOK = false);  //checking the depth

        Mat T = R_vec.at(i-1).clone();  // correct {R:t}

        //Separate R and t from T....

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

        t_scaled = t_unscaled * (height_new_ - height_prev_) / t_unscaled.at<double>(0,2);    // scale the translation ... 
        cout<<t_scaled<<endl;
        t_ = t_ + t_scaled;
        R_ = R_ * R  ;

        Mat R_angle = (Mat_<double>(1,3)<< atan2(R_.at<double>(2,1), R_.at<double>(2,2)), atan2(-1 * R_.at<double>(2,0), sqrt(R_.at<double>(2,1)*R_.at<double>(2,1) + R_.at<double>(2,2)*R_.at<double>(2,2))), atan2(R_.at<double>(1,0),R_.at<double>(0,0))) ;

        height_prev_ = height_new_;
        height_time_prev_ = height_time_new_;
        update_height_ = false;

        goodFeaturesToTrack(image_new_, features_new_, MAX_CORNERS_, k_, minDistance_, noArray(), blockSize_, false, k_);
        cv_ptr_prev_->image = cv_ptr_new_->image;
        features_prev_ = features_new_;

        vo_data << R_angle * 180/PI << " " << t_ <<endl;   //write estimated pose to vo_data.txt
      }
    else
      {
        cout<<"P1 size not equal to P2 size or few features. Cannot triangulate points"<<endl;
        cout<<"skipping current frame"<<endl;
      }
      }
    else 
      { 
    goodFeaturesToTrack(image_new_, features_new_, MAX_CORNERS_, qualityLevel_, minDistance_, noArray(), blockSize_, true, k_);
    cv_ptr_prev_->image = cv_ptr_new_->image;
    features_prev_ = features_new_;
    start_flag_ = 0;
      }
  }    
};

I would be grateful if anyone would point out my mistake and suggest a solution. Thanks.