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