コード例 #1
0
ファイル: Triangulation.cpp プロジェクト: AliAlawieh/kalibr
void triangulate(const Eigen::Vector3d & point1, const Eigen::Vector3d & ray1,
                 const Eigen::Vector3d & point2, const Eigen::Vector3d & ray2,
                 Eigen::Vector3d & outTriangulatedPoint, double & outGap,
                 double & outS1, double & outS2) {

  Eigen::Vector3d t12 = point2 - point1;

  Eigen::Vector2d b;
  b[0] = t12.dot(ray1);
  b[1] = t12.dot(ray2);
  Eigen::Matrix2d A;
  A(0, 0) = ray1.dot(ray1);
  A(1, 0) = ray1.dot(ray2);
  A(0, 1) = -A(1, 0);
  A(1, 1) = -ray2.dot(ray2);
  Eigen::Vector2d lambda = A.inverse() * b;
  Eigen::Vector3d xm = point1 + lambda[0] * ray1;
  Eigen::Vector3d xn = point2 + lambda[1] * ray2;
  t12 = (xm - xn);

  outGap = t12.norm();
  outTriangulatedPoint = xn + 0.5 * t12;
  outS1 = lambda[0];
  outS2 = lambda[1];

}
コード例 #2
0
void GreenStrain_LIMSolver2D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
  // Compute deformation gradients
  int numTets = mesh->Triangles->rows();
  Ms.resize(3,2*numTets);
  MMTs.resize(3,3*numTets);

  Eigen::Matrix<double,3,2> SelectorM;
  SelectorM.block<2,2>(0,0) = Eigen::Matrix<double,2,2>::Identity();
  SelectorM.row(2) = Eigen::Vector2d::Ones()*-1;
  
  for(int t=0;t<numTets;t++)
  {
    Eigen::Vector2d A,B,C;
    if(mesh->IsCorotatedTriangles)
    {
      A = mesh->CorotatedTriangles->row(t).block<1,2>(0,0).cast<double>();
      B = mesh->CorotatedTriangles->row(t).block<1,2>(0,2).cast<double>();
      C = mesh->CorotatedTriangles->row(t).block<1,2>(0,4).cast<double>();	
    }
    else
    {
      A = mesh->InitalVertices->row(mesh->Triangles->coeff(t,0)).block<1,2>(0,0).cast<double>();
      B = mesh->InitalVertices->row(mesh->Triangles->coeff(t,1)).block<1,2>(0,0).cast<double>();
      C = mesh->InitalVertices->row(mesh->Triangles->coeff(t,2)).block<1,2>(0,0).cast<double>();
    }

    Eigen::Matrix2d V;
    V << A-C,B-C;
    
    Eigen::Matrix<double,3,2> Mtemp = SelectorM*V.inverse().cast<double>();
    Ms.block<3,2>(0,2*t) = Mtemp;
    MMTs.block<3,3>(0,3*t) = Mtemp*Mtemp.transpose();
  }
}
コード例 #3
0
ファイル: fastconvexfitting.cpp プロジェクト: RobotSDK/SDK
bool FastConvexFitting::getEvaluation(Geometry & geometry)
{
    QVector<int> edgeid;
    QVector<int> startid;
    QVector<int> endid;
    if(getBeamRange(geometry,edgeid,startid,endid))
    {
        geometry.score=1;
        int j,m=edgeid.size();
        for(j=0;j<m;j++)
        {
            if(startid[j]>endid[j])
            {
                endid[j]+=beamsnum;
            }
            Eigen::Matrix2d A;
            A.block(0,0,2,1)=orientation*(geometry.edges[edgeid[j]].startcorner-geometry.edges[edgeid[j]].endcorner);
            Eigen::Vector2d B;
            B=position+orientation*(geometry.edges[edgeid[j]].startcorner);
            int k;
            int count=0;
            double score=1;
            for(k=startid[j];k<=endid[j];k++)
            {
                int tmpid=k;
                if(tmpid>=beamsnum)
                {
                    tmpid-=beamsnum;
                }
                if(beams[tmpid]<=MINIMUMDISTANCE)
                {
                    continue;
                }
                A.block(0,1,2,1)=points[tmpid];
                Eigen::Vector2d x=A.inverse()*B;
                score*=getGain(x(0),x(1),beams[tmpid]);
                count++;
            }
            if(count>0)
            {
                geometry.score*=pow(score,1.0/double(count));
            }
            else
            {
                geometry.score=0;
                return 0;
            }
        }
        return 1;
    }
    else
    {
        geometry.score=0;
        return 0;
    }
}
コード例 #4
0
bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d) {
    Eigen::Matrix<double,3,2> B;
    B <<  qC2C1.rotate(get_nor().getVec()), other.get_nor().getVec();
    const Eigen::Matrix2d BtB = B.transpose() * B;
    if(BtB.determinant() < 0.000001) { // Projection rays almost parallel.
        return false;
    }
    const Eigen::Vector2d dv = - BtB.inverse() * B.transpose() * C2rC2C1;
    d.setParameter(fabs(dv[0]));
    return true;
}
コード例 #5
0
void RadialTangentialDistortion::undistort(
    const Eigen::MatrixBase<DERIVED> & yconst,
    const Eigen::MatrixBase<DERIVED_JY> & outJy) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED>, 2);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_JY>, 2, 2);

  Eigen::MatrixBase<DERIVED> & y =
      const_cast<Eigen::MatrixBase<DERIVED> &>(yconst);
  y.derived().resize(2);

  // we use f^-1 ' = ( f'(f^-1) ) '
  // with f^-1 the undistortion
  // and  f the distortion
  undistort(y);  // first get the undistorted image

  Eigen::Vector2d kp = y;
  Eigen::Matrix2d Jd;
  distort(kp, Jd);

  // now y = f^-1(y0)
  DERIVED_JY & J = const_cast<DERIVED_JY &>(outJy.derived());

  J = Jd.inverse();

  /*  std::cout << "J: " << std::endl << J << std::endl;

   double mx2_u = y[0]*y[0];
   double my2_u = y[1]*y[1];
   //double mxy_u = y[0]*y[1];
   double rho2_u = mx2_u+my2_u;

   double rad_dist_u = _k1*rho2_u+_k2*rho2_u*rho2_u;
   // take the inverse as Jacobian.
   J(0,0) = 1/(1 + rad_dist_u + _k1*2*mx2_u + _k2*rho2_u*4*mx2_u + 2*_p1*y[1] + 6*_p2*y[0]);
   J(1,0) = (_k1*2*y[0]*y[1] + _k2*4*rho2_u*y[0]*y[1] + _p1*2*y[0] + 2*_p2*y[1]);
   J(0,1) = J(1,0);
   J(1,1) = 1/(1 + rad_dist_u + _k1*2*my2_u + _k2*rho2_u*4*my2_u + 6*_p1*y[1] + 2*_p2*y[0]);

   // this should only happen if the distortion coefficients are 0
   // the coefficients being zero removes the cross dependence then it is safe to set J(1,0) = 0
   if (J(1,0) != 0)
   J(1,0) = 1/J(1,0);
   J(0,1) = J(1,0);*/

}
コード例 #6
0
mathtools::geometry::euclidian::HyperEllipse<2> skeleton::model::Perspective::toObj(
		const Eigen::Matrix<double,skeleton::model::meta<skeleton::model::Projective>::stordim,1> &vec,
		const mathtools::geometry::euclidian::HyperEllipse<2> &) const
{
	double z = 1.0/(sqrt(vec(0)*vec(0) + vec(1)*vec(1) + 1.0 - vec(2)*vec(2)));
	double x = vec(0) * z;
	double y = vec(1) * z;
	
	Eigen::Matrix2d mat;
	mat << 1.0-x*x , -x*y    ,
		   -x*y    , 1.0-y*y ;
	
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solv(mat);
	
	Eigen::Matrix2d axes = Eigen::Matrix2d::Identity();
	double fac = Eigen::Vector2d(-x*z,-y*z).transpose()*mat.inverse()*Eigen::Vector2d(-x*z,-y*z) - 1.0 + z*z;
	axes = solv.eigenvectors()*Eigen::DiagonalMatrix<double,2>(Eigen::Vector2d(sqrt(fac/solv.eigenvalues()(0)),sqrt(fac/solv.eigenvalues()(1))));
	
	mathtools::affine::Point<2> pt(vec(0),vec(1),m_frame2);
	return mathtools::geometry::euclidian::HyperEllipse<2>(pt,m_frame2->getBasis()->getMatrix()*axes);
}
コード例 #7
0
ファイル: ekf.hpp プロジェクト: ashariati/blinker_tracking
    void EKF::correct(Eigen::Vector2d z_i)
    {
        // measurement matrix
        Eigen::Matrix2d I = Eigen::Matrix2d::Identity();
        Eigen::Matrix2d H = Eigen::Matrix2d::Identity();

        // innovation residual
        Eigen::Vector2d y = z_i - H * this->x;

        // innovation covariance
        Eigen::Matrix2d S = H * this->P * H.transpose() + this->R;

        // Kalman gain
        Eigen::Matrix2d K = this->P * H.transpose() * S.inverse();

        // updated state
        this->x = this->x + K * y;

        // updated covariance
        this->P = (I - K * H) * this->P;


    }
