PointMatcher<T>::ErrorMinimizer::ErrorElements::ErrorElements(const DataPoints& reading, const DataPoints reference, const OutlierWeights weights, const Matches matches):
reading(reading),
reference(reference),
weights(weights),
matches(matches)
{
assert(reading.features.cols() == reference.features.cols());
assert(reading.features.cols() == weights.cols());
assert(reading.features.cols() == matches.dists.cols());
// May have no descriptors... size 0
}
void PointMatcher<T>::ICPChainBase::filterGrossOutliersAndCalcErrors
(const DataPoints& referenceIn, double maxDistSq,
DataPoints& reading, Matrix & errors // in-out
){
// Remove points in reading further than sqrt(maxDistSq) from reference.
initICP();
typedef Parametrizable::Parameters Parameters;
Parameters p;
ostringstream os; os.precision(16); os << maxDistSq;
p["maxDist"] = os.str();
this->outlierFilters.clear();
this->outlierFilters.push_back(new typename OutlierFiltersImpl<T>::MaxDistOutlierFilter(p));
// Match to closest point in Reference
const Matches matches = this->matcher->findClosests(reading);
//-----------------------------
// Detect outliers
const OutlierWeights outlierWeights
(
this->outlierFilters.compute(reading, referenceIn, matches)
);
assert(outlierWeights.rows() == matches.ids.rows());
assert(outlierWeights.cols() == matches.ids.cols());
typename ErrorMinimizer::ErrorElements& mPts
= this->errorMinimizer->getMatchedPoints(reading, referenceIn,
matches, outlierWeights);
errors = mPts.matches.dists.cwiseSqrt(); // take square root
reading = mPts.reading;
}
typename PointMatcher<T>::TransformationParameters PointMatcher<T>::ICP::computeWithTransformedReference(
const DataPoints& readingIn,
const DataPoints& reference,
const TransformationParameters& T_refIn_refMean,
const TransformationParameters& T_refIn_dataIn)
{
timer t; // Print how long take the algo
// Apply readings filters
// reading is express in frame <dataIn>
DataPoints reading(readingIn);
//const int nbPtsReading = reading.features.cols();
this->readingDataPointsFilters.init();
this->readingDataPointsFilters.apply(reading);
readingFiltered = reading;
// Reajust reading position:
// from here reading is express in frame <refMean>
TransformationParameters
T_refMean_dataIn = T_refIn_refMean.inverse() * T_refIn_dataIn;
this->transformations.apply(reading, T_refMean_dataIn);
// Prepare reading filters used in the loop
this->readingStepDataPointsFilters.init();
// Since reading and reference are express in <refMean>
// the frame <refMean> is equivalent to the frame <iter(0)>
const int dim(reference.features.rows());
TransformationParameters T_iter = Matrix::Identity(dim, dim);
bool iterate(true);
this->transformationCheckers.init(T_iter, iterate);
size_t iterationCount(0);
// statistics on last step
this->inspector->addStat("ReadingPreprocessingDuration", t.elapsed());
this->inspector->addStat("ReadingInPointCount", readingIn.features.cols());
this->inspector->addStat("ReadingPointCount", reading.features.cols());
LOG_INFO_STREAM("PointMatcher::icp - reading pre-processing took " << t.elapsed() << " [s]");
this->prefilteredReadingPtsCount = reading.features.cols();
t.restart();
// iterations
while (iterate)
{
DataPoints stepReading(reading);
//-----------------------------
// Apply step filter
this->readingStepDataPointsFilters.apply(stepReading);
//-----------------------------
// Transform Readings
this->transformations.apply(stepReading, T_iter);
//-----------------------------
// Match to closest point in Reference
const Matches matches(
this->matcher->findClosests(stepReading)
);
//-----------------------------
// Detect outliers
const OutlierWeights outlierWeights(
this->outlierFilters.compute(stepReading, reference, matches)
);
assert(outlierWeights.rows() == matches.ids.rows());
assert(outlierWeights.cols() == matches.ids.cols());
//cout << "outlierWeights: " << outlierWeights << "\n";
//-----------------------------
// Dump
this->inspector->dumpIteration(
iterationCount, T_iter, reference, stepReading, matches, outlierWeights, this->transformationCheckers
);
//-----------------------------
// Error minimization
// equivalent to:
// T_iter(i+1)_iter(0) = T_iter(i+1)_iter(i) * T_iter(i)_iter(0)
T_iter = this->errorMinimizer->compute(
stepReading, reference, outlierWeights, matches) * T_iter;
// Old version
//T_iter = T_iter * this->errorMinimizer->compute(
// stepReading, reference, outlierWeights, matches);
// in test
this->transformationCheckers.check(T_iter, iterate);
++iterationCount;
}
this->inspector->addStat("IterationsCount", iterationCount);
this->inspector->addStat("PointCountTouched", this->matcher->getVisitCount());
this->matcher->resetVisitCount();
this->inspector->addStat("OverlapRatio", this->errorMinimizer->getWeightedPointUsedRatio());
this->inspector->addStat("ConvergenceDuration", t.elapsed());
this->inspector->finish(iterationCount);
LOG_INFO_STREAM("PointMatcher::icp - " << iterationCount << " iterations took " << t.elapsed() << " [s]");
// Move transformation back to original coordinate (without center of mass)
// T_iter is equivalent to: T_iter(i+1)_iter(0)
// the frame <iter(0)> equals <refMean>
// so we have:
// T_iter(i+1)_dataIn = T_iter(i+1)_iter(0) * T_refMean_dataIn
// T_iter(i+1)_dataIn = T_iter(i+1)_iter(0) * T_iter(0)_dataIn
// T_refIn_refMean remove the temperary frame added during initialization
return (T_refIn_refMean * T_iter * T_refMean_dataIn);
}
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;
}
