typename PointMatcher<T>::ErrorMinimizer::ErrorElements& PointMatcher<T>::ErrorMinimizer::getMatchedPoints(
const DataPoints& requestedPts,
const DataPoints& sourcePts,
const Matches& matches,
const OutlierWeights& outlierWeights)
{
typedef typename Matches::Ids Ids;
typedef typename Matches::Dists Dists;
assert(matches.ids.rows() > 0);
assert(matches.ids.cols() > 0);
assert(matches.ids.cols() == requestedPts.features.cols()); //nbpts
assert(outlierWeights.rows() == matches.ids.rows()); // knn
const int knn = outlierWeights.rows();
const int dimFeat = requestedPts.features.rows();
const int dimReqDesc = requestedPts.descriptors.rows();
// Count points with no weights
const int pointsCount = (outlierWeights.array() != 0.0).count();
if (pointsCount == 0)
throw ConvergenceError("ErrorMnimizer: no point to minimize");
Matrix keptFeat(dimFeat, pointsCount);
Matrix keptDesc;
if(dimReqDesc > 0)
keptDesc = Matrix(dimReqDesc, pointsCount);
Matches keptMatches (Dists(1,pointsCount), Ids(1, pointsCount));
OutlierWeights keptWeights(1, pointsCount);
int j = 0;
int rejectedMatchCount = 0;
int rejectedPointCount = 0;
bool matchExist = false;
this->weightedPointUsedRatio = 0;
for (int i = 0; i < requestedPts.features.cols(); ++i) //nb pts
{
matchExist = false;
for(int k = 0; k < knn; k++) // knn
{
if (outlierWeights(k,i) != 0.0)
{
if(dimReqDesc > 0)
keptDesc.col(j) = requestedPts.descriptors.col(i);
keptFeat.col(j) = requestedPts.features.col(i);
keptMatches.ids(0, j) = matches.ids(k, i);
keptMatches.dists(0, j) = matches.dists(k, i);
keptWeights(0,j) = outlierWeights(k,i);
++j;
this->weightedPointUsedRatio += outlierWeights(k,i);
matchExist = true;
}
else
{
rejectedMatchCount++;
}
}
if(matchExist == false)
{
rejectedPointCount++;
}
}
assert(j == pointsCount);
this->pointUsedRatio = double(j)/double(knn*requestedPts.features.cols());
this->weightedPointUsedRatio /= double(knn*requestedPts.features.cols());
assert(dimFeat == sourcePts.features.rows());
const int dimSourDesc = sourcePts.descriptors.rows();
Matrix associatedFeat(dimFeat, pointsCount);
Matrix associatedDesc;
if(dimSourDesc > 0)
associatedDesc = Matrix(dimSourDesc, pointsCount);
// Fetch matched points
for (int i = 0; i < pointsCount; ++i)
{
const int refIndex(keptMatches.ids(i));
associatedFeat.col(i) = sourcePts.features.block(0, refIndex, dimFeat, 1);
if(dimSourDesc > 0)
associatedDesc.col(i) = sourcePts.descriptors.block(0, refIndex, dimSourDesc, 1);
}
this->lastErrorElements.reading = DataPoints(
keptFeat,
requestedPts.featureLabels,
keptDesc,
requestedPts.descriptorLabels
);
this->lastErrorElements.reference = DataPoints(
associatedFeat,
sourcePts.featureLabels,
associatedDesc,
sourcePts.descriptorLabels
);
this->lastErrorElements.weights = keptWeights;
this->lastErrorElements.matches = keptMatches;
this->lastErrorElements.nbRejectedMatches = rejectedMatchCount;
this->lastErrorElements.nbRejectedPoints = rejectedPointCount;
return this->lastErrorElements;
}
