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