示例#1
0
void updateQR(Matrix & Q, Matrix & R, int NUM_TX, int NUM_RX,int i)
{
		
		int ni = NUM_RX - i;
		int ti = NUM_TX - i;
		Matrix Rtemp(ni,ti);
		Matrix Qtemp(ni,ni);
		Matrix Atemp(ni,ti);
		Matrix Itemp(ni,ni);
		int k,l;
		for(k = i; k < NUM_RX; k++)
		{
			for(l = i ; l < NUM_TX; l++)
			{
				Atemp(k-i , l-i) = R(k,l);
			}
		}
		for(k = 0; k < ni; k++)
		{
			for(l = 0; l < ni; l++)
			{
				if(k == l) Itemp(k,l) = 1;
				else Itemp(k,l) = 0;
			}
		}
		Matrix x(ni,1);
		for(k = 0; k < ni; k++)
			x(k,0) = Atemp(k,0);
		double xSize = eSize(x,ni);
		int sign; 
		if ( x(0,0) >= 0 ) sign = 1; 
		else sign = -1;
		Matrix g(ni,1);
		g(0,0) = x(0,0) + sign * xSize;
		for(k = 1; k < ni; k++)
			g(k,0) = x(k,0);
		double gSize = eSize(g,ni);
		double Qscale = 2/(gSize * gSize);
		Matrix ggT(ni,ni);
		ggT = g * (~g);
		for(k = 0; k < ni; k++)
			for(l = 0; l < ni; l++)
				ggT(k,l) = Qscale * ggT(k,l);
		Qtemp = Itemp - ggT;
		Rtemp = Qtemp * Atemp;

		Matrix Qeff(NUM_RX,NUM_RX);
		for(k = 0; k < NUM_RX; k++)
			for(l = 0; l < NUM_RX; l++)
				Qeff(k,l) = 0;
		for(k = 0; k < i; k++) Qeff(k,k) = 1;
		
		for(k = i; k < NUM_RX; k++)
		{
			for(l = i; l < NUM_TX; l++)
				R(k,l) = Rtemp(k-i,l-i);
			for(l = i; l < NUM_RX; l++)
				Qeff(k,l) = Qtemp(k-i,l-i);
		}
		Q = Q * Qeff; 
}
void FeatureTransformationEstimator::prosac(Eigen::MatrixXd P,
                                            Eigen::MatrixXd Q,
                                            Eigen::Isometry3d &T,
                                            int &consensus,
                                            double &mse,
                                            int iterations,
                                            double breakPercentage,
                                            int minCorrespondenceCount,
                                            PoseEstimationFunction pose_function,
                                            ConsensusFunction consensus_function,
                                            bool do_prosac)
{
    std::vector<int> idx;
    for (int i = 0; i < P.cols(); i++) {
        idx.push_back(i);
    }

    // Consensus variables for PROSAC.
    int maxConsensus = 0;
    consensus = 0;
    Eigen::Array<bool,1,Eigen::Dynamic> consensusSet;
    Eigen::Array<bool,1,Eigen::Dynamic> maxConsensusSet;

    Eigen::Isometry3d Ttemp;
    Eigen::MatrixXd Ptemp(3, minCorrespondenceCount);
    Eigen::MatrixXd Qtemp(3, minCorrespondenceCount);

    // Get maximum consensus transform.
    for (int i = 0; i < iterations; i++) {
        // Shuffle part of the set of correspondences.
        if (do_prosac) {
            std::random_shuffle(idx.begin(), idx.begin() + std::min((int)std::ceil(((i + 3.) / iterations) * P.cols()), (int)P.cols()));
        } else {
            std::random_shuffle(idx.begin(), idx.end());
        }

        for (int j = 0; j < minCorrespondenceCount; j++) {
            Ptemp.col(j) = P.col(idx[j]);
            Qtemp.col(j) = Q.col(idx[j]);
        }

        pose_function(Ptemp, Qtemp, Ttemp);

        // Estimate consensus set.
        consensus = consensus_function(P, Q, Ttemp, consensusSet);

        // Check if this is the new maximum consensus set.
        if (consensus > maxConsensus) {
            maxConsensus = consensus;
            maxConsensusSet = consensusSet;
            T = Ttemp;

            // Break if the consensus set is large enough.
            if (maxConsensus >= minCorrespondenceCount && maxConsensus > breakPercentage * P.cols()) {
                break;
            }
        }
    }

    mse = 0.;
    if (maxConsensus >= minCorrespondenceCount) {
        Eigen::MatrixXd Pfinal(3, maxConsensus);
        Eigen::MatrixXd Qfinal(3, maxConsensus);
        int k = 0;
        for (int i = 0; i < P.cols(); i++) {
            if (maxConsensusSet[i]) {
                Pfinal.col(k) = P.col(i);
                Qfinal.col(k) = Q.col(i);
                k++;
            }
        }
        pose_function(Pfinal, Qfinal, T);
        maxConsensus = consensus_function(P, Q, T, maxConsensusSet);

//        int lastConsensus = maxConsensus;
//        Eigen::Isometry3d Tlast = T;
//        do {
//            ROS_INFO("  cons %d", maxConsensus);
//            if (maxConsensus <= minCorrespondenceCount || lastConsensus > maxConsensus) {
//                break;
//            }

//            T = Tlast;

//            Eigen::MatrixXd Pfinal(3, maxConsensus);
//            Eigen::MatrixXd Qfinal(3, maxConsensus);
//            lastConsensus = maxConsensus;
//            int k = 0;
//            for (int i = 0; i < P.cols(); i++) {
//                if (maxConsensusSet[i]) {
//                    Pfinal.col(k) = P.col(i);
//                    Qfinal.col(k) = Q.col(i);
//                    k++;
//                }
//            }
//            pose_function(Pfinal, Qfinal, Tlast);
//            maxConsensus = consensus_function(P, Q, Tlast, maxConsensusSet);
//        } while (lastConsensus != maxConsensus);

        for (int i = 0; i < P.cols(); i++) {
            if (maxConsensusSet[i]) {
                mse += (T * P.col(i).homogeneous() - Q.col(i)).norm();
            }
        }
        mse /= maxConsensus;
    } else {
        maxConsensus = 0;
        T = Eigen::Isometry3d::Identity();
    }

    consensus = maxConsensus;
}