uncalibvs_alg.cpp

Go to the documentation of this file.

#include "uncalibvs_alg.h"

// Class Constructor
Uncalibvs_Algorithm::Uncalibvs_Algorithm(void) {
   base_rtw();
   initZeros();
}

// Class Destructor
Uncalibvs_Algorithm::~Uncalibvs_Algorithm(void){
}

// Save Current Config
void Uncalibvs_Algorithm::config_update(Config& new_cfg, uint32_t level){
  this->lock();

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

// Uncalibvs_simAlgorithm Public API
void Uncalibvs_Algorithm::uncalib_vs(const bool& traditional, const bool& random_points, const bool& quadrotor, const bool& arm, const bool& output_files, const std::string& path, const double& lambda, const double& final_z, const Eigen::Matrix4f& cTo, const double& secs, const Eigen::MatrixXf& V2, KDL::Chain& chain, Eigen::MatrixXf& QQ,  const Eigen::Matrix3f& Rquad_inertial, Eigen::MatrixXf& Vel_quad){
  this->lock();
  
  traditional_=traditional;
  random_points_=random_points;
  quadrotor_=quadrotor;
  arm_=arm;
  output_files_=output_files;
  path_=path;
  cTo_=cTo;
  secs_=secs;
  V2_=V2;
  chain_=chain;
  QQ_=QQ;
  lambda_=lambda;
  final_z_=final_z;
  
  if (secs_==0) {
   base_rtw();
   initZeros();
  }

  // Desired position of the object r.t. camera (invert to see the camera r.t. object)
  T0c_x_.block(0,0,3,3) << -1, 0, 0, 0, -1, 0, 0, 0, 1;
  T0c_x_.col(3) << 0, 0.2, -final_z_, 1;
  T0c_x_.row(3) << 0, 0, 0, 1;
  
  T0c_0_=cTo_.inverse();

   
  if(output_files_){
    const char * outputFilename  = path_.c_str();
    std::ofstream myfile;
    myfile.open (outputFilename, std::ios::out | std::ios::app | std::ios::binary);
    myfile << cTo_;
    myfile << "\n \n";
    myfile.close();
  }
  
  //camera update
  //T0c_new = diTj_int + T0c_0;
  //Tc0 = T0c_new.inverse();

  T0c_new_=T0c_0_;
  Tc0_=cTo_;
  //Actual features
  //Features transform r.t.camera 
  cP_ = Tc0_ * P0_;
  //image_projection(f,ff,u0,v0,cP);

  //features_vector(cP);
  ccP_=cP_;

  features_vector();
  S_ = s_;

  //Desired features_vector
  Eigen::Matrix4f Tc0_x;
  Tc0_x=T0c_x_.inverse();
  cP_x_ = Tc0_x * P0_; 
  //image_projection(f,ff,u0,v0,cP_x);     
  //features_vector(cP);
  ccP_=cP_x_;
   
  features_vector();
  S_x_ = s_;
 
  //Computation of image jacobian
  //Object coordinates r.t. camera frame (in this case world)
  pj_ = Tc0_ * pn_;
  
  //image_projection(f,f,u0,v0,pj);
  image_projection(ff_,ff_);

  //noise add
  Eigen::MatrixXf ran(2,n_);
  ran.setRandom();
  uv_ = uv_ + noise_ * ran;

  //Jacobian
  if (traditional_){
    compute_jacobian_t();
  }
  else {
    compute_jacobian_u();  
  }
  
  //Diferential change of T0c under Vc
  K_control(); 

  Eigen::Matrix4f diTj;
  diTj=Eigen::Matrix4f::Zero();
  
  diTj = T0c_new_ * V_mod_;

  //Integration aproximation
  diTj_int_ = diTj_int_ + diTj * secs_;
      
  if (quadrotor_){ 

    if (arm_){
      qa_kinematics();
      qa_control();

      //Arm positions
      QQ_.block(6,0,5,1) = QQ_.block(6,0,5,1)+Velqa_.block(6,0,5,1) * secs_;
      QQ=QQ_;
    }
    else{
      q_control();
    }
  }
  
//   Eigen::Matrix3f Rot; 
//   Eigen::MatrixXf angles(3,1);
//   double psi, theta, phi;
//   
//   Rot = Tc0_.block(0,0,3,3); 
//   angles=Rot.eulerAngles(2,1,0);
//   //angles=Rquad_inertial_.eulerAngles(2,1,0);
//   
//   psi=angles(0);
//   theta=angles(1);
//   //convert yaw between 0..2pi
//   //if (angles(2)<0) angles(2)=-angles(2)+3.1416;
//   //phi=angles(2);
//   phi=0;
//  
//   ROS_INFO("roll: %2.4f",psi);
//   ROS_INFO("pitch: %2.4f",theta);
//   ROS_INFO("yaw: %2.4f",phi);
//   
//   Eigen::Matrix3f Ri,Ti;
//   
//   Ri(0,0)= cos(phi)*cos(theta);
//   Ri(0,1)= cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
//   Ri(0,2)= cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
//   Ri(1,0)= sin(phi)*cos(theta);
//   Ri(1,1)= sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi);
//   Ri(1,2)= sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi);
//   Ri(2,0)= -sin(theta);
//   Ri(2,1)= cos(theta)*sin(psi);
//   Ri(2,2)= cos(theta)*cos(psi);
//   
//    Vel_.block(0,0,3,1)=Rquad_inertial_*Vel_.block(0,0,3,1);
  
//   Ti(0,0)= 1;
//   Ti(0,1)= sin(psi)*tan(theta);
//   Ti(0,2)= cos(psi)*tan(theta);
//   Ti(1,0)= 0;
//   Ti(1,1)= cos(psi);
//   Ti(1,2)= -sin(psi);
//   Ti(2,0)= 0;
//   Ti(2,1)= sin(psi)/cos(theta);
//   Ti(2,2)= cos(psi)/cos(theta);
//    
//   Eigen::MatrixXf Jbi(6,6);
//    
//   Jbi=Eigen::MatrixXf::Zero(6,6);
//      
//   Jbi.block(0,0,3,3)=Ri;
//   Jbi.block(3,3,3,3)=Ti;
//   
//   Eigen::MatrixXf J4dof(6,4);
//   J4dof.block(0,0,6,3)=Jbi.block(0,0,6,3);
//   J4dof.block(0,3,6,1)=Jbi.block(0,5,6,1);
// 
//   Vel_.block(0,0,4,1)=J4dof*Vel_.block(0,0,4,1);
  
  Vel_quad=Vel_;
  
  this->unlock();
}

void Uncalibvs_Algorithm::base_rtw(){

  if (random_points_){  
    n_=9;
    // Points of the object
    pn_ = Eigen::MatrixXf::Zero(4,n_);
    pn_=1*pn_.setRandom();
    pn_.row(3) = Eigen::VectorXf::Ones(n_);

    cj_0_= Eigen::MatrixXf::Zero(3,3); 
    cj_0_(0,0)=pn_.row(0).mean();
    cj_0_(1,0)=pn_.row(1).mean();
    cj_0_(2,0)=pn_.row(2).mean();

    // Computing the base points
    Eigen::MatrixXf q(3,n_);
    q=pn_.block(0,0,3,n_);
    q.colwise()-=cj_0_.col(0);

    // Assemble the correlation matrix Q = q * q'
    Eigen::Matrix3f Q = (q * q.transpose());

    // Compute the Singular Value Decomposition
    Eigen::JacobiSVD<Eigen::Matrix3f> svd (Q, Eigen::ComputeFullU | Eigen::ComputeFullV);
    //Eigen::Matrix3f u = svd.matrixU ();
    Eigen::Matrix3f v = svd.matrixV ();

    cj_0_.col(1)=cj_0_.col(0)+v.col(0);
    //with this library the second column of v appear sign changed
    cj_0_.col(2)=cj_0_.col(0)-v.col(1);

    // check the planarity
    if ( svd.singularValues()(2)<0.00001){
      planar_=true; 
    }
    else {
      planar_=false; 
    }

    Eigen::MatrixXf dd1(3,1),dd2(3,1),dd3(3,1);       
    if (planar_){
      std::cout << "\n" << \
      "**************************************** Planar case ***************************************" \
      << std::endl;  
      P0_= Eigen::MatrixXf::Zero(4,3);  
      P0_.block(0,0,3,3)=cj_0_;
      P0_.row(3) << 1, 1, 1;  
   
      Eigen::MatrixXf D(3,2),DD(2,2);
    
      dd1=cj_0_.col(1).array()-cj_0_.col(0).array();
      dd2=cj_0_.col(2).array()-cj_0_.col(0).array();
      D.col(0)=dd1;
      D.col(1)=dd2;
      DD=D.transpose()*D;
    
      // alfas linear combination of vectors
      alfas_= Eigen::MatrixXf::Zero(3,n_);
      // alfas.resize(3,n);
      alfas_.block(1,0,2,4)=(DD.inverse()*D.transpose()) * q;
      //alfas.block(1,0,2,4)=D.inverse() * q;
      alfas_.row(0)=Eigen::MatrixXf::Ones(1,n_)-alfas_.block(1,0,2,n_).colwise().sum();
    }  
    else{
      std::cout << "\n" << \
      "**************************************** NON Planar case ***************************************" \
      << std::endl;
    
      // resizing the base points to 4
      Eigen::Matrix3f Temp;
      Temp=cj_0_;
      cj_0_.resize(3,4);
      cj_0_.block(0,0,3,3)=Temp;
    
      cj_0_.col(3)=cj_0_.col(0)+v.col(2);
      P0_= Eigen::MatrixXf::Zero(4,4); 
      // P0.resize(4,4);
      P0_.block(0,0,3,4)=cj_0_;
      P0_.row(3) << 1, 1, 1, 1;

      dd1=cj_0_.col(1).array()-cj_0_.col(0).array();
      dd2=cj_0_.col(2).array()-cj_0_.col(0).array();
      dd3=cj_0_.col(3).array()-cj_0_.col(0).array();

      Eigen::MatrixXf D(3,3),DD(2,2);
      D.col(0)=dd1;
      D.col(1)=dd2;
      D.col(2)=dd3;
      DD=D.transpose()*D;
      //alfas linear combination of vectors
      alfas_= Eigen::MatrixXf::Zero(4,n_);
      // alfas.resize(4,n);
      alfas_.block(1,0,3,n_)=(DD.inverse()*D.transpose()) * q;
      // alfas.block(1,0,3,4)=D.inverse() * q;
      alfas_.row(0)=Eigen::MatrixXf::Ones(1,n_)-alfas_.block(1,0,3,n_).colwise().sum();  
    }
  }
  else {
    n_=9;
  
    //original points
    pn_ = Eigen::MatrixXf::Zero(4,n_);
    pn_.row(0) << -0.124725, -0.431413, 0.375723, 0.658402, -0.29928, 0.314608, -0.203127, -0.0350187, -0.70468;
    pn_.row(1) << 0.86367, 0.477069, -0.668052, -0.339326, 0.37334, 0.717353, 0.629534, -0.56835, 0.762124;
    pn_.row(2) << 0.86162, 0.279958, -0.119791, -0.542064, 0.912936, -0.12088, 0.368437, 0.900505, 0.282161;
    pn_.row(3) << 1, 1, 1, 1, 1, 1, 1, 1, 1;

    cj_0_= Eigen::MatrixXf::Zero(3,4);
  
    cj_0_.row(0) << -0.0499455, -0.559669, 0.127189, 0.791961;
    cj_0_.row(1) << 0.249707, 0.962512, 0.884623, 0.547683;
    cj_0_.row(2) << 0.313654, 0.795411, -0.438347, 0.763547;

    //base points
    P0_ = Eigen::MatrixXf::Zero(4,4);
  
    P0_.row(0) << -0.0499456, -0.559669, 0.127189, 0.791961;
    P0_.row(1) << 0.249707, 0.962513, 0.884623, 0.547683;
    P0_.row(2) << 0.313654, 0.795411, -0.438347, 0.763547;
    P0_.row(3) << 1, 1, 1, 1;
    
    //alfas: linear combination of vectors
    alfas_= Eigen::MatrixXf::Zero(4,n_); 
    alfas_.row(0) << -0.0707542, 0.826173, 2.37142, 1.76233, 0.815866, 0.122787, 0.443096, 2.23324, 0.495841;
    alfas_.row(1) << 0.73974, 0.340274, -1.07997, -1.19318, 0.503927, -0.0618209, 0.375215, -0.308004, 0.683816;
    alfas_.row(2) << -0.0355018, 0.102124, -0.181349, 0.394987, -0.41633, 0.68826, 0.172827, -0.958066, 0.233048;
    alfas_.row(3) << 0.366516, -0.268571, -0.110102, 0.0358625, 0.0965375, 0.250773, 0.00886206, 0.0328262, -0.412705;    

//  //original points
//     pn_ = Eigen::MatrixXf::Zero(4,n_);
//     pn_.row(0) << 0.0970, 0.1089, 0.0919, 0.1033, 0.0829, 0.1032, 0.0997, 0.1109, 0.1008;
//     pn_.row(1) << 0.1029, 0.0885, 0.0706, 0.0925, 0.0990, 0.1031, 0.0984, 0.1111, 0.0879;
//     pn_.row(2) << 0.0921, 0.0893, 0.1144, 0.1137, 0.0976, 0.0914, 0.1063, 0.0914, 0.0889;
//     pn_.row(3) << 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000;
// 
//     cj_0_= Eigen::MatrixXf::Zero(3,4);
//   
//     cj_0_.row(0) << 0.0100, -0.0208, 0.0801, 0.0743;
//     cj_0_.row(1) << 0.0095, -0.0647, -0.0505, 0.0393;
//     cj_0_.row(2) << 0.0098, 0.0694, -0.0287, 0.0804;
// 
//   
//     //base points
//     P0_ = Eigen::MatrixXf::Zero(4,4);
//   
//     P0_.row(0) << 0.0100, -0.0208, 0.0801, 0.0743;
//     P0_.row(1) << 0.0095, -0.0647, -0.0505, 0.0393;
//     P0_.row(2) << 0.0098, 0.0694, -0.0287, 0.0804;
//     P0_.row(3) << 1.0000, 1.0000, 1.0000, 1.0000;
// 
//     //alfas: linear combination of vectors
//     alfas_= Eigen::MatrixXf::Zero(4,n_); 
//     alfas_.row(0) << 1.1706, 0.9225, 0.6814, 0.7986, 1.2252, 1.1152, 0.9649, 1.1180, 1.0036;
//     alfas_.row(1) << -0.0879, -0.0345, 0.3005, 0.0990, 0.0174, -0.1131, 0.0219, -0.1960, -0.0072;
//     alfas_.row(2) << -0.0446, 0.1363, 0.0285, -0.0207, -0.1405, 0.0009, -0.0524, 0.0073, 0.0852;
//     alfas_.row(3) << -0.0381, -0.0243, -0.0104, 0.1232, -0.1020, -0.0030, 0.0656, 0.0707, -0.0816;
//     
    
    planar_=false;
  }
}

void Uncalibvs_Algorithm::initZeros(){

  if (planar_){
  //features generic
  s_= Eigen::MatrixXf::Zero(6,1);
  //actual features
  S_= Eigen::MatrixXf::Zero(6,1);
  //desired features
  S_x_= Eigen::MatrixXf::Zero(6,1);
  //Actual points camera frame
  cP_= Eigen::MatrixXf::Zero(4,3);
  //Desired points camera frame
  cP_x_= Eigen::MatrixXf::Zero(4,3);
  //Points generic camera frame
  ccP_= Eigen::MatrixXf::Zero(4,3);
  //Image Jacobian
  J_= Eigen::MatrixXf::Zero(6,6);
  diff_=Eigen::MatrixXf::Zero(6,1);
  }
  else {


  //features generic
  s_= Eigen::MatrixXf::Zero(8,1);
  //actual features
  S_= Eigen::MatrixXf::Zero(8,1);
  //desired features
  S_x_= Eigen::MatrixXf::Zero(8,1);
  //Actual points camera frame
  cP_= Eigen::MatrixXf::Zero(4,4);
  //Desired points camera frame
  cP_x_= Eigen::MatrixXf::Zero(4,4);
  //Points generic camera frame
  ccP_= Eigen::MatrixXf::Zero(4,4);
  //Image Jacobian
  J_= Eigen::MatrixXf::Zero(8,6);
  diff_=Eigen::MatrixXf::Zero(8,1);
  }


  //image coordinates
  uv_= Eigen::MatrixXf::Zero(2,n_);  
  //Initial integral condition
  diTj_int_=Eigen::Matrix4f::Zero();
  //Velocity commands
  Vel_= Eigen::MatrixXf::Zero(6,1);  

//  lambda=0.125;

  u0_=376;
  v0_=240;
  ff_=10;
  ff_old_=ff_;
  noise_=0;
  
  //Arm initial positions
//   q_ = Eigen::MatrixXf::Zero(11,1);
  
}

void Uncalibvs_Algorithm::image_projection(const float& fu, const float& fv){  

  //image plane projection
  uv_.row(0) = (pj_.row(0).array()/pj_.row(2).array()) * fu + u0_;
  uv_.row(1) = (pj_.row(1).array()/pj_.row(2).array()) * fv + v0_;  
}

void Uncalibvs_Algorithm::features_vector(){

  
  if (planar_){
    Eigen::ArrayXXf xx(1,3),yy(1,3);
    //features vector
    xx=(ccP_.row(0).array()/ccP_.row(2).array());
    yy=(ccP_.row(1).array()/ccP_.row(2).array());  
    //reshape (actually Eigen has no reshape function)
    s_(0,0)=xx(0,0);
    s_(1,0)=yy(0,0);
    s_(2,0)=xx(0,1);
    s_(3,0)=yy(0,1);
    s_(4,0)=xx(0,2);
    s_(5,0)=yy(0,2);
  }
  else {
    Eigen::ArrayXXf xx(1,4),yy(1,4);
    //features vector
    xx=(ccP_.row(0).array()/ccP_.row(2).array());
    yy=(ccP_.row(1).array()/ccP_.row(2).array());  
    //reshape (actually Eigen has no reshape function)
    s_(0,0)=xx(0,0);
    s_(1,0)=yy(0,0);
    s_(2,0)=xx(0,1);
    s_(3,0)=yy(0,1);
    s_(4,0)=xx(0,2);
    s_(5,0)=yy(0,2);    
    s_(6,0)=xx(0,3);  
    s_(7,0)=yy(0,3);
  }
}

void Uncalibvs_Algorithm::compute_jacobian_t(){
  if (planar_){
    Eigen::ArrayXXf xx(1,3),yy(1,3),zz(1,3);
    //features vector
    xx=(cP_.row(0).array()/cP_.row(2).array());
    yy=(cP_.row(1).array()/cP_.row(2).array());  
    zz=(cP_.row(2).array());
    //Computing Image Jacobian
    float xj,yj,zj;
    for (int i=0; i<3; ++i){
      xj=xx(0,i);
      yj=yy(0,i);
      zj=zz(0,i);
      J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
      J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
    }
  }
  else {
    Eigen::ArrayXXf xx(1,4),yy(1,4),zz(1,4);
    //features vector
    xx=(cP_.row(0).array()/cP_.row(2).array());
    yy=(cP_.row(1).array()/cP_.row(2).array());  
    zz=(cP_.row(2).array());
    //Computing Image Jacobian
    float xj,yj,zj;
    for (int i=0; i<4; ++i){
      xj=xx(0,i);
      yj=yy(0,i);
      zj=zz(0,i);
      J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
      J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
    }         
  }

  // Frame changes 
  Eigen::MatrixXf Jtemp(J_.rows(),J_.cols()); 
  Jtemp=J_;
  J_.col(0)=-Jtemp.col(1);
  J_.col(1)=-Jtemp.col(0);
  J_.col(2)=-Jtemp.col(2);
  J_.col(3)=Jtemp.col(5);
  J_.col(4)=Jtemp.col(4);
  J_.col(5)=Jtemp.col(3); 
  
}

void Uncalibvs_Algorithm::compute_jacobian_u(){
  if (planar_) { 
    Eigen::MatrixXf m;
    m=Eigen::MatrixXf::Zero((2*n_),9);
    
    //Generate M and check the rank
    for (int i=0; i<n_; ++i){
      for (int j=0; j<3; ++j){
        //u rows
        m((2*i),(j*3))=alfas_(j,i);
        m((2*i),(j*3)+2)=alfas_(j,i)*(u0_-uv_(0,i));
        //v rows
        m((2*i)+1,(j*3)+1)=alfas_(j,i);
        m((2*i)+1,(j*3)+2)=alfas_(j,i)*(v0_-uv_(1,i));
      }   
    }
    
    // Assemble the correlation matrix M = m' * m
    Eigen::MatrixXf M(9,9),UU(9,9),VV(9,9);
    M = (m.transpose() * m);

    // Compute the Singular Value Decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(M);
    Eigen::VectorXf v(9,1);
    v =eigensolver.eigenvectors().col(0);

    //Distance equations
    int g=0;
    Eigen::Vector3f d,va,vb,vc,w;
    Eigen::Vector2f b;
    Eigen::MatrixXf L(3,2);
    L=Eigen::MatrixXf::Zero(3,2);
    
    for (int i=0; i<2; ++i){
      for (int j=i+1; j<3; ++j){
        va=v.block(i*3,0,3,1);
        vb=v.block(j*3,0,3,1);
        vc=va-vb;
        L(g,0)=(vc.topRows(2).transpose()*vc.topRows(2));
        L(g,1)= (vc(2)*vc(2));
        w = cj_0_.col(i)-cj_0_.col(j);
        d(g,0)=(w.transpose() * w);
        g=g+1;
      }
    }
  
    Eigen::Matrix2f LL;
    Eigen::MatrixXf L_inv(3,2);

    LL=(L.transpose()*L);
    L_inv=LL.inverse()*L.transpose();
    b(0)= L_inv.row(0) * d;
    b(1)= L_inv.row(1) * d;

    //Obtaining Beta and ff

    float Beta=+sqrt(fabs(b(0)));
    ff_=+sqrt(fabs(b(1)))/Beta;

    ff_old_=ff_;

    Eigen::VectorXf C2(9,1);

    C2=Beta*v;

    //Check the sqrt correct sign
    Eigen::Matrix3f check;
    check.col(0)=C2.block(0,0,3,1);
    check.col(1)=C2.block(3,0,3,1);
    check.col(2)=C2.block(6,0,3,1);
    
    if (check.determinant()>0){
      C2=-C2;  
    }
    C2(2,0)=C2(2,0)*ff_;
    C2(5,0)=C2(5,0)*ff_;
    C2(8,0)=C2(8,0)*ff_;

    //Computing Image Jacobian
    float xj,yj,zj;
    for (int i=0; i<3; ++i){
      xj=C2((i*3),0);
      yj=C2((i*3)+1,0);
      zj=C2((i*3)+2,0);
      J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
      J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
    }
  }

  else {
    Eigen::MatrixXf m;
    m=Eigen::MatrixXf::Zero((2*n_),12);

    //Generate M and check the rank
    for (int i=0; i<n_; ++i){
      for (int j=0; j<4; ++j){
        //u rows
        m((2*i),(j*3))=alfas_(j,i);
        m((2*i),(j*3)+2)=alfas_(j,i)*(u0_-uv_(0,i));
    
        //v rows
        m((2*i)+1,(j*3)+1)=alfas_(j,i);
        m((2*i)+1,(j*3)+2)=alfas_(j,i)*(v0_-uv_(1,i));
      }   
    }

    // Assemble the correlation matrix M = m' * m
    Eigen::MatrixXf M(12,12),UU(12,12),VV(12,12);
    M = (m.transpose() * m);

    // Compute the Singular Value Decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(M);
    Eigen::VectorXf v(12,1);
    v =eigensolver.eigenvectors().col(0);

    //Distance equations
    int g=0;
    Eigen::Vector3f va,vb,vc,w;
    Eigen::VectorXf d(6,1);
    Eigen::Vector2f b;
    Eigen::MatrixXf L;
    L=Eigen::MatrixXf::Zero(6,2);
    
    for (int i=0; i<3; ++i){
      for (int j=i+1; j<4; ++j){
        va=v.block(i*3,0,3,1);
        vb=v.block(j*3,0,3,1);
        vc=va-vb;
        L(g,0)=(vc.topRows(2).transpose()*vc.topRows(2));
        L(g,1)= (vc(2)*vc(2));
        w = cj_0_.col(i)-cj_0_.col(j);
        d(g,0)=(w.transpose() * w);
        g=g+1;
      }
    }

    Eigen::Matrix2f LL;
    Eigen::MatrixXf L_inv(6,2);

    LL=(L.transpose()*L);
    L_inv=LL.inverse()*L.transpose();
    b(0)= L_inv.row(0) * d;
    b(1)= L_inv.row(1) * d;
    
    //Obtaining Beta and ff
    float Beta=+sqrt(fabs(b(0)));
    ff_=+sqrt(fabs(b(1)))/Beta;

    ff_old_=ff_;
    
    Eigen::VectorXf C2(12,1);

    C2=Beta*v;

    //Check the sqrt correct sign
    Eigen::Vector3f cc1,cc2,cc3;
    cc1=(C2.block(3,0,3,1)-C2.block(0,0,3,1));
    cc2=(C2.block(6,0,3,1)-C2.block(0,0,3,1));
    cc3=(C2.block(9,0,3,1)-C2.block(0,0,3,1));
     
    Eigen::Vector3f ccc1,ccc2,ccc3;
    ccc1=cc1.cross(cc2);
    ccc2=cc1.cross(cc3);
    ccc3=cc2.cross(cc3);
     
    float oo1,oo2,oo3; 
    oo1=(ccc1.transpose()*cc3);
    oo2=(ccc2.transpose()*cc2);
    oo3=(ccc3.transpose()*cc1);
     
    if ((oo1 < 0) || (oo2>0) || (oo3<0)){
      C2=-C2;  
    }
    C2(2,0)=C2(2,0)*ff_;
    C2(5,0)=C2(5,0)*ff_;
    C2(8,0)=C2(8,0)*ff_;
    C2(11,0)=C2(11,0)*ff_;
    
    //Computing Image Jacobian
    float xj,yj,zj;
    for (int i=0; i<4; ++i){
      xj=C2((i*3),0);
      yj=C2((i*3)+1,0);
      zj=C2((i*3)+2,0);
      J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
      J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
    }   
  }

  // Frame changes 
  Eigen::MatrixXf Jtemp(J_.rows(),J_.cols()); 
  Jtemp=J_;
  J_.col(0)=-Jtemp.col(1);
  J_.col(1)=-Jtemp.col(0);
  J_.col(2)=-Jtemp.col(2);
  J_.col(3)=Jtemp.col(5);
  J_.col(4)=Jtemp.col(4);
  J_.col(5)=Jtemp.col(3);  
}

void Uncalibvs_Algorithm::K_control(){
  if (planar_) {
    // features error
    diff_=S_.array()-S_x_.array();
    
    //Proportional control
    Eigen::MatrixXf J_pseudo(6,6);
    J_pseudo=Eigen::MatrixXf::Zero(6,6);
    J_pseudo=J_.inverse();
    Vel_=-lambda_*(J_pseudo*diff_);
  }

  else {
    // features error
    diff_=S_.array()-S_x_.array();
    
    Eigen::MatrixXf JJ(6,6);

    JJ=J_.transpose()*J_;
    Eigen::MatrixXf J_pseudo(8,6);
    J_pseudo=Eigen::MatrixXf::Zero(8,6);
    J_pseudo=JJ.inverse()*J_.transpose();  

    //Proportional control
    Vel_=-lambda_*(J_pseudo*diff_);
  }
  
  Eigen::MatrixXf v_linear(3,1),v_angular(3,1);
  Eigen::Matrix3f skew_v_angular;
    
  //Lineal Velocity
  v_linear = Vel_.block(0,0,3,1);
    
  //Angular velocity
  v_angular = Vel_.block(3,0,3,1);

  //Skew-symmetric matrix 
  skew_v_angular.row(0) << 0, -v_angular(2), v_angular(1);
  skew_v_angular.row(1) << v_angular(2), 0, -v_angular(0);
  skew_v_angular.row(2) << -v_angular(1), v_angular(0), 0;

  //Using modified velocity matrix 
  V_mod_.block(0,0,3,3)=skew_v_angular;
  V_mod_.block(0,3,3,1)=v_linear;
  V_mod_.row(3) << 0, 0, 0, 0; 
  

}

void Uncalibvs_Algorithm::q_control(){
  // Underactuated Quadrotor /////////////////////////////////////////////        
  
  Eigen::MatrixXf J1(8,4),J2(8,2),V1(4,1);
      
  J1.col(0)=J_.col(0);
  J1.col(1)=J_.col(1);
  J1.col(2)=J_.col(2);
  J1.col(3)=J_.col(5);
  J2.col(0)=J_.col(3);
  J2.col(1)=J_.col(4);

  //J1.col(0)=J_.col(2);
  //J1.col(1)=J_.col(3);
  //J1.col(2)=J_.col(4);
  //J1.col(3)=J_.col(5);
  //J2.col(0)=J_.col(0);
  //J2.col(1)=J_.col(1); 
 

  Eigen::MatrixXf JJ1(4,4);
  Eigen::MatrixXf J1_pseudo(4,8);
  //pseudo inverse of J1
  JJ1=J1.transpose()*J1;
  J1_pseudo=JJ1.inverse()*J1.transpose();  
 
  //V1=-J1_pseudo*((lambda*diff)+(J2*V2_));
  V1=-J1_pseudo*lambda_*(diff_+(J2*V2_));
      
  Eigen::MatrixXf Velq(6,1);
  Velq=Eigen::MatrixXf::Zero(6,1);

//   Velq.block(2,0,4,1)=V1.block(0,0,4,1);
  Velq.block(0,0,3,1)=V1.block(0,0,3,1);
  Velq(5,0)=-V1(3,0)/2;
      
  Vel_=Velq.block(0,0,6,1);
  
}

void Uncalibvs_Algorithm::qa_control(){
    // Underactuated Quadrotor /////////////////////////////////////////////////// 

          
    Eigen::MatrixXf J1(8,4),J2(8,2),V1(4,1),Jqa1(6,9),Jqa2(6,2),Vtotal(9,1);
    
    Jqa1.block(0,0,6,3)=Jqa_.block(0,0,6,3);
    Jqa1.block(0,3,6,6)=Jqa_.block(0,5,6,6);
    Jqa2=Jqa_.block(0,3,6,2);
    
    Eigen::MatrixXf eye(9,9);
    eye=Eigen::MatrixXf::Identity(9,9);

    Eigen::MatrixXf res(9,1),res2(9,9);
    Eigen::MatrixXf b(6,1);
    
    b=Vel_-Jqa2*V2_;

    res = Jqa1.colPivHouseholderQr().solve(b);
//     res = Jqa1.fullPivHouseholderQr().solve(b);
    res2= Jqa1.colPivHouseholderQr().solve(Jqa1);
    
    v_null_space();
    
    Vtotal=res+(eye-res2)*Vqa0_;

    Velqa_=Eigen::MatrixXf::Zero(11,1);

    Velqa_.block(0,0,3,1)=Vtotal.block(0,0,3,1);
    //Velqa.block(3,0,2,1)=Eigen::MatrixXf::Zero(2,1);
    Velqa_.block(5,0,6,1)=Vtotal.block(3,0,6,1);
      
    Velqa_.block(6,0,1,1)=-Velqa_.block(6,0,1,1);
    Vel_=Velqa_.block(0,0,6,1);
}

void Uncalibvs_Algorithm::v_null_space(){
  Vqa0_=Eigen::MatrixXf::Zero(9,1);
  float qxma,qxmi,qxm,qyma,qymi,qym,qzma,qzmi,qzm,qyama,qyami,qyam,a0ma,a0mi,a0m,a1ma,a1mi,a1m,a2ma,a2mi,a2m,a3ma,a3mi,a3m,a4ma,a4mi,a4m;
      
  //DOF's
  float ndof=9;
            
  //quadrotor Limits
  qxma=10;
  qxmi=-10;
  qxm=0.0;
      
  qyma=10;
  qymi=-10;
  qym=0.0;
      
  qzma=10;
  qzmi=1.0;
  qzm=4.0;
      
  qyama=3.1416;
  qyami=-3.1416;
  qyam=0.0;
      
  //arm limits
  a0ma=0.548;
  a0mi=-0.548;
  a0m=0.0;
  //a0ma=0.3;
  //a0mi=-0.3;
  //a0m=0.0;
      
  a1ma=1.57;
  a1mi=-1.57;
  a1m=0.0;
  //a1ma=1.0;
  //a1mi=-1.0;
  //a1m=0.0;
      
  a2ma=5.23;
  a2mi=0;
  a2m=2.615;
  //a2ma=5.0;
  //a2mi=0.0;
  //a2m=2.5;      
       
  a3ma=5.23;
  a3mi=0;
  a3m=2.615;
  //a3ma=5.0;
  //a3mi=0.0;
  //a3m=2.5;
      
  a4ma=3.1416;
  a4mi=-3.1416;
  a4m=0.0;
  //a4ma=6.0;
  //a4mi=0.0;
  //a4m=3.0;
      
          
  Vqa0_(0,0)=(-0/ndof)*((QQ_(0,0)-qxm)/((qxma-qxmi)*(qxma-qxmi)));
  Vqa0_(1,0)=(-0/ndof)*((QQ_(1,0)-qym)/((qyma-qymi)*(qyma-qymi)));
  Vqa0_(2,0)=(-50/ndof)*((QQ_(2,0)-qzm)/((qzma-qzmi)*(qzma-qzmi)));
  Vqa0_(3,0)=(-30/ndof)*((QQ_(5,0)-qyam)/((qyama-qyami)*(qyama-qyami)));
  Vqa0_(4,0)=(-100/ndof)*((QQ_(6,0)-a0m)/((a0ma-a0mi)*(a0ma-a0mi)));
  Vqa0_(5,0)=(-100/ndof)*((QQ_(7,0)-a1m)/((a1ma-a1mi)*(a1ma-a1mi)));
  Vqa0_(6,0)=(-100/ndof)*((QQ_(8,0)-a2m)/((a2ma-a2mi)*(a2ma-a2mi)));
  Vqa0_(7,0)=(-100/ndof)*((QQ_(9,0)-a3m)/((a3ma-a3mi)*(a3ma-a3mi)));
  Vqa0_(8,0)=(-30/ndof)*((QQ_(10,0)-a4m)/((a4ma-a4mi)*(a4ma-a4mi)));
}

void Uncalibvs_Algorithm::qa_kinematics(){
  //Initialize Quadrotor and Arm terms
  // constructs the kdl solvers in non-realtime
  jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
  jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
    
  // Create joint array
  unsigned int nj = chain_.getNrOfJoints();
  jnt_pos_ = KDL::JntArray(nj);
  // resizes the joint state vectors in non-realtime
  jnt_pos_.resize(nj);
  jacobian_.resize(nj);
    
  for(unsigned int i=0; i < nj; i++){
    jnt_pos_.data(i) = QQ_(6+i,0);
  }

  // computes Cartesian pose in realtime
  KDL::Frame kdlTarm;
  jnt_to_pose_solver_->JntToCart(jnt_pos_, kdlTarm);
    
  double ro,pit,ya;
  kdlTarm.M.GetEulerZYX(ya,pit,ro);
    
  Eigen::Matrix4f Tarm, Tiner;
  Eigen::Matrix3f Rarm;
  // euler convention zyx    
  Rarm = Eigen::AngleAxisf(ro, Eigen::Vector3f::UnitZ()) \
        * Eigen::AngleAxisf(pit, Eigen::Vector3f::UnitY()) \
        * Eigen::AngleAxisf(ya, Eigen::Vector3f::UnitX());
 
  Tarm.block(0,0,3,3)=Rarm;
  Tarm(0,3) = (double)kdlTarm.p.data[0];
  Tarm(1,3) = (double)kdlTarm.p.data[1];
  Tarm(2,3) = (double)kdlTarm.p.data[2];
  Tarm.row(3) << 0, 0, 0, 1;
     
  Tiner=Tc0_*Tarm.inverse();
    
  //Quadrotor linear positions
  QQ_.block(0,0,3,1) = Tiner.block(0,3,3,1);
  Eigen::Matrix3f Riner;
  Riner=Tiner.block(0,0,3,3);
  //Quadrotor angles
  QQ_.block(3,0,3,1) = Riner.eulerAngles(0,1,2);
    
  // compute Jacobian in realtime
  jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
  
  Jqa_=Eigen::MatrixXf::Zero(6,11);
  
  Jqa_.block(0,0,6,6)=(Eigen::MatrixXf::Identity(6,6));
  // Jqa.block(5,5,6,5)=Eigen::Map<Eigen::MatrixXd>(jacobian_.data);
 
  //convert from doubles to float
  for (int i=0; i<6; ++i){
    for (int j=0; j<5; ++j){
      Jqa_(i,6+j)=(float)jacobian_.data(i,j);
    }
  }
}