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