コード例 #8
0
void Deformable::projectPositionsCluster(std::vector<int> cluster, int cluster_indx)
{
    int numVertices =  cluster.size();
    if (numVertices <= 1) return;
    int i;//,j,k;
    double beta_cluster =params.lbeta.at(cluster_indx);
    
    // center of mass
    Eigen::Vector2d cm, originalCm;
    cm.setZero(); originalCm.setZero();
    double mass = 0.0;
    int indx;
    for (i = 0; i < numVertices; i++) {
        indx= cluster.at(i);
        double m = mMasses(indx,0);
        if (mFixed(indx,0)) m *= 100.0;
        mass += m;
        cm += mNewPos.row(indx) * m;
        originalCm += mOriginalPos.row(indx) * m;
        //std::cout<<"before: "<<mNewPos.row(indx)<<std::endl;
    }
    
    cm /= mass;
    originalCm /= mass;
    
    Eigen::Matrix2d Apq;
    Eigen::Matrix2d Aqq;
    Eigen::Vector2d p;
    Eigen::Vector2d q;
    Apq.setZero();
    Aqq.setZero();
    
    for (i = 0; i < numVertices; i++) {
        indx= cluster.at(i);
        p(0) = mNewPos(indx,0) - cm(0);
        p(1) = mNewPos(indx,1) - cm(1);
        
        q(0) = mOriginalPos(indx,0) - originalCm(0);
        q(1) = mOriginalPos(indx,1) - originalCm(1);
        double m = mMasses(indx,0);
        Apq += m*p*q.transpose();
        Aqq += m*q*q.transpose();
    }
    

    
    if (!params.allowFlip && Apq.determinant() < 0.0f)
    {  	// prevent from flipping
        Apq(0,1) = -Apq(0,1);
        Apq(1,1) = -Apq(1,1);
    }
    
    Eigen::Matrix2d R;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Apq, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd U = svd.matrixU();
    Eigen::MatrixXd V = svd.matrixV();
    R = U*V.transpose();
    
    if (!params.quadraticMatch) {	// --------- linear match
        Eigen::Matrix2d A = Aqq;
        A = Apq*A.inverse();
        
        if (params.volumeConservation) {
            
            double det = A.determinant();
            if (det != 0.0) {
                det = 1.0 / sqrt(fabs(det));
                if (det > 2.0) det = 2.0;
                
                A = A*det;
            }
        }
        
        Eigen::Matrix2d T = R * (1.0 - beta_cluster) + A * beta_cluster;
        std::cout<<"cluster's beta "<<beta_cluster<<std::endl;
        
        for (i = 0; i < numVertices; i++) {
            int indx= cluster.at(i);
            indxCount.at(indx) +=1;
            
            if (mFixed(indx)) continue;
            q(0) = mOriginalPos(indx,0) - originalCm(0);
            q(1) = mOriginalPos(indx,1) - originalCm(1);
            Eigen::Vector2d Tq = T*q;
            
            mGoalPos(indx,0) = Tq(0)+cm(0);
            mGoalPos(indx,1) = Tq(1)+cm(1);
            mNewPos.row(indx) += (mGoalPos.row(indx) - mNewPos.row(indx)) * params.alpha;
            
            mGoalPos_sum.row(indx) += mNewPos.row(indx);
        }
    }
}
コード例 #9
0
void Deformable::projectPositions()
{
    if (mNumVertices <= 1) return;
    int i;//,j,k;
    
    // center of mass
    Eigen::Vector2d cm, originalCm;
    cm.setZero(); originalCm.setZero();
    double mass = 0.0;
    
    for (i = 0; i < mNumVertices; i++) {
        double m = mMasses(i,0);
        if (mFixed(i,0)) m *= 1000.0;
        mass += m;
        cm += mNewPos.row(i) * m;
        originalCm += mOriginalPos.row(i) * m;
        
    }
    //std::cout<<std::endl;
    cm /= mass;
    
    originalCm /= mass;
    
    Eigen::Matrix2d Apq;
    Eigen::Matrix2d Aqq;
    Eigen::Vector2d p;
    Eigen::Vector2d q;
    Apq.setZero();
    Aqq.setZero();
    
    for (i = 0; i < mNumVertices; i++) {
        p(0) = mNewPos(i,0) - cm(0);
        p(1) = mNewPos(i,1) - cm(1);
        
        q(0) = mOriginalPos(i,0) - originalCm(0);
        q(1) = mOriginalPos(i,1) - originalCm(1);
        double m = mMasses(i,0);
        Apq += m*p*q.transpose();
        Aqq += m*q*q.transpose();
    }
  
    /*if (!params.allowFlip && Apq.determinant() < 0.0f) {  	// prevent from flipping
        Apq(0,1) = -Apq(0,1);
        Apq(1,1) = -Apq(1,1);
    }*/
    
    //std::cout<<Apq<<std::endl;
    
    Eigen::Matrix2d R;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Apq, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd U = svd.matrixU();
    Eigen::MatrixXd V = svd.matrixV();
    R = U*V.transpose();
    if (!params.quadraticMatch) {	// --------- linear match
        
        
        Eigen::Matrix2d A = Aqq;
        

        Eigen::Matrix2d A_=A.inverse();
        /*std::cout<<A_.row(0)<<std::endl;
        std::cout<<A_.row(1)<<std::endl;*/
        A = Apq*A_;
        
        if (params.volumeConservation) {
            double det = A.determinant();
            if(det<0) det=-det;
            if (det != 0.0) {
                det = 1.0 / sqrt(fabs(det));
                if (det > 2.0) det = 2.0;
                A *= det;
            }
        }
        float one_beta =1.0 - params.beta;
        Eigen::Matrix2d T = R * one_beta;
        
        
        A=A*params.beta;
        T = T + A;
        
        
        for (i = 0; i < mNumVertices; i++) {
            if (mFixed(i)) continue;
            q(0) = mOriginalPos(i,0) - originalCm(0);
            q(1) = mOriginalPos(i,1) - originalCm(1);
            Eigen::Vector2d Tq = T*q;
            mGoalPos(i,0) = Tq(0)+cm(0);
            mGoalPos(i,1) = Tq(1)+cm(1);
            //mGoalPos.row(i)=(R * (1.0f - params.beta) + A * params.beta)*q+cm;
            mNewPos.row(i) += (mGoalPos.row(i) - mNewPos.row(i)) * params.alpha;
            
            //std::cout<<T.row(0)<<std::endl;
            //std::cout<<T.row(1)<<std::endl;
        }
    }
}