stereo_visual_odometry_alg.cpp

Go to the documentation of this file.

#include "stereo_visual_odometry_alg.h"

using namespace std;

StereoVisualOdometryAlgorithm::StereoVisualOdometryAlgorithm(void)
{
}

StereoVisualOdometryAlgorithm::~StereoVisualOdometryAlgorithm(void)
{
}

void StereoVisualOdometryAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// StereoVisualOdometryAlgorithm Public API
/*
void StereoVisualOdometryAlgorithm::show_image_alg_test(cv::Mat img){
  ROS_WARN("mostra imatge");
  imshow("img", img);
}
*/

void StereoVisualOdometryAlgorithm::compute_H_Hcov(cv::Mat& image1, cv::Mat& image2, cv::Mat& image3, cv::Mat& image4, cv::Mat& H, cv::Mat& Hcov, cv::Mat& point)
{
  CStereo_Vo vo;
  
  int minhessian=400;
  
  vector<cv::KeyPoint> kp_image1;
  vector<cv::KeyPoint> kp_image2;
  vector<cv::KeyPoint> kp_image3;
  vector<cv::KeyPoint> kp_image4;
  
  cv::Mat des_image1;
  cv::Mat des_image2;
  cv::Mat des_image3;
  cv::Mat des_image4;
  
  vector<cv::DMatch> matches_1_2;
  vector<cv::DMatch> matches_3_4;
  
  vector<cv::Point2f> kp_1;
  vector<cv::Point2f> kp_2;
  vector<cv::Point2f> kp_3;
  vector<cv::Point2f> kp_4;
  
  cv::Mat l_kp_1;
  cv::Mat r_kp_2;
  cv::Mat l_kp_3;
  cv::Mat r_kp_4;
  
  cv::Mat reconstructed_1_2;
  cv::Mat reconstructed_3_4;
  
  cv::Mat final_kp1;
  cv::Mat final_kp2;
  cv::Mat final_kp3;
  cv::Mat final_kp4;
  
  cv::Mat kp1;
  cv::Mat kp2;
  cv::Mat kp3;
  cv::Mat kp4;
  
  cv::Mat final_points_1_2;
  cv::Mat final_points_3_4;
  
  int dthld=5000;
  
  cv::Mat inliers_1_2;
  cv::Mat inliers_3_4;
  
  cv::Mat S12;
  cv::Mat S34;
  
  cv::Mat mean_1_2;
  cv::Mat mean_3_4;
  
  cv::Mat R;
  cv::Mat T;
  cv::Mat RT;
  
  int inliers;
  
  cv::Mat Srt;
  cv::Mat Jx;
  cv::Mat Ju;
  cv::Mat Jxt;
  cv::Mat Jut;
  
  cv::Mat left_projection_matrix;
  cv::Mat right_projection_matrix;
  cv::Mat initial_T;
  double fl1, fl2, u0l1, u0l2, fr1, fr2, u0r1, u0r2;
  
  //getting matrix projections
  
  vo.matrix_projections(fl1, fl2, u0l1, u0l2, fr1, fr2, u0r1, u0r2, initial_T, left_projection_matrix, right_projection_matrix);
  
  //detecting keypoints of two image pair
  
  vo.Keypoints_detection(image1, kp_image1, image2, kp_image2, image3, kp_image3, image4, kp_image4, minhessian);
  
  //computing descriptors of two image pair
    
   vo.descriptors_computing (image1, kp_image1, des_image1, image2, kp_image2, des_image2,
                             image3, kp_image3, des_image3, image4, kp_image4, des_image4);
   
  //matching descriptors
  
  vo.matching(image1, kp_image1, des_image1, image2, kp_image2, des_image2, image3, kp_image3, des_image3,
              image4, kp_image4, des_image4, matches_1_2, matches_3_4);
  
  for(unsigned int i = 0; i < matches_1_2.size(); i++ )
  {
    kp_1.push_back(kp_image1[matches_1_2[i].queryIdx].pt);
    kp_2.push_back(kp_image2[matches_1_2[i].trainIdx].pt);
    kp_3.push_back(kp_image3[matches_3_4[i].queryIdx].pt);
    kp_4.push_back(kp_image4[matches_3_4[i].trainIdx].pt);
  }
  
  //getting the triangulate points
  
  vo.triangulate_points(kp_1, kp_2, l_kp_1, r_kp_2, left_projection_matrix, right_projection_matrix, reconstructed_1_2);
  vo.triangulate_points(kp_3, kp_4, l_kp_3, r_kp_4, left_projection_matrix, right_projection_matrix, reconstructed_3_4);
  
  vo.dthld(l_kp_1, r_kp_2, l_kp_3, r_kp_4, final_kp1, final_kp2, final_kp3, final_kp4, reconstructed_1_2, reconstructed_3_4, final_points_1_2, final_points_3_4, dthld);
  
  //RANSAC
    
  vo.ransac(final_kp1, final_kp2, final_kp3, final_kp4, kp1, kp2, kp3, kp4, final_points_1_2, final_points_3_4, inliers_1_2, inliers_3_4);
    
  //getting the stereo covariance
    
  vo.stereocovar(fl1, fl2, u0l1, u0l2, fr1, fr2, u0r1, u0r2, initial_T, kp1, kp2, S12);
  vo.stereocovar(fl1, fl2, u0l1, u0l2, fr1, fr2, u0r1, u0r2, initial_T, kp3, kp4, S34);

  mean_1_2=cv::Mat::zeros(3,1,CV_64F);
  vo.mean_points(inliers_1_2, mean_1_2);
  
  mean_3_4=cv::Mat::zeros(3,1,CV_64F);
  vo.mean_points(inliers_3_4, mean_3_4);
    
  //Computing the transformation between two states
  
  vo.compute_RT(inliers_1_2, inliers_3_4, mean_1_2, mean_3_4, R, T, RT);
  
  inliers=kp1.cols;
  
  //getting the covariance of the transformation
    
  vo.covarRT(inliers, R, T, S12, S34, mean_1_2, mean_3_4, inliers_1_2, inliers_3_4, Srt);
    
  vo.JxJu(R, T, H, Jx, Ju);
    
  cv::transpose(Jx, Jxt);
  cv::transpose(Ju, Jut);
    
  //getting the error propagation for each iteration
    
  Hcov=Jx*Hcov*Jxt+Ju*Srt*Jut;
    
  //updating the trasformation for each iteration
    
  H=H*RT;
  point=RT*point;
  
  cout<<"current position: "<<point<<endl;
  
  //results
  
  cout<<"Transformation: "<< RT << endl; 
  cout<<"Updated transformation: "<< H << endl;
  cout<<"Updated error propagation: "<<Hcov<<endl;
  
}

void StereoVisualOdometryAlgorithm::extract_ypr(cv::Mat& H,double& y,double& p,double& r)
{
  CStereo_Vo vo;
  cv::Mat R;
  
  R=cv::Mat::zeros(3,3,CV_64F);
  R.at<double>(0,0)=H.at<double>(0,0);
  R.at<double>(0,1)=H.at<double>(0,1);
  R.at<double>(0,2)=H.at<double>(0,2);
  R.at<double>(1,0)=H.at<double>(1,0);
  R.at<double>(1,1)=H.at<double>(1,1);
  R.at<double>(1,2)=H.at<double>(1,2);
  R.at<double>(2,0)=H.at<double>(2,0);
  R.at<double>(2,1)=H.at<double>(2,1);
  R.at<double>(2,2)=H.at<double>(2,2);
  
  r=atan2(R.at<double>(1,0), R.at<double>(0,0));
  p=atan2(-R.at<double>(2,0), R.at<double>(0,0)*cos(r)+R.at<double>(1,0)*sin(r));
  y=atan2(R.at<double>(2,1), R.at<double>(2,2));
  
  //cout <<"yaw: "<<y<<"    pitch: "<<p<<"      roll: "<<r<<endl;
  
}