typename ErrorMinimizersImpl<T>::Matrix
ErrorMinimizersImpl<T>::PointToPlaneWithCovErrorMinimizer::estimateCovariance(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationParameters& transformation)
{
int max_nbr_point = outlierWeights.cols();
Matrix covariance(Matrix::Zero(6,6));
Matrix J_hessian(Matrix::Zero(6,6));
Matrix d2J_dReadingdX(Matrix::Zero(6, max_nbr_point));
Matrix d2J_dReferencedX(Matrix::Zero(6, max_nbr_point));
Vector reading_point(Vector::Zero(3));
Vector reference_point(Vector::Zero(3));
Vector normal(3);
Vector reading_direction(Vector::Zero(3));
Vector reference_direction(Vector::Zero(3));
Matrix normals = reference.getDescriptorViewByName("normals");
if (normals.rows() < 3) // Make sure there are normals in DataPoints
return std::numeric_limits<T>::max() * Matrix::Identity(6,6);
T beta = -asin(transformation(2,0));
T alpha = atan2(transformation(2,1), transformation(2,2));
T gamma = atan2(transformation(1,0)/cos(beta), transformation(0,0)/cos(beta));
T t_x = transformation(0,3);
T t_y = transformation(1,3);
T t_z = transformation(2,3);
Vector tmp_vector_6(Vector::Zero(6));
int valid_points_count = 0;
for(int i = 0; i < max_nbr_point; ++i)
{
if (outlierWeights(0,i) > 0.0)
{
reading_point = reading.features.block(0,i,3,1);
int reference_idx = matches.ids(0,i);
reference_point = reference.features.block(0,reference_idx,3,1);
normal = normals.block(0,reference_idx,3,1);
T reading_range = reading_point.norm();
reading_direction = reading_point / reading_range;
T reference_range = reference_point.norm();
reference_direction = reference_point / reference_range;
T n_alpha = normal(2)*reading_direction(1) - normal(1)*reading_direction(2);
T n_beta = normal(0)*reading_direction(2) - normal(2)*reading_direction(0);
T n_gamma = normal(1)*reading_direction(0) - normal(0)*reading_direction(1);
T E = normal(0)*(reading_point(0) - gamma*reading_point(1) + beta*reading_point(2) + t_x - reference_point(0));
E += normal(1)*(gamma*reading_point(0) + reading_point(1) - alpha*reading_point(2) + t_y - reference_point(1));
E += normal(2)*(-beta*reading_point(0) + alpha*reading_point(1) + reading_point(2) + t_z - reference_point(2));
T N_reading = normal(0)*(reading_direction(0) - gamma*reading_direction(1) + beta*reading_direction(2));
N_reading += normal(1)*(gamma*reading_direction(0) + reading_direction(1) - alpha*reading_direction(2));
N_reading += normal(2)*(-beta*reading_direction(0) + alpha*reading_direction(1) + reading_direction(2));
T N_reference = -(normal(0)*reference_direction(0) + normal(1)*reference_direction(1) + normal(2)*reference_direction(2));
// update the hessian and d2J/dzdx
tmp_vector_6 << normal(0), normal(1), normal(2), reading_range * n_alpha, reading_range * n_beta, reading_range * n_gamma;
J_hessian += tmp_vector_6 * tmp_vector_6.transpose();
tmp_vector_6 << normal(0) * N_reading, normal(1) * N_reading, normal(2) * N_reading, n_alpha * (E + reading_range * N_reading), n_beta * (E + reading_range * N_reading), n_gamma * (E + reading_range * N_reading);
d2J_dReadingdX.block(0,valid_points_count,6,1) = tmp_vector_6;
tmp_vector_6 << normal(0) * N_reference, normal(1) * N_reference, normal(2) * N_reference, reference_range * n_alpha * N_reference, reference_range * n_beta * N_reference, reference_range * n_gamma * N_reference;
d2J_dReferencedX.block(0,valid_points_count,6,1) = tmp_vector_6;
valid_points_count++;
} // if (outlierWeights(0,i) > 0.0)
}
Matrix d2J_dZdX(Matrix::Zero(6, 2 * valid_points_count));
d2J_dZdX.block(0,0,6,valid_points_count) = d2J_dReadingdX.block(0,0,6,valid_points_count);
d2J_dZdX.block(0,valid_points_count,6,valid_points_count) = d2J_dReferencedX.block(0,0,6,valid_points_count);
Matrix inv_J_hessian = J_hessian.inverse();
covariance = d2J_dZdX * d2J_dZdX.transpose();
covariance = inv_J_hessian * covariance * inv_J_hessian;
return (sensorStdDev * sensorStdDev) * covariance;
}
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;
}
void InspectorsImpl<T>::AbstractVTKInspector::dumpDataLinks(
const DataPoints& ref,
const DataPoints& reading,
const Matches& matches,
const OutlierWeights& featureOutlierWeights,
std::ostream& stream)
{
const Matrix& refFeatures(ref.features);
const int refPtCount(refFeatures.cols());
//const int featDim(refFeatures.rows());
const Matrix& readingFeatures(reading.features);
const int readingPtCount(readingFeatures.cols());
const int totalPtCount(refPtCount+readingPtCount);
stream << "# vtk DataFile Version 3.0\n";
stream << "comment\n";
stream << "ASCII\n";
stream << "DATASET POLYDATA\n";
stream << "POINTS " << totalPtCount << " float\n";
if(refFeatures.rows() == 4)
{
// reference pt
stream << refFeatures.topLeftCorner(3, refFeatures.cols()).transpose() << "\n";
// reading pt
stream << readingFeatures.topLeftCorner(3, readingFeatures.cols()).transpose() << "\n";
}
else
{
// reference pt
stream << refFeatures.transpose() << "\n";
// reading pt
stream << readingFeatures.transpose() << "\n";
}
const int knn = matches.ids.rows();
stream << "LINES " << readingPtCount*knn << " " << readingPtCount*knn * 3 << "\n";
//int j = 0;
for(int k = 0; k < knn; k++) // knn
{
for (int i = 0; i < readingPtCount; ++i)
{
stream << "2 " << refPtCount + i << " " << matches.ids(k, i) << "\n";
}
}
stream << "CELL_DATA " << readingPtCount*knn << "\n";
stream << "SCALARS outlier float 1\n";
stream << "LOOKUP_TABLE default\n";
//stream << "LOOKUP_TABLE alphaOutlier\n";
for(int k = 0; k < knn; k++) // knn
{
for (int i = 0; i < readingPtCount; ++i) //nb pts
{
stream << featureOutlierWeights(k, i) << "\n";
}
}
//stream << "LOOKUP_TABLE alphaOutlier 2\n";
//stream << "1 0 0 0.5\n";
//stream << "0 1 0 1\n";
}