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