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