void TransformationCheckersImpl<T>::BoundTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
{
typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
if (parameters.rows() == 4)
{
const Quaternion currentRotation = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
this->conditionVariables(0) = currentRotation.angularDistance(initialRotation3D);
}
else if (parameters.rows() == 3)
{
const T currentRotation(acos(parameters(0,0)));
this->conditionVariables(0) = normalizeAngle(currentRotation - initialRotation2D);
}
else
assert(false);
const unsigned int nbRows = parameters.rows()-1;
const Vector currentTranslation = parameters.topRightCorner(nbRows,1);
this->conditionVariables(1) = (currentTranslation - initialTranslation).norm();
if (this->conditionVariables(0) > this->limits(0) || this->conditionVariables(1) > this->limits(1))
{
ostringstream oss;
oss << "limit out of bounds: ";
oss << "rot: " << this->conditionVariables(0) << "/" << this->limits(0) << " ";
oss << "tr: " << this->conditionVariables(1) << "/" << this->limits(1);
throw ConvergenceError(oss.str());
}
}
void TransformationCheckersImpl<T>::DifferentialTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
{
typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
const unsigned int nbRows = parameters.rows()-1;
translations.push_back(parameters.topRightCorner(nbRows,1));
this->conditionVariables.setZero(2);
if(rotations.size() > smoothLength)
{
for(size_t i = rotations.size()-1; i >= rotations.size()-smoothLength; i--)
{
//Compute the mean derivative
this->conditionVariables(0) += anyabs(rotations[i].angularDistance(rotations[i-1]));
this->conditionVariables(1) += anyabs((translations[i] - translations[i-1]).norm());
}
this->conditionVariables /= smoothLength;
if(this->conditionVariables(0) < this->limits(0) && this->conditionVariables(1) < this->limits(1))
iterate = false;
}
//std::cout << "Abs Rotation: " << this->conditionVariables(0) << " / " << this->limits(0) << std::endl;
//std::cout << "Abs Translation: " << this->conditionVariables(1) << " / " << this->limits(1) << std::endl;
if (boost::math::isnan(this->conditionVariables(0)))
throw ConvergenceError("abs rotation norm not a number");
if (boost::math::isnan(this->conditionVariables(1)))
throw ConvergenceError("abs translation norm not a number");
}
bool TransformationsImpl<T>::RigidTransformation::checkParameters(const TransformationParameters& parameters) const
{
//FIXME: FP - should we put that as function argument?
const T epsilon = 0.001;
const TransformationParameters R(parameters.topLeftCorner(parameters.rows()-1, parameters.cols()-1));
if(anyabs(1 - R.determinant()) > epsilon)
return false;
else
return true;
}
void TransformationCheckersImpl<T>::BoundTransformationChecker::init(const TransformationParameters& parameters, bool& iterate)
{
this->conditionVariables.setZero(2);
if (parameters.rows() == 4)
initialRotation3D = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
else if (parameters.rows() == 3)
initialRotation2D = acos(parameters(0,0));
else
throw runtime_error("BoundTransformationChecker only works in 2D or 3D");
const unsigned int nbRows = parameters.rows()-1;
initialTranslation = parameters.topRightCorner(nbRows,1);
}
typename PointMatcher<T>::DataPoints TransformationsImpl<T>::PureTranslation::compute(const DataPoints& input,
const TransformationParameters& parameters) const {
assert(input.features.rows() == parameters.rows());
assert(parameters.rows() == parameters.cols());
if(this->checkParameters(parameters) == false)
throw PointMatcherSupport::TransformationError("PureTranslation: Error, left part not identity.");
DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.features.cols());
// Apply the transformation to features
transformedCloud.features = parameters * input.features;
return transformedCloud;
}
bool TransformationsImpl<T>::PureTranslation::checkParameters(
const TransformationParameters& parameters) const {
const int rows = parameters.rows();
const int cols = parameters.cols();
// make a copy of parameters to perform the check
TransformationParameters parameters_(parameters);
// set the translation components of the transformation matrix to 0
parameters_.block(0,cols-1,rows-1,1).setZero();
// If we have the identity matrix, than this is indeed a pure translation
if (parameters_.isApprox(TransformationParameters::Identity(rows,cols)))
return true;
else
return false;
}
typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::PureTranslation::correctParameters(
const TransformationParameters& parameters) const {
const int rows = parameters.rows();
const int cols = parameters.cols();
// make a copy of the parameters to perform corrections on
TransformationParameters correctedParameters(parameters);
// set the top left block to the identity matrix
correctedParameters.block(0,0,rows-1,cols-1).setIdentity();
// fix the bottom row
correctedParameters.block(rows-1,0,1,cols-1).setZero();
correctedParameters(rows-1,cols-1) = 1;
return correctedParameters;
}
typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::RigidTransformation::correctParameters(const TransformationParameters& parameters) const
{
TransformationParameters ortho = parameters;
if(ortho.cols() == 4)
{
const Eigen::Matrix<T, 3, 1> col0 = parameters.block(0, 0, 3, 1).normalized();
const Eigen::Matrix<T, 3, 1> col1 = parameters.block(0, 1, 3, 1).normalized();
const Eigen::Matrix<T, 3, 1> col2 = parameters.block(0, 2, 3, 1).normalized();
ortho.block(0, 0, 3, 1) = col1.cross(col2);
ortho.block(0, 1, 3, 1) = col2.cross(col0);
ortho.block(0, 2, 3, 1) = col2;
}
else if(ortho.cols() == 3)
{
// R = [ a b]
// [-b a]
// mean of a and b
T a = (parameters(0,0) + parameters(1,1))/2;
T b = (-parameters(1,0) + parameters(0,1))/2;
T sum = sqrt(pow(a,2) + pow(b,2));
a = a/sum;
b = b/sum;
ortho(0,0) = a; ortho(0,1) = b;
ortho(1,0) = -b; ortho(1,1) = a;
}
return ortho;
}
typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::TransformationWithScale::correctParameters(
const TransformationParameters& parameters) const {
std::cout << "Correcting Paramsa. .. .. . . " << std::endl;
const int rows = parameters.rows();
const int cols = parameters.cols();
TransformationParameters correctedParameters(parameters);
return correctedParameters;
// make a copy of the parameters to perform corrections on
// // set the top left block to the identity matrix
// correctedParameters.block(0,0,rows-1,cols-1).setIdentity();
// // fix the bottom row
// correctedParameters.block(rows-1,0,1,cols-1).setZero();
// correctedParameters(rows-1,cols-1) = 1;
// return correctedParameters;
}
void TransformationCheckersImpl<T>::DifferentialTransformationChecker::init(const TransformationParameters& parameters, bool& iterate)
{
this->conditionVariables.setZero(2);
rotations.clear();
translations.clear();
if (parameters.rows() == 4)
{
rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
}
else
{
// Handle the 2D case
Eigen::Matrix<T,3,3> m(Matrix::Identity(3,3));
m.topLeftCorner(2,2) = parameters.topLeftCorner(2,2);
rotations.push_back(Quaternion(m));
}
const unsigned int nbRows = parameters.rows()-1;
translations.push_back(parameters.topRightCorner(nbRows,1));
}
typename PointMatcher<T>::DataPoints TransformationsImpl<T>::RigidTransformation::compute(
const DataPoints& input,
const TransformationParameters& parameters) const
{
typedef typename PointMatcher<T>::Matrix Matrix;
assert(input.features.rows() == parameters.rows());
assert(parameters.rows() == parameters.cols());
const unsigned int nbRows = parameters.rows()-1;
const unsigned int nbCols = parameters.cols()-1;
const TransformationParameters R(parameters.topLeftCorner(nbRows, nbCols));
if(this->checkParameters(parameters) == false)
throw TransformationError("RigidTransformation: Error, rotation matrix is not orthogonal.");
DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.timeLabels, input.features.cols());
// Apply the transformation to features
transformedCloud.features = parameters * input.features;
// Apply the transformation to descriptors
int row(0);
const int descCols(input.descriptors.cols());
for (size_t i = 0; i < input.descriptorLabels.size(); ++i)
{
const int span(input.descriptorLabels[i].span);
const std::string& name(input.descriptorLabels[i].text);
const BOOST_AUTO(inputDesc, input.descriptors.block(row, 0, span, descCols));
BOOST_AUTO(outputDesc, transformedCloud.descriptors.block(row, 0, span, descCols));
if (name == "normals" || name == "observationDirections")
outputDesc = R * inputDesc;
else
outputDesc = inputDesc;
row += span;
}
return transformedCloud;
}
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);
}
FinalRepresentation
IntermediateRepresentator::transform(const TransformationParameters& parameters,
const CompileErrorList& parsingErrors,
MemoryAccess& memoryAccess) {
auto errors = parsingErrors;
if (_currentOutput) {
errors.pushError(_currentOutput->positionInterval(),
"Macro not closed. Missing a macro end directive?");
}
PrecompileImmutableArguments precompileArguments(parameters.architecture(),
parameters.generator());
SymbolGraph graph;
MacroDirectiveTable macroTable;
for (const auto& command : _commandList) {
command->precompile(precompileArguments, errors, graph, macroTable);
}
IntermediateMacroInstruction::replaceWithMacros(
_commandList.begin(), _commandList.end(), macroTable, errors);
auto macroList = generateMacroInformation();
auto preliminaryEvaluation = graph.evaluate();
if (!evaluateGraph(preliminaryEvaluation, errors)) {
return FinalRepresentation({}, errors, macroList);
}
SymbolReplacer preliminaryReplacer(preliminaryEvaluation);
MemoryAllocator allocator(parameters.allocator());
SectionTracker tracker;
auto allowedSize = memoryAccess.getMemorySize().get();
IntermediateOperationPointer firstMemoryExceedingOperation(nullptr);
AllocateMemoryImmutableArguments allocateMemoryArguments(precompileArguments,
preliminaryReplacer);
for (const auto& command : _commandList) {
command->allocateMemory(
allocateMemoryArguments, errors, allocator, tracker);
if (allocator.estimateSize() > allowedSize &&
!firstMemoryExceedingOperation) {
firstMemoryExceedingOperation = command;
}
}
auto allocatedSize = allocator.calculatePositions();
EnhanceSymbolTableImmutableArguments symbolTableArguments(
allocateMemoryArguments, allocator);
for (const auto& command : _commandList) {
command->enhanceSymbolTable(symbolTableArguments, errors, graph);
}
auto graphEvaluation = graph.evaluate();
auto graphValid = evaluateGraph(graphEvaluation, errors);
auto memoryValid = checkMemorySize(
allocatedSize, allowedSize, firstMemoryExceedingOperation, errors);
if (!(graphValid && memoryValid)) {
return FinalRepresentation({}, errors, macroList);
}
SymbolReplacer replacer(graphEvaluation);
ExecuteImmutableArguments executeArguments(symbolTableArguments, replacer);
FinalCommandVector commandOutput;
for (const auto& command : _commandList) {
command->execute(executeArguments, errors, commandOutput, memoryAccess);
}
return FinalRepresentation(commandOutput, errors, macroList);
}
