Exemplo n.º 1
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;
}
Exemplo n.º 2
0
double covToSDPixels(const Eigen::Matrix2d & S)
{
    const double dProductOfEigenvectors = S.determinant();
    if(IS_DEBUG) CHECK(dProductOfEigenvectors < 0, "cov matrix not positive definite");
    return pow(dProductOfEigenvectors, 0.25); //Approx. sigma in pixels
}
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);
        }
    }
}
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;
        }
    }
}