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