Esempio n. 1
0
//===========================================================================
void AddOrthRow(cv::Mat &R)
{
  assert((R.rows == 3) && (R.cols == 3));
  R.db(2,0) = R.db(0,1)*R.db(1,2) - R.db(0,2)*R.db(1,1);
  R.db(2,1) = R.db(0,2)*R.db(1,0) - R.db(0,0)*R.db(1,2);
  R.db(2,2) = R.db(0,0)*R.db(1,1) - R.db(0,1)*R.db(1,0);
  return;
}
Esempio n. 2
0
//===========================================================================
void Euler2Rot(cv::Mat &R,const double pitch,const double yaw,const double roll,
	       bool full = true)
{
  if(full){if((R.rows != 3) || (R.cols != 3))R.create(3,3,CV_64F);}
  else{if((R.rows != 2) || (R.cols != 3))R.create(2,3,CV_64F);}
  double sina = sin(pitch), sinb = sin(yaw), sinc = sin(roll);
  double cosa = cos(pitch), cosb = cos(yaw), cosc = cos(roll);
  R.db(0,0) = cosb*cosc; R.db(0,1) = -cosb*sinc; R.db(0,2) = sinb;
  R.db(1,0) = cosa*sinc + sina*sinb*cosc;
  R.db(1,1) = cosa*cosc - sina*sinb*sinc;
  R.db(1,2) = -sina*cosb; if(full)AddOrthRow(R); return;
}
Esempio n. 3
0
//===========================================================================
void PDM::ApplySimT(double a,double b,double tx,double ty,cv::Mat &pglobl)
{
  assert((pglobl.rows == 6) && (pglobl.cols == 1) && (pglobl.type()==CV_64F));
  double angle = atan2(b,a),scale = a/cos(angle);
  double ca = cos(angle),sa = sin(angle);
  double xc = pglobl.db(4,0),yc = pglobl.db(5,0);
  R1_ = cv::Scalar(0); R1_.db(2,2) = 1.0;
  R1_.db(0,0) =  ca; R1_.db(0,1) = -sa; R1_.db(1,0) =  sa; R1_.db(1,1) =  ca;
  Euler2Rot(R2_,pglobl); R3_ = R1_*R2_; 
  pglobl.db(0,0) *= scale; Rot2Euler(R3_,pglobl);
  pglobl.db(4,0) = a*xc - b*yc + tx; pglobl.db(5,0) = b*xc + a*yc + ty;
  return;
}
Esempio n. 4
0
//===========================================================================
void PDM::CalcRigidJacob(cv::Mat &plocal,cv::Mat &pglobl,cv::Mat &Jacob)
{
  int i,n = _M.rows/3,m = _V.cols; double X,Y,Z;
  assert((plocal.rows == m)  && (plocal.cols == 1) && 
	 (pglobl.rows == 6)  && (pglobl.cols == 1) &&
	 (Jacob.rows == 2*n) && (Jacob.cols == 6));
  double rx[3][3] = {{0,0,0},{0,0,-1},{0,1,0}}; cv::Mat Rx(3,3,CV_64F,rx);
  double ry[3][3] = {{0,0,1},{0,0,0},{-1,0,0}}; cv::Mat Ry(3,3,CV_64F,ry);
  double rz[3][3] = {{0,-1,0},{1,0,0},{0,0,0}}; cv::Mat Rz(3,3,CV_64F,rz);
  double s = pglobl.db(0,0);
  this->CalcShape3D(S_,plocal); Euler2Rot(R_,pglobl); 
  P_ = s*R_(cv::Rect(0,0,3,2)); Px_ = P_*Rx; Py_ = P_*Ry; Pz_ = P_*Rz;
  assert(R_.isContinuous() && Px_.isContinuous() && 
	 Py_.isContinuous() && Pz_.isContinuous());
  const double* px = Px_.ptr<double>(0);
  const double* py = Py_.ptr<double>(0);
  const double* pz = Pz_.ptr<double>(0);
  const double* r  =  R_.ptr<double>(0);
  cv::MatIterator_<double> Jx = Jacob.begin<double>();
  cv::MatIterator_<double> Jy = Jx + n*6;
  for(i = 0; i < n; i++){
    X=S_.db(i,0); Y=S_.db(i+n,0); Z=S_.db(i+n*2,0);    
    *Jx++ =  r[0]*X +  r[1]*Y +  r[2]*Z;
    *Jy++ =  r[3]*X +  r[4]*Y +  r[5]*Z;
    *Jx++ = px[0]*X + px[1]*Y + px[2]*Z;
    *Jy++ = px[3]*X + px[4]*Y + px[5]*Z;
    *Jx++ = py[0]*X + py[1]*Y + py[2]*Z;
    *Jy++ = py[3]*X + py[4]*Y + py[5]*Z;
    *Jx++ = pz[0]*X + pz[1]*Y + pz[2]*Z;
    *Jy++ = pz[3]*X + pz[4]*Y + pz[5]*Z;
    *Jx++ = 1.0; *Jy++ = 0.0; *Jx++ = 0.0; *Jy++ = 1.0;
  }return;
}
Esempio n. 5
0
//===========================================================================
void PDM::CalcShape2D(cv::Mat &s,cv::Mat &plocal,cv::Mat &pglobl)
{
  assert((s.type() == CV_64F) && (plocal.type() == CV_64F) && 
	 (pglobl.type() == CV_64F));
  assert((plocal.rows == _E.cols) && (plocal.cols == 1));
  assert((pglobl.rows == 6) && (pglobl.cols == 1));
  int n = _M.rows/3; double a=pglobl.db(0,0),x=pglobl.db(4,0),y=pglobl.db(5,0);
  Euler2Rot(R_,pglobl); S_ = _M + _V*plocal;
  if((s.rows != _M.rows) || (s.cols = 1))s.create(2*n,1,CV_64F);
  for(int i = 0; i < n; i++){
    s.db(i  ,0) = a*( R_.db(0,0)*S_.db(i    ,0) + R_.db(0,1)*S_.db(i+n  ,0) +
		      R_.db(0,2)*S_.db(i+n*2,0) )+x;
    s.db(i+n,0) = a*( R_.db(1,0)*S_.db(i    ,0) + R_.db(1,1)*S_.db(i+n  ,0) +
		      R_.db(1,2)*S_.db(i+n*2,0) )+y;
  }return;
}
Esempio n. 6
0
//=============================================================================
int isWithinTri(double x,double y,cv::Mat &tri,cv::Mat &shape)
{
    int i,j,k,t,n = tri.rows,p = shape.rows/2;
    double s11,s12,s21,s22,s31,s32;
    for(t = 0; t < n; t++) {
        i = tri.it(t,0);
        j = tri.it(t,1);
        k = tri.it(t,2);
        s11 = shape.db(i  ,0);
        s21 = shape.db(j  ,0);
        s31 = shape.db(k  ,0);
        s12 = shape.db(i+p,0);
        s22 = shape.db(j+p,0);
        s32 = shape.db(k+p,0);
        if(sameSide(x,y,s11,s12,s21,s22,s31,s32) &&
                sameSide(x,y,s21,s22,s11,s12,s31,s32) &&
                sameSide(x,y,s31,s32,s11,s12,s21,s22))return t;
    }
    return -1;
}
Esempio n. 7
0
//===========================================================================
void PDM::CalcParams(cv::Mat &s,cv::Mat &plocal,cv::Mat &pglobl)
{  
  assert((s.type() == CV_64F) && (s.rows == 2*(_M.rows/3)) && (s.cols = 1));
  if((pglobl.rows != 6) || (pglobl.cols != 1) || (pglobl.type() != CV_64F))
    pglobl.create(6,1,CV_64F);
  int j,n = _M.rows/3; double si,scale,pitch,yaw,roll,tx,ty,Tx,Ty,Tz; 
  cv::Mat R(3,3,CV_64F),z(n,1,CV_64F),t(3,1,CV_64F),p(_V.cols,1,CV_64F);  
  cv::Mat r = R.row(2), S(this->nPoints(),3,CV_64F);
  plocal = cv::Mat::zeros(_V.cols,1,CV_64F); 
  for(int iter = 0; iter < 100; iter++){
    this->CalcShape3D(S_,plocal);
    Align3Dto2DShapes(scale,pitch,yaw,roll,tx,ty,s,S_);
    Euler2Rot(R,pitch,yaw,roll); S = (S_.reshape(1,3)).t();
    z = scale*S*r.t(); si = 1.0/scale; 
    Tx = -si*(R.db(0,0)*tx + R.db(1,0)*ty);
    Ty = -si*(R.db(0,1)*tx + R.db(1,1)*ty);
    Tz = -si*(R.db(0,2)*tx + R.db(1,2)*ty);
    for(j = 0; j < n; j++){
      t.db(0,0) = s.db(j,0); t.db(1,0) = s.db(j+n,0); t.db(2,0) = z.db(j,0);
      S_.db(j    ,0) = si*t.dot(R.col(0))+Tx;
      S_.db(j+n  ,0) = si*t.dot(R.col(1))+Ty;
      S_.db(j+n*2,0) = si*t.dot(R.col(2))+Tz;
    }
    plocal = _V.t()*(S_-_M); 
    if(iter > 0){if(cv::norm(plocal-p) < 1.0e-5)break;}
    plocal.copyTo(p);
  }
  pglobl.db(0,0) = scale; pglobl.db(1,0) = pitch;
  pglobl.db(2,0) = yaw;   pglobl.db(3,0) = roll;
  pglobl.db(4,0) = tx;    pglobl.db(5,0) = ty;
  return;
}
Esempio n. 8
0
//=============================================================================
void Rot2Euler(cv::Mat &R,double& pitch,double& yaw,double& roll)
{
  assert((R.rows == 3) && (R.cols == 3));
  double q[4];
  q[0] = sqrt(1+R.db(0,0)+R.db(1,1)+R.db(2,2))/2;
  q[1] = (R.db(2,1) - R.db(1,2)) / (4*q[0]) ;
  q[2] = (R.db(0,2) - R.db(2,0)) / (4*q[0]) ;
  q[3] = (R.db(1,0) - R.db(0,1)) / (4*q[0]) ;
  yaw  = asin(2*(q[0]*q[2] + q[1]*q[3]));
  pitch= atan2(2*(q[0]*q[1]-q[2]*q[3]),
	       q[0]*q[0]-q[1]*q[1]-q[2]*q[2]+q[3]*q[3]); 
  roll = atan2(2*(q[0]*q[3]-q[1]*q[2]),
	       q[0]*q[0]+q[1]*q[1]-q[2]*q[2]-q[3]*q[3]);
  return;
}
Esempio n. 9
0
//===========================================================================
void PDM::CalcReferenceUpdate(cv::Mat &dp,cv::Mat &plocal,cv::Mat &pglobl)
{
  assert((dp.rows == 6+_V.cols) && (dp.cols == 1));
  plocal += dp(cv::Rect(0,6,1,_V.cols));
  pglobl.db(0,0) += dp.db(0,0);
  pglobl.db(4,0) += dp.db(4,0);
  pglobl.db(5,0) += dp.db(5,0);
  Euler2Rot(R1_,pglobl); R2_ = cv::Mat::eye(3,3,CV_64F);
  R2_.db(1,2) = -1.0*(R2_.db(2,1) = dp.db(1,0));
  R2_.db(2,0) = -1.0*(R2_.db(0,2) = dp.db(2,0));
  R2_.db(0,1) = -1.0*(R2_.db(1,0) = dp.db(3,0));
  MetricUpgrade(R2_); R3_ = R1_*R2_; Rot2Euler(R3_,pglobl); return;
}
Esempio n. 10
0
//=============================================================================
void Rot2Euler(cv::Mat &R,cv::Mat &p)
{
  assert((p.rows == 6) && (p.cols == 1));
  Rot2Euler(R,p.db(1,0),p.db(2,0),p.db(3,0)); return;
}
Esempio n. 11
0
//===========================================================================
void Euler2Rot(cv::Mat &R,cv::Mat &p,bool full = true)
{
  assert((p.rows == 6) && (p.cols == 1));
  Euler2Rot(R,p.db(1,0),p.db(2,0),p.db(3,0),full); return;
}