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;
}
